Development towards 4.0.0

.pre-commit-config.yaml

@@ -3,7 +3,7 @@
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.4.0
+    rev: v4.6.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -22,13 +22,13 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/pycqa/isort
-    rev: 5.12.0
+    rev: 5.13.2
     hooks:
       - id: isort
         args: ["--profile", "black"]
 
   - repo: https://github.com/psf/black
-    rev: 23.1.0
+    rev: 24.4.2
     hooks:
       - id: black
 
@@ -38,11 +38,11 @@ repos:
       - id: beautysh
 
   - repo: https://github.com/executablebooks/mdformat
-    rev: 0.7.16
+    rev: 0.7.17
     hooks:
       - id: mdformat
 
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.2.4
+    rev: v2.2.6
     hooks:
       - id: codespell

CMakeLists.txt

@@ -11,6 +11,7 @@ ament_python_install_package(pymoveit2)
 # Install examples
 set(EXAMPLES_DIR examples)
 install(PROGRAMS
+    ${EXAMPLES_DIR}/ex_allow_collisions.py
     ${EXAMPLES_DIR}/ex_collision_mesh.py
     ${EXAMPLES_DIR}/ex_collision_primitive.py
     ${EXAMPLES_DIR}/ex_fk.py

README.md

@@ -29,7 +29,7 @@ Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services.
 
 These are the primary dependencies required to use this project.
 
-- ROS 2 [Galactic](https://docs.ros.org/en/galactic/Installation.html), [Humble](https://docs.ros.org/en/humble/Installation.html) or [Rolling](https://docs.ros.org/en/rolling/Installation.html) (tested June 2022)
+- ROS 2 [Galactic](https://docs.ros.org/en/galactic/Installation.html), [Humble](https://docs.ros.org/en/humble/Installation.html) or [Iron](https://docs.ros.org/en/iron/Installation.html)
 - [MoveIt 2](https://moveit.ros.org/install-moveit2/binary) corresponding to the selected ROS 2 distribution
 
 All additional dependencies are installed via [rosdep](https://wiki.ros.org/rosdep) during the building process below.

examples/ex_allow_collisions.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+"""
+Example of (dis)allowing collisions between the robot object and an object with
+the specified ID.
+- ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=true
+- ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=false
+"""
+
+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
+
+
+def main():
+    rclpy.init()
+
+    # Create node for this example
+    node = Node("ex_allow_collisions")
+
+    # Declare parameter for joint positions
+    node.declare_parameter(
+        "id",
+        "box",
+    )
+    node.declare_parameter(
+        "allow",
+        True,
+    )
+
+    # 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) and wait a bit for initialization
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+    node.create_rate(1.0).sleep()
+
+    # Get parameters
+    object_id = node.get_parameter("id").get_parameter_value().string_value
+    allow = node.get_parameter("allow").get_parameter_value().bool_value
+
+    # (Dis)allow collisions
+    moveit2.allow_collisions(object_id, allow)
+    node.get_logger().info(
+        f"{'Allow' if allow else 'Disallow'}ed collisions between all objects and '{object_id}'"
+    )
+
+    rclpy.shutdown()
+    executor_thread.join()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()

examples/ex_collision_mesh.py

@@ -2,7 +2,8 @@
 """
 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_mesh.py --ros-args -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.7071, 0.7071]"
+- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.7071, 0.7071]" -p scale:="[1.0, 1.0, 1.0]"
+- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="move" -p position:="[0.2, 0.0, 0.2]"
 - ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p filepath:="./my_favourity_mesh.stl"
 - ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="remove"
 """
@@ -11,6 +12,7 @@
 from threading import Thread
 
 import rclpy
+import trimesh
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
@@ -39,6 +41,8 @@ def main():
     )
     node.declare_parameter("position", [0.5, 0.0, 0.5])
     node.declare_parameter("quat_xyzw", [0.0, 0.0, -0.7071, 0.7071])
+    node.declare_parameter("scale", [1.0, 1.0, 1.0])
+    node.declare_parameter("preload_mesh", False)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -65,10 +69,14 @@ def main():
     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
+    scale = node.get_parameter("scale").get_parameter_value().double_array_value
+    preload_mesh = node.get_parameter("preload_mesh").get_parameter_value().bool_value
 
     # Use the default example mesh if invalid
     if not filepath:
-        node.get_logger().info(f"Using the default example mesh file")
+        node.get_logger().info(
+            f"Using the default example mesh file {DEFAULT_EXAMPLE_MESH}"
+        )
         filepath = DEFAULT_EXAMPLE_MESH
 
     # Make sure the mesh file exists
@@ -80,21 +88,42 @@ def main():
     # Determine ID of the collision mesh
     object_id = path.basename(filepath).split(".")[0]
 
-    if "add" == action:
+    if action == "add":
         # Add collision mesh
         node.get_logger().info(
-            f"Adding collision mesh '{filepath}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+            f"Adding collision mesh '{filepath}' "
+            f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
         )
+
+        # Load the mesh if specified
+        mesh = None
+        if preload_mesh:
+            mesh = trimesh.load(filepath)
+            filepath = None
+
         moveit2.add_collision_mesh(
             filepath=filepath,
             id=object_id,
             position=position,
             quat_xyzw=quat_xyzw,
+            scale=scale,
+            mesh=mesh,
         )
-    else:
+    elif action == "remove":
         # Remove collision mesh
         node.get_logger().info(f"Removing collision mesh with ID '{object_id}'")
         moveit2.remove_collision_object(id=object_id)
+    elif action == "move":
+        # Move collision mesh
+        node.get_logger().info(
+            f"Moving collision mesh with ID '{object_id}' to "
+            f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+        )
+        moveit2.move_collision(id=object_id, position=position, quat_xyzw=quat_xyzw)
+    else:
+        raise ValueError(
+            f"Unknown action '{action}'. Valid values are 'add', 'remove', 'move'"
+        )
 
     rclpy.shutdown()
     executor_thread.join()

examples/ex_collision_primitive.py

@@ -4,6 +4,7 @@
 - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.5, 0.0, 0.5]" -p dimensions:="[0.04]"
 - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="cylinder" -p position:="[0.2, 0.0, -0.045]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p dimensions:="[0.04, 0.02]"
 - ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p action:="remove" -p shape:="sphere"
+- ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p action:="move" -p shape:="sphere" -p position:="[0.2, 0.0, 0.2]"
 """
 
 from threading import Thread
@@ -67,10 +68,11 @@ def main():
     # Use the name of the primitive shape as the ID
     object_id = shape
 
-    if "add" == action:
+    if action == "add":
         # Add collision primitive
         node.get_logger().info(
-            f"Adding collision primitive of type '{shape}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}, dimensions: {list(dimensions)}}}"
+            f"Adding collision primitive of type '{shape}' "
+            f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}, dimensions: {list(dimensions)}}}"
         )
         if shape == "box":
             moveit2.add_collision_box(
@@ -98,10 +100,21 @@ def main():
             )
         else:
             raise ValueError(f"Unknown shape '{shape}'")
-    else:
+    elif action == "remove":
         # Remove collision primitive
         node.get_logger().info(f"Removing collision primitive with ID '{object_id}'")
         moveit2.remove_collision_object(id=object_id)
+    elif action == "move":
+        # Move collision primitive
+        node.get_logger().info(
+            f"Moving collision primitive with ID '{object_id}' to "
+            f"{{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+        )
+        moveit2.move_collision(id=object_id, position=position, quat_xyzw=quat_xyzw)
+    else:
+        raise ValueError(
+            f"Unknown action '{action}'. Valid values are 'add', 'remove', 'move'"
+        )
 
     rclpy.shutdown()
     executor_thread.join()

examples/ex_pose_goal.py

@@ -25,12 +25,17 @@ def main():
     # 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)
     node.declare_parameter("synchronous", True)
     # If non-positive, don't cancel. Only used if synchronous is False
     node.declare_parameter("cancel_after_secs", 0.0)
     # Planner ID
     node.declare_parameter("planner_id", "RRTConnectkConfigDefault")
+    # Declare parameters for cartesian planning
+    node.declare_parameter("cartesian", False)
+    node.declare_parameter("cartesian_max_step", 0.0025)
+    node.declare_parameter("cartesian_fraction_threshold", 0.0)
+    node.declare_parameter("cartesian_jump_threshold", 0.0)
+    node.declare_parameter("cartesian_avoid_collisions", False)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -62,17 +67,45 @@ def main():
     # 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
     synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
     cancel_after_secs = (
         node.get_parameter("cancel_after_secs").get_parameter_value().double_value
     )
+    cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value
+    cartesian_max_step = (
+        node.get_parameter("cartesian_max_step").get_parameter_value().double_value
+    )
+    cartesian_fraction_threshold = (
+        node.get_parameter("cartesian_fraction_threshold")
+        .get_parameter_value()
+        .double_value
+    )
+    cartesian_jump_threshold = (
+        node.get_parameter("cartesian_jump_threshold")
+        .get_parameter_value()
+        .double_value
+    )
+    cartesian_avoid_collisions = (
+        node.get_parameter("cartesian_avoid_collisions")
+        .get_parameter_value()
+        .bool_value
+    )
+
+    # Set parameters for cartesian planning
+    moveit2.cartesian_avoid_collisions = cartesian_avoid_collisions
+    moveit2.cartesian_jump_threshold = cartesian_jump_threshold
 
     # 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, cartesian=cartesian)
+    moveit2.move_to_pose(
+        position=position,
+        quat_xyzw=quat_xyzw,
+        cartesian=cartesian,
+        cartesian_max_step=cartesian_max_step,
+        cartesian_fraction_threshold=cartesian_fraction_threshold,
+    )
     if synchronous:
         # Note: the same functionality can be achieved by setting
         # `synchronous:=false` and `cancel_after_secs` to a negative value.