robot = client.load_robot()
scene = PlanningScene(robot)
scene.remove_collision_mesh('brick_wall')
# attach tool
robot.attach_tool(tool)
# add tool to scene
scene.add_attached_tool()
# create an attached collision mesh to the robot's end effector.
ee_link_name = robot.get_end_effector_link_name()
brick_acm = AttachedCollisionMesh(
CollisionMesh(element_tool0.mesh, 'brick'), ee_link_name)
# add the collision mesh to the scene
scene.add_attached_collision_mesh(brick_acm)
# ==========================================================================
# 1. Calculate a cartesian motion from the picking frame to the savelevel_picking_frame
frames = [picking_frame, savelevel_picking_frame]
start_configuration = picking_configuration
trajectory1 = robot.plan_cartesian_motion(
robot.from_attached_tool_to_tool0(frames),
start_configuration,
max_step=0.01,
attached_collision_meshes=[brick_acm])
assert (trajectory1.fraction == 1.)
# ==========================================================================
key = 0
print(sequence)
for key in sequence:
print("=" * 30 + "\nCalculating path for element with key %d." % key)
element = assembly.element(key)
# 4. Create an attached collision mesh and attach it to the robot's end
# effector.
T = Transformation.from_frame_to_frame(element._tool_frame, tool.frame)
element_tool0 = element.transformed(T)
ee_link_name = robot.get_end_effector_link_name()
attached_element_mesh = AttachedCollisionMesh(
CollisionMesh(element_tool0.mesh, 'element'), ee_link_name)
# add the collision mesh to the scene
scene.add_attached_collision_mesh(attached_element_mesh)
# 5. Calculate moving_ and placing trajectories
for i in range(10):
try:
moving_trajectory, placing_trajectory = plan_moving_and_placing_motion(
robot, element, start_configuration, tolerance_vector,
safelevel_vector, attached_element_mesh)
if placing_trajectory.fraction != 1:
raise BackendError("Cartesian path not working")
else:
break
except BackendError:
print("Trying the %d. time" % (i + 2))
