def RunScript(self, robot, visual_mesh, collision_mesh, tcf_plane):
if robot and robot.client and robot.client.is_connected and visual_mesh:
if not collision_mesh:
collision_mesh = visual_mesh
c_visual_mesh = RhinoMesh.from_geometry(visual_mesh).to_compas()
c_collision_mesh = RhinoMesh.from_geometry(collision_mesh).to_compas()
if not tcf_plane:
frame = Frame.worldXY()
else:
frame = plane_to_compas_frame(tcf_plane)
tool = Tool(c_visual_mesh, frame, c_collision_mesh)
scene = PlanningScene(robot)
robot.attach_tool(tool)
scene.add_attached_tool()
return robot
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)
from compas.geometry import Frame
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.robots import PlanningScene
from compas_fab.robots import Tool
with RosClient() as client:
robot = client.load_robot()
scene = PlanningScene(robot)
assert robot.name == 'ur5_robot'
# create collision object
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
t1cf = Frame([0.14, 0, 0], [0, 0, 1], [0, 1, 0]) # TODO: check this frame!
tool = Tool(mesh, t1cf, name='tip')
scene.add_attached_tool(tool)
# sleep a bit before removing the tip
time.sleep(1)
# check if it's really there
planning_scene = robot.client.get_planning_scene()
acm = planning_scene.robot_state.attached_collision_objects
assert acm[0].object['id'].startswith('tip_')
scene.remove_attached_tool()
planning_scene = robot.client.get_planning_scene()
assert acm[0].object['id'] not in [c.object['id'] for c in planning_scene.robot_state.attached_collision_objects]