diff --git a/CMakeLists.txt b/CMakeLists.txt index b2087af..aba31f2 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_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 index 504c3b2..a14644e 100644 --- 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 new file mode 100644 index 0000000..7c6e9cf Binary files /dev/null and b/examples/assets/suzanne.stl differ diff --git a/examples/ex_collision_object.py b/examples/ex_collision_object.py new file mode 100755 index 0000000..6802213 --- /dev/null +++ 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 index 13075be..d35c6ea 100755 --- 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() diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 88f64c1..c5019b0 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -6,13 +6,19 @@ from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit_msgs.action import MoveGroup from moveit_msgs.msg import ( + CollisionObject, Constraints, JointConstraint, MoveItErrorCodes, OrientationConstraint, PositionConstraint, ) -from moveit_msgs.srv import GetMotionPlan, GetPositionFK, GetPositionIK +from moveit_msgs.srv import ( + GetCartesianPath, + GetMotionPlan, + GetPositionFK, + GetPositionIK, +) from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup from rclpy.node import Node @@ -23,7 +29,7 @@ QoSReliabilityPolicy, ) from sensor_msgs.msg import JointState -from shape_msgs.msg import SolidPrimitive +from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -131,6 +137,20 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() + # Create a separate service client for Cartesian planning + self._plan_cartesian_path_service = self._node.create_client( + srv_type=GetCartesianPath, + srv_name="compute_cartesian_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__cartesian_path_request = GetCartesianPath.Request() + # Create action client for trajectory execution self.__follow_joint_trajectory_action_client = ActionClient( node=self._node, @@ -169,6 +189,10 @@ def __init__( callback_group=self._callback_group, ) + self.__collision_object_publisher = self._node.create_publisher( + CollisionObject, "/collision_object", 10 + ) + self.__joint_state_mutex = threading.Lock() self.__joint_state = None self.__new_joint_state_available = False @@ -208,6 +232,7 @@ def move_to_pose( tolerance_position: float = 0.001, tolerance_orientation: float = 0.001, weight_position: float = 1.0, + cartesian: bool = False, weight_orientation: float = 1.0, ): """ @@ -255,6 +280,7 @@ def move_to_pose( tolerance_orientation=tolerance_orientation, weight_position=weight_position, weight_orientation=weight_orientation, + cartesian=cartesian, ) ) @@ -263,6 +289,7 @@ def move_to_configuration( joint_positions: List[float], joint_names: Optional[List[str]] = None, tolerance: float = 0.001, + cartesian: bool = False, weight: float = 1.0, ): """ @@ -303,6 +330,7 @@ def move_to_configuration( joint_names=joint_names, tolerance_joint_position=tolerance, weight_joint_position=weight, + cartesian=cartesian, ) ) @@ -322,6 +350,7 @@ def plan( weight_orientation: float = 1.0, weight_joint_position: float = 1.0, start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian: bool = False, ) -> Optional[JointTrajectory]: """ Plan motion based on previously set goals. Optional arguments can be passed in to @@ -372,12 +401,15 @@ def plan( self.__move_action_goal.request.start_state.joint_state = self.joint_state # Plan trajectory by sending a goal (blocking) - if self.__execute_via_moveit: - # Use action client - joint_trajectory = self._send_goal_move_action_plan_only() + if cartesian: + joint_trajectory = self._plan_cartesian_path() else: - # Use service - joint_trajectory = self._plan_kinematic_path() + if self.__execute_via_moveit: + # Use action client + joint_trajectory = self._send_goal_move_action_plan_only() + else: + # Use service + joint_trajectory = self._plan_kinematic_path() # Clear all previous goal constrains self.clear_goal_constraints() @@ -788,6 +820,77 @@ def force_reset_executing_state(self): self.__is_motion_requested = False self.__is_executing = False + def add_collision_mesh( + self, + filepath: str, + id: str, + position: Union[Point, Tuple[float, float, float]], + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + operation: int = CollisionObject.ADD, + frame_id: Optional[str] = None, + ): + """ + Add collision object with a mesh geometry specified by `filepath`. + Note: This function required 'trimesh' Python module to be installed. + """ + + try: + import trimesh + except ImportError as err: + raise ImportError( + "Python module 'trimesh' not found! Please install it manually in order " + "to add collision objects into the MoveIt 2 planning scene." + ) from err + + mesh = trimesh.load(filepath) + 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.meshes.append( + Mesh( + triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces], + vertices=[ + Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices + ], + ) + ) + + msg.id = id + msg.operation = operation + 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 remove_collision_mesh(self, id: str): + """ + Remove collision object specified by its `id`. + """ + + msg = CollisionObject() + msg.id = id + msg.operation = CollisionObject.REMOVE + msg.header.stamp = self._node.get_clock().now().to_msg() + self.__collision_object_publisher.publish(msg) + def __joint_state_callback(self, msg: JointState): # Update only if all relevant joints are included in the message @@ -873,6 +976,71 @@ def _plan_kinematic_path( ) return None + def _plan_cartesian_path( + self, + max_step: float = 0.0025, + wait_for_server_timeout_sec: Optional[float] = 1.0, + ) -> Optional[JointTrajectory]: + + # Re-use request from move action goal + self.__cartesian_path_request.start_state = ( + self.__move_action_goal.request.start_state + ) + self.__cartesian_path_request.group_name = ( + self.__move_action_goal.request.group_name + ) + self.__cartesian_path_request.link_name = self.__end_effector_name + self.__cartesian_path_request.max_step = max_step + + stamp = self._node.get_clock().now().to_msg() + self.__cartesian_path_request.header.stamp = stamp + + self.__cartesian_path_request.path_constraints = ( + self.__move_action_goal.request.path_constraints + ) + for ( + position_constraint + ) in self.__cartesian_path_request.path_constraints.position_constraints: + position_constraint.header.stamp = stamp + for ( + orientation_constraint + ) in self.__cartesian_path_request.path_constraints.orientation_constraints: + orientation_constraint.header.stamp = stamp + # no header in joint_constraint message type + + target_pose = Pose() + target_pose.position = ( + self.__move_action_goal.request.goal_constraints[-1] + .position_constraints[-1] + .constraint_region.primitive_poses[0] + .position + ) + target_pose.orientation = ( + self.__move_action_goal.request.goal_constraints[-1] + .orientation_constraints[-1] + .orientation + ) + + self.__cartesian_path_request.waypoints = [target_pose] + + if not self._plan_cartesian_path_service.wait_for_service( + timeout_sec=wait_for_server_timeout_sec + ): + self._node.get_logger().warn( + f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!" + ) + return None + + res = self._plan_cartesian_path_service.call(self.__cartesian_path_request) + + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + def _send_goal_async_move_action( self, wait_for_server_timeout_sec: Optional[float] = 1.0 ):