From f948565a0e62e57073a7788bae5ab002a15e8831 Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Sat, 30 Mar 2024 05:46:27 -0700
Subject: [PATCH 01/11] Allow users to (dis)allow collisions with an object
 (#50)

* Added functions to enable/disable collisions with an object vis the allowed collision matrix

* Clarified function name, reduced replicated code

* Made toggling collisions async

* Fix type annotations for consistency

* formatting

* Added tests

* Spelling

* Update docstring

* Fix unnecessary change
---
 CMakeLists.txt                  |   1 +
 examples/ex_allow_collisions.py |  71 +++++++++++++++++++
 pymoveit2/moveit2.py            | 116 ++++++++++++++++++++++++++++++++
 3 files changed, 188 insertions(+)
 create mode 100755 examples/ex_allow_collisions.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 43d5fc3..8e195a8 100644
--- 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_allow_collisions.py
     ${EXAMPLES_DIR}/ex_collision_mesh.py
     ${EXAMPLES_DIR}/ex_collision_primitive.py
     ${EXAMPLES_DIR}/ex_fk.py
diff --git a/examples/ex_allow_collisions.py b/examples/ex_allow_collisions.py
new file mode 100755
index 0000000..d6519a0
--- /dev/null
+++ b/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()
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 6d7684e..38566cd 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -1,3 +1,4 @@
+import copy
 import threading
 from enum import Enum
 from typing import List, Optional, Tuple, Union
@@ -6,6 +7,7 @@
 from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
 from moveit_msgs.action import ExecuteTrajectory, MoveGroup
 from moveit_msgs.msg import (
+    AllowedCollisionEntry,
     AttachedCollisionObject,
     CollisionObject,
     Constraints,
@@ -15,8 +17,10 @@
     PositionConstraint,
 )
 from moveit_msgs.srv import (
+    ApplyPlanningScene,
     GetCartesianPath,
     GetMotionPlan,
+    GetPlanningScene,
     GetPositionFK,
     GetPositionIK,
 )
@@ -221,6 +225,34 @@ def __init__(
             callback_group=self._callback_group,
         )
 
+        # Create a service for getting the planning scene
+        self._get_planning_scene_service = self._node.create_client(
+            srv_type=GetPlanningScene,
+            srv_name="get_planning_scene",
+            qos_profile=QoSProfile(
+                durability=QoSDurabilityPolicy.VOLATILE,
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                history=QoSHistoryPolicy.KEEP_LAST,
+                depth=1,
+            ),
+            callback_group=callback_group,
+        )
+        self.__planning_scene = None
+        self.__old_allowed_collision_matrix = None
+
+        # Create a service for applying the planning scene
+        self._apply_planning_scene_service = self._node.create_client(
+            srv_type=ApplyPlanningScene,
+            srv_name="apply_planning_scene",
+            qos_profile=QoSProfile(
+                durability=QoSDurabilityPolicy.VOLATILE,
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                history=QoSHistoryPolicy.KEEP_LAST,
+                depth=1,
+            ),
+            callback_group=callback_group,
+        )
+
         self.__collision_object_publisher = self._node.create_publisher(
             CollisionObject, "/collision_object", 10
         )
@@ -1691,6 +1723,90 @@ def detach_all_collision_objects(self):
         )
         self.__attached_collision_object_publisher.publish(msg)
 
+    def __update_planning_scene(self) -> bool:
+        """
+        Gets the current planning scene. Returns whether the service call was
+        successful.
+        """
+
+        if not self._get_planning_scene_service.service_is_ready():
+            self._node.get_logger().warn(
+                f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
+            )
+            return False
+        self.__planning_scene = self._get_planning_scene_service.call(
+            GetPlanningScene.Request()
+        ).scene
+        return True
+
+    def allow_collisions(self, id: str, allow: bool) -> Optional[Future]:
+        """
+        Takes in the ID of an element in the planning scene. Modifies the allowed
+        collision matrix to (dis)allow collisions between that object and all other
+        object.
+
+        If `allow` is True, a plan will succeed even if the robot collides with that object.
+        If `allow` is False, a plan will fail if the robot collides with that object.
+        Returns whether it successfully updated the allowed collision matrix.
+
+        Returns the future of the service call.
+        """
+        # Update the planning scene
+        if not self.__update_planning_scene():
+            return None
+        allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix
+        self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix)
+
+        # Get the location in the allowed collision matrix of the object
+        j = None
+        if id not in allowed_collision_matrix.entry_names:
+            allowed_collision_matrix.entry_names.append(id)
+        else:
+            j = allowed_collision_matrix.entry_names.index(id)
+        # For all other objects, (dis)allow collisions with the object with `id`
+        for i in range(len(allowed_collision_matrix.entry_values)):
+            if j is None:
+                allowed_collision_matrix.entry_values[i].enabled.append(allow)
+            elif i != j:
+                allowed_collision_matrix.entry_values[i].enabled[j] = allow
+        # For the object with `id`, (dis)allow collisions with all other objects
+        allowed_collision_entry = AllowedCollisionEntry(
+            enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))]
+        )
+        if j is None:
+            allowed_collision_matrix.entry_values.append(allowed_collision_entry)
+        else:
+            allowed_collision_matrix.entry_values[j] = allowed_collision_entry
+
+        # Apply the new planning scene
+        if not self._apply_planning_scene_service.service_is_ready():
+            self._node.get_logger().warn(
+                f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
+            )
+            return None
+        return self._apply_planning_scene_service.call_async(
+            ApplyPlanningScene.Request(scene=self.__planning_scene)
+        )
+
+    def process_allow_collision_future(self, future: Future) -> bool:
+        """
+        Return whether the allow collision service call is done and has succeeded
+        or not. If it failed, reset the allowed collision matrix to the old one.
+        """
+        if not future.done():
+            return False
+
+        # Get response
+        resp = future.result()
+
+        # If it failed, restore the old planning scene
+        if not resp.success:
+            self.__planning_scene.allowed_collision_matrix = (
+                self.__old_allowed_collision_matrix
+            )
+
+        return resp.success
+
     def __joint_state_callback(self, msg: JointState):
         # Update only if all relevant joints are included in the message
         for joint_name in self.joint_names:

From 13b2b70c2bc0f976bcdbcd1584eb5c7baa86f55a Mon Sep 17 00:00:00 2001
From: Andrej Orsula <orsula.andrej@gmail.com>
Date: Sat, 30 Mar 2024 13:47:41 +0100
Subject: [PATCH 02/11] Use `cartesian_speed_limited_link` instead of
 `cartesian_speed_end_effector_link` when available (#56)

* Update msg

* Use  `cartesian_speed_limited_link` instead of `cartesian_speed_end_effector_link` when available

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

---------

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
Co-authored-by: Apurv Saha <apurv.robotics@gmail.com>
---
 pymoveit2/moveit2.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 38566cd..ef2ca26 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -2056,7 +2056,11 @@ def __init_move_action_goal(
         move_action_goal.request.allowed_planning_time = 0.5
         move_action_goal.request.max_velocity_scaling_factor = 0.0
         move_action_goal.request.max_acceleration_scaling_factor = 0.0
-        move_action_goal.request.cartesian_speed_end_effector_link = end_effector
+        # Note: Attribute was renamed in Iron (https://github.com/ros-planning/moveit_msgs/pull/130)
+        if hasattr(move_action_goal.request, "cartesian_speed_limited_link"):
+            move_action_goal.request.cartesian_speed_limited_link = end_effector
+        else:
+            move_action_goal.request.cartesian_speed_end_effector_link = end_effector
         move_action_goal.request.max_cartesian_speed = 0.0
 
         # move_action_goal.planning_options.planning_scene_diff = "Ignored"

From fe26066112b8f50023c93af336e65425a535990a Mon Sep 17 00:00:00 2001
From: Zarnack <56802851+Zarnack@users.noreply.github.com>
Date: Thu, 4 Apr 2024 11:41:14 +0200
Subject: [PATCH 03/11] Added setting of cartesian speed and acceleration 
 (#53)

* added re-use of max_velocity_scaling_factor and max_acceleration_scaling_factor from move_action goal to cartesian_path_request

* Removed getter and setter of 'max_cartesian_speed' to avoid confusion
as the corresponding value of the MotionPlanRequest.msg
(respectively the self.__move_action_goal.request object) is currently
not used in moveit2.
--> The 'max_cartesian_speed' variable requires the
'default_planner_request_adapters/SetMaxCartesianEndEffectorSpeed'
which currently only exists in moveit1.
---
 pymoveit2/moveit2.py | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index ef2ca26..45db2e5 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -1855,6 +1855,15 @@ def _plan_cartesian_path(
         self.__cartesian_path_request.start_state = (
             self.__move_action_goal.request.start_state
         )
+
+        self.__cartesian_path_request.max_velocity_scaling_factor = (
+            self.__move_action_goal.request.max_velocity_scaling_factor
+        )
+
+        self.__cartesian_path_request.max_acceleration_scaling_factor = (
+            self.__move_action_goal.request.max_acceleration_scaling_factor
+        )
+
         self.__cartesian_path_request.group_name = (
             self.__move_action_goal.request.group_name
         )
@@ -2152,14 +2161,6 @@ def max_acceleration(self) -> float:
     def max_acceleration(self, value: float):
         self.__move_action_goal.request.max_acceleration_scaling_factor = value
 
-    @property
-    def max_cartesian_speed(self) -> float:
-        return self.__move_action_goal.request.max_cartesian_speed
-
-    @max_cartesian_speed.setter
-    def max_cartesian_speed(self, value: float):
-        self.__move_action_goal.request.max_cartesian_speed = value
-
     @property
     def num_planning_attempts(self) -> int:
         return self.__move_action_goal.request.num_planning_attempts

From 3f87e3b4f106c6dcc8b1316d0e3ef36ca87f4d7b Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Thu, 4 Apr 2024 02:53:24 -0700
Subject: [PATCH 04/11] Allow users to set various cartesian path service
 parameters (#58)

* Added ability to have cartesian planning calls avoid collisions

* Fix bad merge

* Allow users to set cartesian jump threshold

* Allow users to set cartesian max step

* Pass frame_id into the cartesian planning service

* Add ability to specify a required fraction for a cartesian plan to succeed

* Allowed users to get end effector name and base link name

* Add revolute and prismatic jump thresholds

* Formatting

* Added test cases
---
 examples/ex_pose_goal.py | 39 ++++++++++++++++++--
 pymoveit2/moveit2.py     | 79 +++++++++++++++++++++++++++++++++++++---
 2 files changed, 110 insertions(+), 8 deletions(-)

diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py
index 401fd19..fa701ad 100755
--- a/examples/ex_pose_goal.py
+++ b/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.
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 45db2e5..b3918be 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -344,6 +344,8 @@ def move_to_pose(
         weight_position: float = 1.0,
         cartesian: bool = False,
         weight_orientation: float = 1.0,
+        cartesian_max_step: float = 0.0025,
+        cartesian_fraction_threshold: float = 0.0,
     ):
         """
         Plan and execute motion based on previously set goals. Optional arguments can be
@@ -428,6 +430,8 @@ def move_to_pose(
                     weight_position=weight_position,
                     weight_orientation=weight_orientation,
                     cartesian=cartesian,
+                    max_step=cartesian_max_step,
+                    cartesian_fraction_threshold=cartesian_fraction_threshold,
                 )
             )
 
@@ -500,12 +504,18 @@ def plan(
         weight_joint_position: float = 1.0,
         start_joint_state: Optional[Union[JointState, List[float]]] = None,
         cartesian: bool = False,
+        max_step: float = 0.0025,
+        cartesian_fraction_threshold: float = 0.0,
     ) -> Optional[JointTrajectory]:
         """
         Call plan_async and wait on future
         """
         future = self.plan_async(
-            **{key: value for key, value in locals().items() if key != "self"}
+            **{
+                key: value
+                for key, value in locals().items()
+                if key not in ["self", "cartesian_fraction_threshold"]
+            }
         )
 
         if future is None:
@@ -516,7 +526,11 @@ def plan(
         while not future.done():
             rate.sleep()
 
-        return self.get_trajectory(future, cartesian=cartesian)
+        return self.get_trajectory(
+            future,
+            cartesian=cartesian,
+            cartesian_fraction_threshold=cartesian_fraction_threshold,
+        )
 
     def plan_async(
         self,
@@ -537,6 +551,7 @@ def plan_async(
         weight_joint_position: float = 1.0,
         start_joint_state: Optional[Union[JointState, List[float]]] = None,
         cartesian: bool = False,
+        max_step: float = 0.0025,
     ) -> Optional[Future]:
         """
         Plan motion based on previously set goals. Optional arguments can be passed in to
@@ -634,9 +649,10 @@ def plan_async(
         # Plan trajectory asynchronously by service call
         if cartesian:
             future = self._plan_cartesian_path(
+                max_step=max_step,
                 frame_id=pose_stamped.header.frame_id
                 if pose_stamped is not None
-                else frame_id
+                else frame_id,
             )
         else:
             # Use service
@@ -649,11 +665,17 @@ def plan_async(
         return future
 
     def get_trajectory(
-        self, future: Future, cartesian: bool = False
+        self,
+        future: Future,
+        cartesian: bool = False,
+        cartesian_fraction_threshold: float = 0.0,
     ) -> Optional[JointTrajectory]:
         """
         Takes in a future returned by plan_async and returns the trajectory if the future is done
         and planning was successful, else None.
+
+        For cartesian plans, the plan is rejected if the fraction of the path that was completed is
+        less than `cartesian_fraction_threshold`.
         """
         if not future.done():
             self._node.get_logger().warn(
@@ -666,7 +688,14 @@ def get_trajectory(
         # Cartesian
         if cartesian:
             if MoveItErrorCodes.SUCCESS == res.error_code.val:
-                return res.solution.joint_trajectory
+                if res.fraction >= cartesian_fraction_threshold:
+                    return res.solution.joint_trajectory
+                else:
+                    self._node.get_logger().warn(
+                        f"Planning failed! Cartesian planner completed {res.fraction} "
+                        f"of the trajectory, less than the threshold {cartesian_fraction_threshold}."
+                    )
+                    return None
             else:
                 self._node.get_logger().warn(
                     f"Planning failed! Error code: {res.error_code.val}."
@@ -2130,6 +2159,14 @@ def __init_compute_ik(self):
     def follow_joint_trajectory_action_client(self) -> str:
         return self.__follow_joint_trajectory_action_client
 
+    @property
+    def end_effector_name(self) -> str:
+        return self.__end_effector_name
+
+    @property
+    def base_link_name(self) -> str:
+        return self.__base_link_name
+
     @property
     def joint_names(self) -> List[str]:
         return self.__joint_names
@@ -2177,6 +2214,38 @@ def allowed_planning_time(self) -> float:
     def allowed_planning_time(self, value: float):
         self.__move_action_goal.request.allowed_planning_time = value
 
+    @property
+    def cartesian_avoid_collisions(self) -> bool:
+        return self.__cartesian_path_request.request.avoid_collisions
+
+    @cartesian_avoid_collisions.setter
+    def cartesian_avoid_collisions(self, value: bool):
+        self.__cartesian_path_request.avoid_collisions = value
+
+    @property
+    def cartesian_jump_threshold(self) -> float:
+        return self.__cartesian_path_request.request.jump_threshold
+
+    @cartesian_jump_threshold.setter
+    def cartesian_jump_threshold(self, value: float):
+        self.__cartesian_path_request.jump_threshold = value
+
+    @property
+    def cartesian_prismatic_jump_threshold(self) -> float:
+        return self.__cartesian_path_request.request.prismatic_jump_threshold
+
+    @cartesian_prismatic_jump_threshold.setter
+    def cartesian_prismatic_jump_threshold(self, value: float):
+        self.__cartesian_path_request.prismatic_jump_threshold = value
+
+    @property
+    def cartesian_revolute_jump_threshold(self) -> float:
+        return self.__cartesian_path_request.request.revolute_jump_threshold
+
+    @cartesian_revolute_jump_threshold.setter
+    def cartesian_revolute_jump_threshold(self, value: float):
+        self.__cartesian_path_request.revolute_jump_threshold = value
+
     @property
     def pipeline_id(self) -> int:
         return self.__move_action_goal.request.pipeline_id

From 4b73e331954c6162833413b1aa18b533370634f8 Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Mon, 8 Apr 2024 02:40:02 -0700
Subject: [PATCH 05/11] Add ability to move collision objects (#59)

* Added a function to move collision objects

* Add examples
---
 examples/ex_collision_mesh.py      | 19 ++++++++++++---
 examples/ex_collision_primitive.py | 19 ++++++++++++---
 pymoveit2/moveit2.py               | 38 ++++++++++++++++++++++++++++++
 3 files changed, 70 insertions(+), 6 deletions(-)

diff --git a/examples/ex_collision_mesh.py b/examples/ex_collision_mesh.py
index a705652..0fa16fe 100755
--- a/examples/ex_collision_mesh.py
+++ b/examples/ex_collision_mesh.py
@@ -3,6 +3,7 @@
 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 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"
 """
@@ -80,10 +81,11 @@ 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)}}}"
         )
         moveit2.add_collision_mesh(
             filepath=filepath,
@@ -91,10 +93,21 @@ def main():
             position=position,
             quat_xyzw=quat_xyzw,
         )
-    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()
diff --git a/examples/ex_collision_primitive.py b/examples/ex_collision_primitive.py
index 5d3b2f8..5b2d8d5 100755
--- a/examples/ex_collision_primitive.py
+++ b/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()
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index b3918be..90ed568 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -1752,6 +1752,44 @@ def detach_all_collision_objects(self):
         )
         self.__attached_collision_object_publisher.publish(msg)
 
+    def move_collision(
+        self,
+        id: str,
+        position: Union[Point, Tuple[float, float, float]],
+        quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
+        frame_id: Optional[str] = None,
+    ):
+        """
+        Move collision object specified by its `id`.
+        """
+
+        msg = CollisionObject()
+
+        if not isinstance(position, Point):
+            position = Point(
+                x=float(position[0]), y=float(position[1]), z=float(position[2])
+            )
+        if not isinstance(quat_xyzw, Quaternion):
+            quat_xyzw = Quaternion(
+                x=float(quat_xyzw[0]),
+                y=float(quat_xyzw[1]),
+                z=float(quat_xyzw[2]),
+                w=float(quat_xyzw[3]),
+            )
+
+        pose = Pose()
+        pose.position = position
+        pose.orientation = quat_xyzw
+        msg.pose = pose
+        msg.id = id
+        msg.operation = CollisionObject.MOVE
+        msg.header.frame_id = (
+            frame_id if frame_id is not None else self.__base_link_name
+        )
+        msg.header.stamp = self._node.get_clock().now().to_msg()
+
+        self.__collision_object_publisher.publish(msg)
+
     def __update_planning_scene(self) -> bool:
         """
         Gets the current planning scene. Returns whether the service call was

From 044940201aa3a8b6d3502d219828eee48f5b8f57 Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Fri, 26 Apr 2024 04:26:13 -0700
Subject: [PATCH 06/11] Allow Scaling Collision Meshes (#62)

* Allow scaling of collision meshes

* Added trimesh to rosdeps for easier installation

* Ran tests with example

* Remove trimesh as a required dependency
---
 examples/ex_collision_mesh.py |  9 +++++++--
 pymoveit2/moveit2.py          | 11 +++++++++++
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/examples/ex_collision_mesh.py b/examples/ex_collision_mesh.py
index 0fa16fe..f0bea29 100755
--- a/examples/ex_collision_mesh.py
+++ b/examples/ex_collision_mesh.py
@@ -2,7 +2,7 @@
 """
 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"
@@ -40,6 +40,7 @@ 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])
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -66,10 +67,13 @@ 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
 
     # 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
@@ -92,6 +96,7 @@ def main():
             id=object_id,
             position=position,
             quat_xyzw=quat_xyzw,
+            scale=scale,
         )
     elif action == "remove":
         # Remove collision mesh
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 90ed568..ceb3670 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -3,6 +3,7 @@
 from enum import Enum
 from typing import List, Optional, Tuple, Union
 
+import numpy as np
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
 from moveit_msgs.action import ExecuteTrajectory, MoveGroup
@@ -1618,12 +1619,14 @@ def add_collision_mesh(
         ] = None,
         frame_id: Optional[str] = None,
         operation: int = CollisionObject.ADD,
+        scale: Union[float, Tuple[float, float, float]] = 1.0,
     ):
         """
         Add collision object with a mesh geometry specified by `filepath`.
         Note: This function required 'trimesh' Python module to be installed.
         """
 
+        # Load the mesh
         try:
             import trimesh
         except ImportError as err:
@@ -1679,6 +1682,14 @@ def add_collision_mesh(
         )
 
         mesh = trimesh.load(filepath)
+
+        # Scale the mesh
+        if isinstance(scale, float):
+            scale = (scale, scale, scale)
+        transform = np.eye(4)
+        np.fill_diagonal(transform, scale)
+        mesh.apply_transform(transform)
+
         msg.meshes.append(
             Mesh(
                 triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],

From d034cd2258ef2a79e6de407d9d823fbec4f7699b Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Tue, 7 May 2024 02:12:21 -0700
Subject: [PATCH 07/11] Reinstate Humble Compatibility for `GetCartesianPath` &
 Expose `planning_scene` To User (#66)

* Reinstate Humble compatibility for GetCartesianPath

* Expose planning scene to user
---
 pymoveit2/moveit2.py | 25 ++++++++++++++++---------
 1 file changed, 16 insertions(+), 9 deletions(-)

diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index ceb3670..5cbc753 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -15,6 +15,7 @@
     JointConstraint,
     MoveItErrorCodes,
     OrientationConstraint,
+    PlanningScene,
     PositionConstraint,
 )
 from moveit_msgs.srv import (
@@ -1801,7 +1802,7 @@ def move_collision(
 
         self.__collision_object_publisher.publish(msg)
 
-    def __update_planning_scene(self) -> bool:
+    def update_planning_scene(self) -> bool:
         """
         Gets the current planning scene. Returns whether the service call was
         successful.
@@ -1830,7 +1831,7 @@ def allow_collisions(self, id: str, allow: bool) -> Optional[Future]:
         Returns the future of the service call.
         """
         # Update the planning scene
-        if not self.__update_planning_scene():
+        if not self.update_planning_scene():
             return None
         allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix
         self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix)
@@ -1934,13 +1935,15 @@ def _plan_cartesian_path(
             self.__move_action_goal.request.start_state
         )
 
-        self.__cartesian_path_request.max_velocity_scaling_factor = (
-            self.__move_action_goal.request.max_velocity_scaling_factor
-        )
-
-        self.__cartesian_path_request.max_acceleration_scaling_factor = (
-            self.__move_action_goal.request.max_acceleration_scaling_factor
-        )
+        # The below attributes were introduced in Iron and do not exist in Humble.
+        if hasattr(__cartesian_path_request, "max_velocity_scaling_factor"):
+            self.__cartesian_path_request.max_velocity_scaling_factor = (
+                self.__move_action_goal.request.max_velocity_scaling_factor
+            )
+        if hasattr(__cartesian_path_request, "max_acceleration_scaling_factor"):
+            self.__cartesian_path_request.max_acceleration_scaling_factor = (
+                self.__move_action_goal.request.max_acceleration_scaling_factor
+            )
 
         self.__cartesian_path_request.group_name = (
             self.__move_action_goal.request.group_name
@@ -2204,6 +2207,10 @@ def __init_compute_ik(self):
         # self.__compute_ik_req.ik_request.timeout.sec = "Ignored"
         # self.__compute_ik_req.ik_request.timeout.nanosec = "Ignored"
 
+    @property
+    def planning_scene(self) -> Optional[PlanningScene]:
+        return self.__planning_scene
+
     @property
     def follow_joint_trajectory_action_client(self) -> str:
         return self.__follow_joint_trajectory_action_client

From 38467ad463cf1f6d755c7c4930453eace5ce4d03 Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Fri, 10 May 2024 06:39:06 -0700
Subject: [PATCH 08/11] Fix bug in previous PR (#68)

---
 pymoveit2/moveit2.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 5cbc753..69aa1bc 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -1936,11 +1936,11 @@ def _plan_cartesian_path(
         )
 
         # The below attributes were introduced in Iron and do not exist in Humble.
-        if hasattr(__cartesian_path_request, "max_velocity_scaling_factor"):
+        if hasattr(self.__cartesian_path_request, "max_velocity_scaling_factor"):
             self.__cartesian_path_request.max_velocity_scaling_factor = (
                 self.__move_action_goal.request.max_velocity_scaling_factor
             )
-        if hasattr(__cartesian_path_request, "max_acceleration_scaling_factor"):
+        if hasattr(self.__cartesian_path_request, "max_acceleration_scaling_factor"):
             self.__cartesian_path_request.max_acceleration_scaling_factor = (
                 self.__move_action_goal.request.max_acceleration_scaling_factor
             )

From 3896807a40e61b55de7224b13f5fa4d03be361f9 Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Fri, 10 May 2024 06:40:48 -0700
Subject: [PATCH 09/11] Allow users to preload a collision mesh (#67)

---
 examples/ex_collision_mesh.py | 11 +++++++++++
 pymoveit2/moveit2.py          | 18 ++++++++++++++----
 2 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/examples/ex_collision_mesh.py b/examples/ex_collision_mesh.py
index f0bea29..fa6ec0d 100755
--- a/examples/ex_collision_mesh.py
+++ b/examples/ex_collision_mesh.py
@@ -12,6 +12,7 @@
 from threading import Thread
 
 import rclpy
+import trimesh
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
@@ -41,6 +42,7 @@ 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()
@@ -68,6 +70,7 @@ def main():
     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:
@@ -91,12 +94,20 @@ def main():
             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,
         )
     elif action == "remove":
         # Remove collision mesh
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 69aa1bc..6127bd9 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -1,7 +1,7 @@
 import copy
 import threading
 from enum import Enum
-from typing import List, Optional, Tuple, Union
+from typing import Any, List, Optional, Tuple, Union
 
 import numpy as np
 from action_msgs.msg import GoalStatus
@@ -1611,7 +1611,7 @@ def add_collision_cone(
 
     def add_collision_mesh(
         self,
-        filepath: str,
+        filepath: Optional[str],
         id: str,
         pose: Optional[Union[PoseStamped, Pose]] = None,
         position: Optional[Union[Point, Tuple[float, float, float]]] = None,
@@ -1621,9 +1621,11 @@ def add_collision_mesh(
         frame_id: Optional[str] = None,
         operation: int = CollisionObject.ADD,
         scale: Union[float, Tuple[float, float, float]] = 1.0,
+        mesh: Optional[Any] = None,
     ):
         """
-        Add collision object with a mesh geometry specified by `filepath`.
+        Add collision object with a mesh geometry. Either `filepath` must be
+        specified or `mesh` must be provided.
         Note: This function required 'trimesh' Python module to be installed.
         """
 
@@ -1636,10 +1638,17 @@ def add_collision_mesh(
                 "to add collision objects into the MoveIt 2 planning scene."
             ) from err
 
+        # Check the parameters
         if (pose is None) and (position is None or quat_xyzw is None):
             raise ValueError(
                 "Either `pose` or `position` and `quat_xyzw` must be specified!"
             )
+        if (filepath is None and mesh is None) or (
+            filepath is not None and mesh is not None
+        ):
+            raise ValueError("Exactly one of `filepath` or `mesh` must be specified!")
+        if mesh is not None and not isinstance(mesh, trimesh.Trimesh):
+            raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!")
 
         if isinstance(pose, PoseStamped):
             pose_stamped = pose
@@ -1682,7 +1691,8 @@ def add_collision_mesh(
             pose=pose_stamped.pose,
         )
 
-        mesh = trimesh.load(filepath)
+        if filepath is not None:
+            mesh = trimesh.load(filepath)
 
         # Scale the mesh
         if isinstance(scale, float):

From 2f4b35fa2876914dabbae87a2b98113454a0117a Mon Sep 17 00:00:00 2001
From: Andrej Orsula <orsula.andrej@gmail.com>
Date: Fri, 10 May 2024 16:35:47 +0200
Subject: [PATCH 10/11] Update list of supported ROS 2 distributions

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index dad4a07..87a05e9 100644
--- a/README.md
+++ b/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.

From 2e4fdddff2f01544f3f15fdab36f23edfa7647d5 Mon Sep 17 00:00:00 2001
From: Andrej Orsula <orsula.andrej@gmail.com>
Date: Fri, 10 May 2024 16:37:14 +0200
Subject: [PATCH 11/11] Autoupdate pre-commit hooks

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
---
 .pre-commit-config.yaml | 10 +++----
 pymoveit2/moveit2.py    | 66 +++++++++++++++++++++--------------------
 2 files changed, 39 insertions(+), 37 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 6b11f54..6a08d1c 100644
--- a/.pre-commit-config.yaml
+++ b/.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
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 6127bd9..1a5619b 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -360,9 +360,9 @@ def move_to_pose(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=pose,
             )
@@ -381,9 +381,9 @@ def move_to_pose(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=Pose(position=position, orientation=quat_xyzw),
             )
@@ -571,9 +571,9 @@ def plan_async(
                 pose_stamped = PoseStamped(
                     header=Header(
                         stamp=self._node.get_clock().now().to_msg(),
-                        frame_id=frame_id
-                        if frame_id is not None
-                        else self.__base_link_name,
+                        frame_id=(
+                            frame_id if frame_id is not None else self.__base_link_name
+                        ),
                     ),
                     pose=pose,
                 )
@@ -652,9 +652,11 @@ def plan_async(
         if cartesian:
             future = self._plan_cartesian_path(
                 max_step=max_step,
-                frame_id=pose_stamped.header.frame_id
-                if pose_stamped is not None
-                else frame_id,
+                frame_id=(
+                    pose_stamped.header.frame_id
+                    if pose_stamped is not None
+                    else frame_id
+                ),
             )
         else:
             # Use service
@@ -807,9 +809,9 @@ def set_pose_goal(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=pose,
             )
@@ -828,9 +830,9 @@ def set_pose_goal(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=Pose(position=position, orientation=quat_xyzw),
             )
@@ -1451,9 +1453,9 @@ def add_collision_primitive(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=pose,
             )
@@ -1472,9 +1474,9 @@ def add_collision_primitive(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=Pose(position=position, orientation=quat_xyzw),
             )
@@ -1656,9 +1658,9 @@ def add_collision_mesh(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=pose,
             )
@@ -1677,9 +1679,9 @@ def add_collision_mesh(
             pose_stamped = PoseStamped(
                 header=Header(
                     stamp=self._node.get_clock().now().to_msg(),
-                    frame_id=frame_id
-                    if frame_id is not None
-                    else self.__base_link_name,
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
                 ),
                 pose=Pose(position=position, orientation=quat_xyzw),
             )
@@ -1908,7 +1910,7 @@ def __joint_state_callback(self, msg: JointState):
         self.__joint_state_mutex.release()
 
     def _plan_kinematic_path(self) -> Optional[Future]:
-        # Re-use request from move action goal
+        # Reuse request from move action goal
         self.__kinematic_path_request.motion_plan_request = (
             self.__move_action_goal.request
         )
@@ -1940,7 +1942,7 @@ def _plan_cartesian_path(
         max_step: float = 0.0025,
         frame_id: Optional[str] = None,
     ) -> Optional[Future]:
-        # Re-use request from move action goal
+        # Reuse request from move action goal
         self.__cartesian_path_request.start_state = (
             self.__move_action_goal.request.start_state
         )