Advanced Wandelscript examples
To inspire you when writing your own Wandelscript, we've published a set of example programs. These examples cover a wide range of automation tasks and involve robot movements, I/Os or other sensors, coordinate systems, and geometric operations.
A little selection of those examples can be found on this page, find the complete example collection (opens in a new tab) on GitHub.
Multi-robot
Pick and place using conveyors, sensors and OPC UA.
# defines the pick position of the box
# 0 - 8 --> back to 0
TCP_Sucker = frame("WOS_Sucker")
TCP_HookedHook = frame("WOS_HookedHook")
TCP_ABB = frame("BASE/tool0")
LEFT_SENSOR_IO = "digital_in[1]"
RIGHT_SENSOR_IO = "digital_in[0]"
RED_BUTTON_IO = "digital_in[2]"
GREEN_BUTTON_IO = "digital_in[3]"
prod = False
debug = False
run_no = 0
abb_home = (475.7, -94.4, 553.8, -0.027, -0.024, 1.176)
ur_home = (191.6, -717.3, 433.2, 0, pi, 0)
ur_pick_pose = (184, -1038, 150, 0, pi, 0)
ur_drop_pose = (460, -832, 346, 0, pi, 0)
abb_pick_pose = (484.1, 151.4, 446.6, 0.019, 0.006, 1.335)
abb_drop_pose = (484.3, -388.3, 303, 0.017, 0.004, 1.335)
def wait_for(key, value):
while (read(controller, key) != value):
pass
def wait_for_left_sensor_touched():
wait_for(LEFT_SENSOR_IO, True)
def wait_for_right_sensor_touched():
wait_for(RIGHT_SENSOR_IO, True)
def move_conveyor_belt_to_the_left():
already_left = read(controller, LEFT_SENSOR_IO)
if (already_left == False):
wait_for(RIGHT_SENSOR_IO, True)
write(controller, "digital_out[4]", True)
write(controller, "digital_out[5]", True)
write(controller, "digital_out[6]", False)
sync
def move_conveyor_belt_to_the_right():
already_right = read(controller, RIGHT_SENSOR_IO)
if (already_right == False):
wait_for(LEFT_SENSOR_IO, True)
write(controller, "digital_out[4]", True)
write(controller, "digital_out[5]", False)
write(controller, "digital_out[6]", True)
sync
def abb_gripper(open):
if open:
write(controller, "digital_out[0]", True)
write(controller, "digital_out[1]", False)
write(controller, "digital_out[2]", True)
else:
write(controller, "digital_out[0]", True)
write(controller, "digital_out[1]", True)
write(controller, "digital_out[2]", False)
def ur_gripper(on):
if on:
write(controller, "tool_out[1]", True)
else:
write(controller, "tool_out[1]", False)
def is_red_button_pressed():
return read(controller, "digital_in[2]") == False
def is_green_button_pressed():
return read(controller, "digital_in[3]") == False
def is_any_sensor_touched():
return (is_left_sensor_touched() + is_right_sensor_touched()) == 1
# def is_object_in_drop_position():
# return read(controller, "digital_in[4]") == 1
def get_grid_pose(corner_pose, idx):
grid_step = 63
grid_n = 3
dir_1 = (grid_step * modulo(idx, grid_n), 0, 0, 0, 0, 0)
dir_2 = (0, grid_step * intdiv(idx, grid_n), 0, 0, 0, 0)
pose_for_idx = corner_pose :: dir_1 :: dir_2
return pose_for_idx
def move_ur_to_suck_object(idx):
pick_pose = get_grid_pose(ur_pick_pose, idx)
move TCP_Sucker via p2p() to pick_pose :: (0, 0, -100)
ur_gripper(True)
move TCP_Sucker via p2p() to pick_pose
move TCP_Sucker via p2p() to pick_pose :: (0, 0, -100)
def move_ur_to_unsuck_object():
move TCP_Sucker via p2p() to ur_drop_pose :: (0, 0, -150)
ur_gripper(False)
move TCP_Sucker via p2p() to ur_drop_pose
wait(2000)
move TCP_Sucker via p2p() to ur_drop_pose :: (0, 0, -150)
move TCP_Sucker via p2p() to ur_home
def move_ur_to_hook_object():
pick_pose = [211.3, -1036.5, 211.0, 0, pi, 0] # TODO: find pick position
move TCP_HookedHook via p2p() to pick_pose :: (0, 0, -100)
move TCP_HookedHook via p2p() to pick_pose
move TCP_HookedHook via p2p() to pick_pose :: (0, 0, -100)
def move_ur_to_unhook_object():
pick_pose = (0, 0, 100, 0, pi, 0) # TODO: find pick position
move TCP_HookedHook via p2p() to pick_pose :: (0, 0, -100)
move TCP_HookedHook via p2p() to pick_pose
move TCP_HookedHook via p2p() to pick_pose :: (0, 0, -100)
def move_ur_to_suck_hook():
hook_pick_pose = (451, -543.7, 107.6, 0, pi, 0)
move TCP_Sucker via p2p() to hook_pick_pose :: (0, 0, -100)
move TCP_Sucker via p2p() to hook_pick_pose
ur_gripper(True)
move TCP_Sucker via p2p() to hook_pick_pose :: (0, 0, -100)
def move_ur_to_unsuck_hook():
hook_drop_pose = [451, -543.7, 107.6, 0, pi, 0]
move TCP_Sucker via p2p() to hook_drop_pose :: (0, 0, -100)
move TCP_Sucker via p2p() to hook_drop_pose
ur_gripper(False)
move TCP_Sucker via p2p() to hook_drop_pose :: (0, 0, -100)
def move_abb_to_pick_object():
with velocity(300):
do with robot_abb1200:
move TCP_ABB via p2p() to abb_pick_pose :: (0, 0, 100, 0, 0, 0)
move TCP_ABB via p2p() to abb_pick_pose
abb_gripper(False)
wait(1000)
do with robot_abb1200:
move TCP_ABB via p2p() to abb_pick_pose :: (0, 0, 100)
def move_abb_to_drop_object():
with velocity(300):
do with robot_abb1200:
move TCP_ABB via p2p() to abb_drop_pose :: (0, 0, 100)
move TCP_ABB via p2p() to abb_drop_pose :: (0, 0, 0, 0, 0, 0)
move TCP_ABB via p2p() to abb_drop_pose :: (-200, 200, 0, 0, 0, 0)
abb_gripper(True)
wait(500)
do with robot_abb1200:
move TCP_ABB via p2p() to abb_drop_pose :: (-200, 200, 100)
def reset_cell():
ur_gripper(False)
abb_gripper(False)
do with robot_ur10e:
move TCP_Sucker via p2p() to ur_home
#and do with robot_abb1200:
# move via p2p() to abb_home
# move_conveyor_belt_to_the_right()
def skill_suck(idx):
# skill_a picks the object with a vacuum gripper
move_ur_to_suck_object(idx)
move_conveyor_belt_to_the_right()
move_ur_to_unsuck_object()
move_conveyor_belt_to_the_left()
# ABB movement
wait_for_left_sensor_touched()
move_abb_to_pick_object()
sync
#abb_gripper(False)
move_abb_to_drop_object()
sync
#abb_gripper(True)
def skill_hook():
# skill_b picks the object with the hook
#move_ur_to_suck_hook()
#move_ur_to_hook_object()
#wait_for_io(controller, RIGHT_SENSOR_IO)
#move_ur_to_unhook_object()
#move_conveyor_belt_to_the_left()
#move_ur_to_unsuck_hook()
#wait_for_io(controller, LEFT_SENSOR_IO)
#move_abb_to_pick_object()
#move_abb_to_drop_object()
print("hook")
# -----------------------------------------
# TESTING
# -----------------------------------------
velocity(300)
# move to home
do with robot:
move TCP_Sucker via p2p() to ur_home
move TCP_Sucker via p2p() to ur_home :: (0, 0, -136)
and do with robot_abb1200:
move TCP_ABB via p2p() to abb_home
move TCP_ABB via p2p() to abb_home :: (0, 0, 100)
# abb_pose = read(robot_abb1200, "pose")
# print(abb_pose)
# -----------------------------------------
# MAIN
# -----------------------------------------
#abb_pose = read(robot_abb1200, "BASE/tool0")
#print(abb_pose)
#do with robot_abb1200:
# move TCP_ABB via p2p() to abb_home
# move TCP_ABB via p2p() to abb_home :: [0, 0, 100]
# move TCP_ABB via p2p() to abb_home
i = 0
while True:
print(i)
button_red_not_pressed = True
button_green_not_pressed = True
while True:
button_red_not_pressed = read(controller, RED_BUTTON_IO)
button_green_not_pressed = read(controller, GREEN_BUTTON_IO)
if button_red_not_pressed == False:
break
if button_green_not_pressed == False:
break
# reset_cell()
if button_red_not_pressed == False:
i = 0
skill_suck(i)
if button_green_not_pressed == False:
skill_suck(i)
i = i + 1
if i == 9:
i = 0Applications
Sanding 2 edges and 2 surfaces. Including camera and IOs.
# ### -----------------------------------------------------------
# ### Includes -------------------------
def set_motor(state, is_active):
if is_active:
set_io_map(get_aov_motor_ios(state, aov))
def set_compliance(state, is_active):
if is_active:
set_io_map(get_aov_compliance_ios(state, aov))
def align_approach(pose, align_to_origin, flip_axis, use_x_axis):
if align_to_origin:
return rotate_z_origin_direction(pose, flip_axis, use_x_axis)
return rotate_z(pose, flip_axis, use_x_axis)
def move_via_collision_free_p2p(tool, joints):
convex_hulls = [ ]
move tool via collision_free_p2p(convex_hulls) to joints
def move_to_home_safely(flange_tcp_name, workspace_tcp_name, home, workspace_robot_mounting):
# ### -----------------------------------------------------------
# ### Definitions -------------------------
pullout_mm = 100
lift_mm = 100
moving_velocity_slow = 20
moving_velocity = 50
# ### -----------------------------------------------------------
# ### Path -------------------------
flange = frame(check_tcp_name(flange_tcp_name))
workspace_tcp = frame(check_tcp_name(workspace_tcp_name))
workspace_tcp_pose = get_tcp_pose(robot, check_tcp_name(workspace_tcp_name))
velocity(moving_velocity)
current = solve_point_forward(robot, read(robot, "joints"), check_tcp_name(workspace_tcp_name))
up_direction_pose = ~workspace_robot_mounting
home_global = workspace_robot_mounting :: solve_point_forward(robot, home, check_tcp_name(flange_tcp_name))
current_global = workspace_robot_mounting :: current
if home_global[2] - current_global[2] >= lift_mm:
pullout = current :: (0, 0, -pullout_mm, 0, 0, 0)
lift = up_direction_pose :: (0, 0, lift_mm, 0, 0, 0) :: ~up_direction_pose :: pullout
velocity(moving_velocity_slow)
move workspace_tcp via p2p() to current
move workspace_tcp via line() to pullout
velocity(moving_velocity)
move workspace_tcp via line() to lift
move_via_collision_free_p2p(workspace_tcp, home)
def sand_surface(path, pressure, moving_velocity, sanding_velocity, is_hot_sanding, up_direction_pose, start_joints, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_surface):
# ### -----------------------------------------------------------
# ### Definitions -------------------------
def align_pose(pose):
return align_approach(pose, approach_align_to_origin, approach_flip_axis, approach_use_x_axis)
ABOVE_HIGH = lift_above_surface
ABOVE_CLOSE = 10
# ### -----------------------------------------------------------
# ### Path -------------------------
set_compliance(pressure, is_hot_sanding)
velocity(moving_velocity)
highest_polygon_pose = get_highest_point(path, up_direction_pose)
move_via_collision_free_p2p(sanding_tool, start_joints)
move sanding_tool via p2p() to align_pose(highest_polygon_pose :: (0, 0, -ABOVE_HIGH, 0, 0, 0) :: ~tcp_offset)
move sanding_tool via line() to align_pose(highest_polygon_pose :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
move sanding_tool via line() to align_pose(path[0] :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
velocity(sanding_velocity)
for i = 0..<len(path):
if i == 1:
set_motor(True, is_hot_sanding)
move sanding_tool via line() to align_pose(path(i) :: ~tcp_offset)
velocity(moving_velocity)
move sanding_tool via line() to align_pose(path[-1] :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
set_motor(False, is_hot_sanding)
move sanding_tool via line() to align_pose(highest_polygon_pose :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
move sanding_tool via line() to align_pose(highest_polygon_pose :: (0, 0, -ABOVE_HIGH, 0, 0, 0) :: ~tcp_offset)
move_via_collision_free_p2p(sanding_tool, start_joints)
def sand_edge(path, pressure, moving_velocity, sanding_velocity, is_hot_sanding, sanding_tool, start_joints, up_direction_pose, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_edge):
# ### -----------------------------------------------------------
# ### Definitions -------------------------
def align_pose(pose):
return align_approach(pose, approach_align_to_origin, approach_flip_axis, approach_use_x_axis)
ABOVE_HIGH = lift_above_edge
ABOVE_CLOSE = 10
# ### -----------------------------------------------------------
# ### Path -------------------------
edge_no_approach = slice_array(path, 2, -2)
highest_pose = get_highest_point(edge_no_approach, up_direction_pose)
set_compliance(pressure, is_hot_sanding)
velocity(moving_velocity)
move_via_collision_free_p2p(sanding_tool, start_joints)
move sanding_tool via p2p() to align_pose(highest_pose :: (0, 0, -ABOVE_HIGH, 0, 0, 0))
move sanding_tool via line() to align_pose(highest_pose :: (0, 0, -ABOVE_CLOSE+1, 0, 0, 0))
move sanding_tool via line() to align_pose(edge_no_approach[0] :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
set_motor(True, is_hot_sanding)
velocity(sanding_velocity)
for i = 0..<len(edge_no_approach):
move sanding_tool via line() to align_pose(edge_no_approach[i] :: ~tcp_offset)
set_motor(False, is_hot_sanding)
velocity(moving_velocity)
move sanding_tool via line() to align_pose(edge_no_approach[-1] :: (0, 0, -ABOVE_CLOSE, 0, 0, 0) :: ~tcp_offset)
move sanding_tool via line() to align_pose(highest_pose :: (0, 0, -ABOVE_CLOSE-1, 0, 0, 0)) # The -1 here is to prevent a wbr bug https://wandelbots.atlassian.net/browse/AA-1405
move sanding_tool via line() to align_pose(highest_pose :: (0, 0, -ABOVE_HIGH, 0, 0, 0))
move_via_collision_free_p2p(sanding_tool, start_joints)
def sand_straight_edge(edge, target_radius, lane_number_range, lane_density_weight, level_distance, pressure, moving_velocity, sanding_velocity, is_hot_sanding, sanding_tool, start_joints, up_direction_pose, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_edge):
# ### -----------------------------------------------------------
# ### Path -------------------------
meander = generate_edge_meander(edge[0], edge[1], target_radius, lane_number_range, lane_density_weight, level_distance)
sand_edge(meander, pressure, moving_velocity, sanding_velocity, is_hot_sanding, sanding_tool, start_joints, up_direction_pose, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_edge)
def sand_curved_edge(edge, target_radius, lane_number_range, lane_density_weight, level_distance, pressure, moving_velocity, sanding_velocity, is_hot_sanding, sanding_tool, start_joints, up_direction_pose, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_edge):
# ### -----------------------------------------------------------
# ### Path -------------------------
meander = generate_curved_edge_meander(edge[0], edge[1], target_radius, lane_number_range, lane_density_weight, level_distance)
sand_edge(meander, pressure, moving_velocity, sanding_velocity, is_hot_sanding, sanding_tool, start_joints, up_direction_pose, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_edge)
def sand_planar_surface(poly, distance, margin, pressure, moving_velocity, sanding_velocity, is_hot_sanding, up_direction_pose, start_joints, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_surface):
# ### -----------------------------------------------------------
# ### Path -------------------------
meander = generate_plane_meander(poly, distance, margin)
sand_surface(meander, pressure, moving_velocity, sanding_velocity, is_hot_sanding, up_direction_pose, start_joints, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_surface)
def sand_curved_surface(pcd, distance, margin, pressure, moving_velocity, sanding_velocity, is_hot_sanding, up_direction_pose, start_joints, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_surface):
# ### -----------------------------------------------------------
# ### Path -------------------------
meander = generate_curved_surface_meander(pcd, distance, margin)
sand_surface(meander, pressure, moving_velocity, sanding_velocity, is_hot_sanding, up_direction_pose, start_joints, tcp_offset, approach_align_to_origin, approach_flip_axis, approach_use_x_axis, lift_above_surface)
# ### -----------------------------------------------------------
# ### Input parameters -------------------------
# General
polygons = read(arguments, "polygons")
edges = read(arguments, "edges")
curved_edges = read(arguments, "curved_edges")
curved_surfaces = read(arguments, "curved_surfaces")
hot_sanding_on = True
safety_offset_z = 0 # (mm) positive value goes away from the workpiece
moving_velocity = 1000
workspace_robot_mounting = (0,0,0,0,0,0)
workspace_tcp = "TOOL 5"
home = [-0.08263993263244629,0.17805440723896027,0.4916095733642578,0.014160103164613247,-2.345040798187256,1.635414719581604]
use_approach_optimization = False
approach_align_to_origin = False
approach_flip_axis = False
approach_use_x_axis = False
# ### -----------------------------------------------------------
# ### Definitions -------------------------
def align_pose(pose):
return align_approach(pose, approach_align_to_origin, approach_flip_axis, approach_use_x_axis)
# Safety features
MOVING_OFFSET_HIGHEST = 150
# Path optimization features
use_start_joint_search = False
max_allowed_joint_deviations = [1, 1, 3, 3, 6, 6]
apply_operation_sync = use_start_joint_search
# ### -----------------------------------------------------------
# ### Configuration settings (developer can change) -------------
sanding_tool = frame(check_tcp_name(workspace_tcp))
tcp_pose = get_tcp_pose(robot, check_tcp_name(workspace_tcp))
tcp_correction_offset = (0, 0, 0, 0, 0, 0) # Normally set to zero (This gets applied to the points before the actual robot TCP is used)
# Construct the via pose:
up_direction_pose = ~workspace_robot_mounting
highest_pose = get_highest_point(polygons + edges + curved_surfaces + curved_edges, up_direction_pose)
above_workpiece = (highest_pose[0], highest_pose[1], highest_pose[2], 0, 0, 0) :: up_direction_pose :: (0, 0, MOVING_OFFSET_HIGHEST, 0, 0, 0) :: (0, 0, 0, pi, 0, 0)
above_workpiece = align_pose(above_workpiece)
# ### -----------------------------------------------------------
# ### Preparation (don't change) --------------------------------
safety_offset = (0, 0, safety_offset_z, 0, 0, 0)
if hot_sanding_on:
tcp_offset = tcp_correction_offset
else:
# This variable is kind of a 'local/relative' tcp, in relation to the actual used TCP of the robot
tcp_offset = tcp_correction_offset :: safety_offset
# ### -----------------------------------------------------------
# ### Skill starts ---------------------------------
above_workpiece_joints = solve_point_inverse(robot, above_workpiece :: ~tcp_offset, check_tcp_name(workspace_tcp), home)
blending(10)
velocity(moving_velocity)
move_via_collision_free_p2p(sanding_tool, home)
move_via_collision_free_p2p(sanding_tool, above_workpiece_joints)
sand_straight_edge(edges[0], 4, [5,17], 0.2, 100, 0.5, moving_velocity, 100, hot_sanding_on, sanding_tool, above_workpiece_joints, up_direction_pose, tcp_offset, False, False, False, 100)
if apply_operation_sync:
sync
sand_straight_edge(edges[1], 4, [5,17], 0.2, 100, 0.5, moving_velocity, 100, hot_sanding_on, sanding_tool, above_workpiece_joints, up_direction_pose, tcp_offset, False, False, False, 100)
if apply_operation_sync:
sync
sand_planar_surface(polygons[0], 80, 0, 2, moving_velocity, 100, hot_sanding_on, up_direction_pose, above_workpiece_joints, tcp_offset, False, False, False, 100)
if apply_operation_sync:
sync
sand_planar_surface(polygons[1], 80, 0, 2, moving_velocity, 100, hot_sanding_on, up_direction_pose, above_workpiece_joints, tcp_offset, False, False, False, 100)
if apply_operation_sync:
sync
# ### -----------------------------------------------------------
move_via_collision_free_p2p(sanding_tool, home)
# Motions are not resolved at this point
motion = motion_trajectory_to_json_string(robot)Control flow
Loop
Loop with any robot brand.
# Robot Agnostic
C1_HOME_J = [1.3950, 0.6053, 0.3194, 0.0089, 0.6564, 1.3092]
C1_PICK_APPROACH_ROTATED = (-62.1, 421.9, 434.9, -2.223, -2.22, 0)
C1_PICK_1 = (-44.9, 412.9, 118.8, 0, -3.1, 0)
C1_PICK_2 = (-231, 383.2, 119.1, 0, -3.1001, 0.0002)
C1_PICK_3 = (-265.7, 567.5, 118.8, -0.0001, -3.1079, 0.0113)
C1_PICK_4 = (-81.1, 595.1, 118.8, -0.0003, -3.1019, 0.0062)
# Piece 1
C1_SCANNER_APPROACH = (237.3, 478.1, 457.8, -2.225, -2.222, 0)
C1_SCANNER = (352.9, 478.1, 457.8, -2.224, -2.221, 0)
C1_SCANNER_LEAVE = (353.6, 319.0, 457.7, -2.226, -2.223, 0)
# Piece 2
C1_SCANNER_APPROACH_2 = (237.3, 478.1, 457.8, 0, -3.1, 0)
C1_SCANNER_2 = (352.9, 478.1, 457.8, 0, -3.1, 0)
C1_SCANNER_LEAVE_2 = (353.6, 319.0, 457.7, 0, -3.1, 0)
C1_PLACE_APPROACH = (484.3, 15.3, 200.9, -3.148, 0, 0)
C1_PLACE_1 = (452.2, -12.1, 132, 3.1, 0.0, 0.0)
C1_PLACE_2 = (635, 5, 132, 3.1367, 0, -0.0054)
def cell1_abb_open_gripper():
write(cell1_abb, True, "Local/SC_1/SC1CBCOK")
write(cell1_abb, False, "Local/SC_1/SC1CBCPREWARN")
def cell2_abb_close_gripper():
write(cell1_abb, False, "Local/SC_1/SC1CBCOK")
write(cell1_abb, True, "Local/SC_1/SC1CBCPREWARN")
# ---------------- programm ------------ '
velocity(300)
cell1_abb_open_gripper()
do with cell1_abb_0:
move via joint_p2p() to C1_HOME_J
move via p2p() to C1_PICK_2 :: (0, 0, -50, 0, 0, 0)
move via line() to C1_PICK_2
cell2_abb_close_gripper()
wait(1000)
do with cell1_abb_0:
move via line() to C1_PICK_2 :: (0, 0, -50, 0, 0, 0)
move via line() to C1_SCANNER_APPROACH_2
move via line() to C1_SCANNER_2 with velocity(50)
wait(2000)
do with cell1_abb_0:
move via line() to C1_SCANNER_APPROACH_2 with velocity(50)
move via line() to C1_PLACE_2 :: (0, 0, -50, 0, 0, 0)
move via line() to C1_PLACE_2 with velocity(50)
cell1_abb_open_gripper()
wait(1000)
velocity(300)
do with cell1_abb_0:
move via line() to C1_PLACE_APPROACH
move via joint_p2p() to C1_HOME_J
# Bringe back
wait(2000)
do with cell1_abb_0:
move via p2p() to C1_PLACE_2 :: (0, 0, -50, 0, 0, 0)
move via p2p() to C1_PLACE_2 :: (0, 0, 12.8, 0, 0, 0) with velocity(50)
cell2_abb_close_gripper()
wait(500)
do with cell1_abb_0:
move via p2p() to C1_PLACE_2 :: (0, 0, -50, 0, 0, 0)
move via joint_p2p() to C1_HOME_J
move via p2p() to C1_PICK_2 :: (0, 0, -50, 0, 0, 0)
move via p2p() to C1_PICK_2 :: (0, 0, -5, 0, 0, 0)
cell1_abb_open_gripper()
wait(500)
do with cell1_abb_0:
move via joint_p2p() to C1_HOME_J