Add examples for lecture 05

Author: gonzalocasas

lecture_05/01_plan_cartesian_motion_ros_loader.py

@@ -0,0 +1,25 @@
+from compas.geometry import Frame
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    group = robot.main_group_name
+
+    frames = []
+    frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
+    frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
+
+    start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])
+
+    trajectory = robot.plan_cartesian_motion(frames,
+                                             start_configuration,
+                                             group=group,
+                                             options=dict(
+                                                 max_step=0.01,
+                                                 avoid_collisions=True,
+                                             ))
+
+    print("Computed cartesian path with %d configurations, " % len(trajectory.points))
+    print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

lecture_05/02_plan_cartesian_motion_ros_loader_viz.py

@@ -0,0 +1,28 @@
+from compas.geometry import Frame
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+from helpers import show_trajectory
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    group = robot.main_group_name
+
+    frames = []
+    frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
+    frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
+
+    start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])
+
+    trajectory = robot.plan_cartesian_motion(frames,
+                                             start_configuration,
+                                             group=group,
+                                             options=dict(
+                                                 max_step=0.01,
+                                                 avoid_collisions=True,
+                                             ))
+
+    print("Computed cartesian path with %d configurations, " % len(trajectory.points))
+    print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
+
+    show_trajectory(trajectory)

lecture_05/03_plan_motion_ros_loader.py

@@ -0,0 +1,28 @@
+import math
+from compas.geometry import Frame
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    group = robot.main_group_name
+
+    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
+    tolerance_position = 0.001
+    tolerance_axes = [math.radians(1)] * 3
+
+    start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.])
+
+    # create goal constraints from frame
+    goal_constraints = robot.constraints_from_frame(frame,
+                                                    tolerance_position,
+                                                    tolerance_axes,
+                                                    group)
+
+    trajectory = robot.plan_motion(goal_constraints,
+                                   start_configuration,
+                                   group,
+                                   options=dict(planner_id='RRT'))
+
+    print("Computed kinematic path with %d configurations." % len(trajectory.points))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

lecture_05/04_plan_motion_ros_loader_viz.py

@@ -0,0 +1,31 @@
+import math
+from compas.geometry import Frame
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+from helpers import show_trajectory
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    group = robot.main_group_name
+
+    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
+    tolerance_position = 0.001
+    tolerance_axes = [math.radians(1)] * 3
+
+    start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.])
+
+    # create goal constraints from frame
+    goal_constraints = robot.constraints_from_frame(frame,
+                                                    tolerance_position,
+                                                    tolerance_axes,
+                                                    group)
+
+    trajectory = robot.plan_motion(goal_constraints,
+                                   start_configuration,
+                                   group,
+                                   options=dict(planner_id='RRT'))
+
+    print("Computed kinematic path with %d configurations." % len(trajectory.points))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
+
+    show_trajectory(trajectory)

lecture_05/07_add_collision_mesh.py

@@ -0,0 +1,22 @@
+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)
+    mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl'))
+    cm = CollisionMesh(mesh, 'floor')
+    scene.add_collision_mesh(cm)
+
+    # sleep a bit before terminating the client
+    #time.sleep(1)
+
+    #scene.remove_collision_mesh('floor')
+

lecture_05/08_append_collision_meshes.py

@@ -0,0 +1,26 @@
+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)

lecture_05/09_remove_collision_mesh.py

@@ -0,0 +1,13 @@
+import time
+
+from compas_fab.backends import RosClient
+from compas_fab.robots import PlanningScene
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    scene = PlanningScene(robot)
+    scene.remove_collision_mesh('brick_wall')
+    scene.remove_collision_mesh('floor')
+
+    # sleep a bit before terminating the client
+    time.sleep(1)

lecture_05/10_attach_tool.py

@@ -0,0 +1,32 @@
+import os
+import time
+
+from compas_fab.backends import RosClient
+from compas_fab.robots import PlanningScene
+from compas_fab.robots import Tool
+
+from compas.datastructures import Mesh
+from compas.geometry import Frame
+
+HERE = os.path.dirname(__file__)
+
+# create tool from mesh and frame
+mesh = Mesh.from_stl(os.path.join(HERE, 'vacuum_gripper.stl'))
+frame = Frame([0.07, 0, 0], [0, 0, 1], [0, 1, 0])
+tool = Tool(mesh, frame)
+tool.to_json(os.path.join(HERE, 'vacuum_gripper.json'))
+
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+    scene = PlanningScene(robot)
+    robot.attach_tool(tool)
+
+    # add attached tool to planning scene
+    scene.add_attached_tool()
+
+    # now we can convert frames at robot's tool tip and flange
+    frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))]
+    frames_tcf0 = robot.from_tcf_to_t0cf(frames_tcf)
+
+    time.sleep(1)

lecture_05/11_detach_tool.py

@@ -0,0 +1,24 @@
+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__)
+tool = Tool.from_json(os.path.join(HERE, '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(1)
+
+    # Remove the tool
+    scene.remove_attached_tool()
+    scene.remove_collision_mesh(tool.name)
+    robot.detach_tool()
+    time.sleep(1)

lecture_05/12_plan_cartesian_motion_with_attached_tool.py

@@ -0,0 +1,38 @@
+import os
+from compas.geometry import Frame
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+from compas_fab.robots import Tool
+
+# create tool from json
+HERE = os.path.dirname(__file__)
+filepath = os.path.join(HERE, 'vacuum_gripper.json')
+tool = Tool.from_json(filepath)
+
+element_height = 0.014
+approach_distance = 0.05
+
+with RosClient('localhost') as client:
+    robot = client.load_robot()
+
+    # 1. Set tool
+    robot.attach_tool(tool)
+
+    # 2. Define start configuration
+    start_configuration = Configuration.from_revolute_values([-5.961, 4.407, -2.265, 5.712, 1.571, -2.820])
+
+    # 3. Define frames
+    picking_frame = Frame([-0.43, 0, element_height], [1, 0, 0], [0, 1, 0])
+    approach_picking_frame = Frame([-0.43, 0, element_height + approach_distance], [1, 0, 0], [0, 1, 0])
+    frames = [picking_frame, approach_picking_frame]
+
+    # 4. Convert frames to tool0_frames
+    frames_tool0 = robot.from_tcf_to_t0cf(frames)
+
+    trajectory = robot.plan_cartesian_motion(frames_tool0,
+                                             start_configuration,
+                                             options=dict(max_step=0.01))
+
+    print("Computed cartesian path with %d configurations, " % len(trajectory.points))
+    print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)