import time
from compas.datastructures import Mesh
from compas.geometry import Box
from compas_fab.backends import RosClient
from compas_fab.robots import CollisionMesh
from compas_fab.robots import PlanningScene
with RosClient() as client:
robot = client.load_robot()
scene = PlanningScene(robot)
assert robot.name == 'ur5_robot'
brick = Box.from_width_height_depth(0.11, 0.07, 0.25)
for i in range(5):
mesh = Mesh.from_vertices_and_faces(brick.vertices, brick.faces)
cm = CollisionMesh(mesh, 'brick')
cm.frame.point.y += 0.5
cm.frame.point.z += brick.zsize * i
scene.append_collision_mesh(cm)
# sleep a bit before removing the bricks
time.sleep(1)
scene.remove_collision_mesh('brick')
import os
import time
from compas_fab.backends import RosClient
from compas_fab.robots import PlanningScene
from compas_fab.robots import Tool
HERE = os.path.dirname(__file__)
DATA = os.path.abspath(os.path.join(HERE, "..", "data"))
tool = Tool.from_json(os.path.join(DATA, "vacuum_gripper.json"))
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
# Attach the tool
robot.attach_tool(tool)
scene.add_attached_tool()
time.sleep(3)
# Remove the tool
scene.remove_attached_tool()
scene.remove_collision_mesh(tool.name)
robot.detach_tool()
time.sleep(1)
# define savelevel frames 'above' the picking- and target frames
savelevel_picking_frame = picking_frame.copy()
savelevel_picking_frame.point += savelevel_vector
savelevel_target_frame = target_frame.copy()
savelevel_target_frame.point += savelevel_vector
# settings for plan_motion
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
with RosClient('localhost') as client:
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
import time
from compas.datastructures import Mesh
import compas_fab
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)
# create collison objects
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
# attach it to the end-effector
group = robot.main_group_name
scene.attach_collision_mesh_to_robot_end_effector(cm, group=group)
# sleep a bit before removing collision mesh
time.sleep(3)
scene.remove_attached_collision_mesh('tip')
scene.remove_collision_mesh('tip')
time.sleep(1)
import time
from compas_fab.backends import RosClient
from compas_fab.robots import PlanningScene
from compas_fab.robots.ur5 import Robot
with RosClient('localhost') as client:
robot = Robot(client)
scene = PlanningScene(robot)
scene.remove_collision_mesh('floor')
# sleep a bit before terminating the client
time.sleep(1)
T = Transformation.from_frame_to_frame(element0_tool0._tool_frame, tool.frame)
element0_tool0.transform(T)
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
# 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(element0_tool0.mesh, 'brick'), ee_link_name)
# add the collision mesh to the scene
scene.add_attached_collision_mesh(brick_acm)
time.sleep(2)
# Remove tool and brick
scene.remove_attached_collision_mesh(brick_acm.collision_mesh.id)
scene.remove_collision_mesh(brick_acm.collision_mesh.id)
scene.remove_attached_tool()
scene.remove_collision_mesh(tool.name)
robot.detach_tool()
time.sleep(1)
# ==============================================================================
# From here on: fill in code, whereever you see this dots ...
# NOTE: If you run Docker Toolbox, change `localhost` to `192.168.99.100`
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
robot.attach_tool(tool)
# 1. Add a collison mesh to the planning scene: floor, desk, etc.
for cm in scene_collision_meshes:
scene.add_collision_mesh(cm)
if not LOAD_FROM_EXISTING:
scene.remove_collision_mesh('assembly')
# 2. Compute picking trajectory
picking_trajectory = plan_picking_motion(robot, picking_frame,
safelevel_picking_frame,
picking_configuration,
attached_element_mesh)
# 3. Save the last configuration from that trajectory as new
# start_configuration
start_configuration = Configuration(picking_trajectory.points[-1].values,
picking_trajectory.points[-1].types)
sequence = [key for key in assembly.network.nodes()]
sequence = list(range(10))
print(sequence)