import time
from compas.datastructures import Mesh
from compas.geometry import Box, Translation
from compas_fab.backends import RosClient
from compas_fab.robots import CollisionMesh
from compas_fab.robots import PlanningScene
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
brick = Box.from_width_height_depth(0.016, 0.012, 0.031)
brick.transform(Translation.from_vector([0, 0, brick.zsize / 2.]))
for i in range(5):
mesh = Mesh.from_shape(brick)
cm = CollisionMesh(mesh, 'brick_wall')
cm.frame.point.y += 0.5
cm.frame.point.z += brick.zsize * i
scene.append_collision_mesh(cm)
# sleep a bit before terminating the client
time.sleep(1)
trajectory2 = robot.plan_motion(goal_constraints,
start_configuration,
planner_id='RRT',
attached_collision_meshes=[brick_acm])
# 3. Calculate a cartesian motion to the target_frame
frames = [savelevel_target_frame, target_frame]
start_configuration = trajectory2.points[
-1] # as start configuration take last trajectory's end configuration
trajectory3 = robot.plan_cartesian_motion(
robot.from_attached_tool_to_tool0(frames),
start_configuration,
max_step=0.01,
attached_collision_meshes=[brick_acm])
assert (trajectory3.fraction == 1.)
# 4. Add the brick to the planning scene
brick = CollisionMesh(element.mesh, 'brick_wall')
scene.append_collision_mesh(brick)
# 5. Add trajectories to element and set to 'planned'
element.trajectory = [trajectory1, trajectory2, trajectory3]
assembly.network.set_vertex_attribute(key, 'is_planned', True)
# ==========================================================================
time.sleep(1)
# 6. Save assembly to json
assembly.to_json(PATH_TO)