Add examples for lecture 05

Author: gonzalocasas

Diff for: 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)

Diff for: 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)

Diff for: 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)

Diff for: 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)

Diff for: 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')
+

Diff for: 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)

Diff for: 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)

Diff for: 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)

Diff for: 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)

Diff for: 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)

Diff for: lecture_05/README.md

@@ -0,0 +1,38 @@
+# COMPAS II: Path planning
+
+Cartesian and kinematic path planning using MoveIt.
+
+Planning scene operations.
+
+End effectors.
+
+Pick and Place processes with discrete building elements.
+
+👉 [Slides](lecture_05.pdf)
+📜 [Assignment 04](assignment_04/README.md)
+
+## Examples
+
+Make sure you start (`compose up`) the container with a UR5 planner for these examples. You can use
+either [the one with browser-based UI](..\docker\moveit\docker-compose.yml) or
+the [lightweight version of it without UI]((..\docker\ur5-planner\docker-compose.yml)).
+
+* Path planning with MoveIt
+  * [Cartesian motion planning](01_plan_cartesian_motion_ros_loader.py)
+  * [Cartesian motion planning + graphs](02_plan_cartesian_motion_ros_loader_viz.py)
+  * [Free space motion planning](03_plan_motion_ros_loader.py)
+  * [Free space motion planning + graphs](04_plan_motion_ros_loader_viz.py)
+  * [Constraints](05_constraints.py)
+
+* Planning scene in MoveIt
+  * [Add objects to the scene](07_add_collision_mesh.py)
+  * [Append nested objects to the scene](08_append_collision_meshes.py)
+  * [Remove objects from the scene](09_remove_collision_mesh.py)
+
+* Pick and place process
+  * [Attach tool][10_attach_tool.py]
+  * [Detach tool][11_detach_tool.py]
+  * [Plan cartesian motion with tool][12_plan_cartesian_motion_with_attached_tool.py]
+
+* MoveIt & Grasshopper
+  * [Planning playground](16_robot_example.ghx)

Diff for: lecture_05/assignment_04/README.md

@@ -0,0 +1,52 @@
+# Assignment 04
+
+
+## How to start
+
+Use the following code as a starting point for your assignment:
+
+```python
+```
+
+## Expected result
+
+
+## How to submit your assignment
+
+1. Make sure you have forked this repository already, if not, check [assignment submission instructions in lecture 02](../../lecture_02/assignment_01#how-to-submit-your-assignment).
+2. Make sure your local clone is up to date on the `main` branch
+
+       (compas-fs2021) git checkout main
+       (compas-fs2021) git pull origin
+
+3. Use a branch called `assignment-04` for this week's assignment
+
+       (compas-fs2021) git checkout -b assignment-04
+       (compas-fs2021) git push -u assignments assignment-04
+
+4. Create a folder with your name and last name, eg. `elvis_presley` (make sure it is inside the current assignment folder)
+5. Create a Python file named `assignment_04.py` and paste the starting point code.
+6. For visual inspection, **copy** the file `solution_viewer.ghx` in the same folder of your `assignment_04.py`.
+6. Solve the coding assignment and commit
+    <details><summary><small>(How do I commit?)</small></summary>
+    <p>
+
+    Usually, commits are done from a visual client or VS code,
+    but you can also commit your changes from the command line:
+
+       (compas-fs2021) git add lecture_05/assignment_04/elvis_presley/\* && git commit -m "hello world"
+
+    
+    </p>
+    </details>
+
+8. Once you're ready to submit, push the changes:
+
+       (compas-fs2021) git push assignments
+
+9. And create a pull request (<small>[What's a pull request?](https://docs.github.com/en/github/collaborating-with-issues-and-pull-requests/about-pull-requests)</small>)
+
+    1. Open your browser and go to your fork
+    2. Create the pull request clicking `Compare & pull request` and follow the instructions
+
+    ![Start a pull request](../../.github/pull-request.png)