2.1.0 (#11)

* Add support for Cartesian paths (#8) * Add support for collision objects (#9) * Remove trimesh from ROS 2 dependencies Signed-off-by: Andrej Orsula <[email protected]> * Expose cartesian planning as a parameter in the pose goal example Signed-off-by: Andrej Orsula <[email protected]> * Add default test mesh for collision object example Signed-off-by: Andrej Orsula <[email protected]> * Move public functions before private Signed-off-by: Andrej Orsula <[email protected]> Co-authored-by: Nils Schulte <[email protected]>
AndrejOrsula · May 2, 2022 · 8802000 · 8802000
1 parent 70f8e00
commit 8802000
Show file tree

Hide file tree

Showing 6 changed files with 283 additions and 11 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -11,6 +11,7 @@ ament_python_install_package(pymoveit2)
 # Install examples
 set(EXAMPLES_DIR examples)
 install(PROGRAMS
+    ${EXAMPLES_DIR}/ex_collision_object.py
     ${EXAMPLES_DIR}/ex_gripper_command.py
     ${EXAMPLES_DIR}/ex_gripper.py
     ${EXAMPLES_DIR}/ex_joint_goal.py

diff --git a/README.md b/README.md
@@ -64,8 +64,8 @@ After that, the individual scripts can be run.
 ```bash
 # Move to joint configuration
 ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
-# Move to Cartesian pose
-ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"
+# Move to Cartesian pose (motion in either joint or Cartesian space)
+ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
 # Repeatadly toggle the gripper (or use "open"/"close" actions)
 ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle"
 # Example of using MoveIt 2 Servo to move the end-effector in a circular motion

diff --git a/examples/assets/suzanne.stl b/examples/assets/suzanne.stl
diff --git a/examples/ex_collision_object.py b/examples/ex_collision_object.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+"""
+Example of adding and removing a collision object with a mesh geometry.
+Note: Python module `trimesh` is required for this example (`pip install trimesh`).
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]"`
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p filepath:="./my_favourity_mesh.stl"`
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="remove"`
+"""
+
+from os import path
+from threading import Thread
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+from pymoveit2 import MoveIt2
+from pymoveit2.robots import panda
+
+DEFAULT_EXAMPLE_MESH = path.join(
+    path.dirname(path.realpath(__file__)), "assets", "suzanne.stl"
+)
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    # Create node for this example
+    node = Node("ex_collision_object")
+
+    # Declare parameter for joint positions
+    node.declare_parameter(
+        "filepath",
+        "",
+    )
+    node.declare_parameter(
+        "action",
+        "add",
+    )
+    node.declare_parameter("position", [0.5, 0.0, 0.5])
+    node.declare_parameter("quat_xyzw", [0.0, 0.0, -0.707, 0.707])
+
+    # Create callback group that allows execution of callbacks in parallel without restrictions
+    callback_group = ReentrantCallbackGroup()
+
+    # Create MoveIt 2 interface
+    moveit2 = MoveIt2(
+        node=node,
+        joint_names=panda.joint_names(),
+        base_link_name=panda.base_link_name(),
+        end_effector_name=panda.end_effector_name(),
+        group_name=panda.MOVE_GROUP_ARM,
+        callback_group=callback_group,
+    )
+
+    # Spin the node in background thread(s)
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+
+    # Get parameters
+    filepath = node.get_parameter("filepath").get_parameter_value().string_value
+    action = node.get_parameter("action").get_parameter_value().string_value
+    position = node.get_parameter("position").get_parameter_value().double_array_value
+    quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
+
+    # Use the default example mesh if invalid
+    if not filepath:
+        node.get_logger().info(f"Using the default example mesh file")
+        filepath = DEFAULT_EXAMPLE_MESH
+
+    # Make sure the mesh file exists
+    if not path.exists(filepath):
+        node.get_logger().error(f"File '{filepath}' does not exist")
+        rclpy.shutdown()
+        exit(1)
+
+    # Determine ID of the collision mesh
+    mesh_id = path.basename(filepath).split(".")[0]
+
+    if "add" == action:
+        # Add collision mesh
+        node.get_logger().info(
+            f"Adding collision mesh '{filepath}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+        )
+        moveit2.add_collision_mesh(
+            filepath=filepath, id=mesh_id, position=position, quat_xyzw=quat_xyzw
+        )
+    else:
+        # Remove collision mesh
+        node.get_logger().info(f"Removing collision mesh with ID '{mesh_id}'")
+        moveit2.remove_collision_mesh(id=mesh_id)
+
+    rclpy.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """
 Example of moving to a pose goal.
-`ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"`
+`ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False`
 """
 
 from threading import Thread
@@ -24,6 +24,7 @@ def main(args=None):
     # Declare parameters for position and orientation
     node.declare_parameter("position", [0.5, 0.0, 0.25])
     node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0])
+    node.declare_parameter("cartesian", False)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -47,12 +48,13 @@ def main(args=None):
     # Get parameters
     position = node.get_parameter("position").get_parameter_value().double_array_value
     quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
+    cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value
 
     # Move to pose
     node.get_logger().info(
         f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
     )
-    moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw)
+    moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian)
     moveit2.wait_until_executed()
 
     rclpy.shutdown()