Merge pull request #212 from lbr-stack/dev-humble-cartesian-velocity

MoveIt Servo Support

CHANGELOG.rst

@@ -1,6 +1,13 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package LBR FRI ROS 2 Stack
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Humble v2.1.2 (2024-10-18)
+--------------------------
+* Adds MoveIt Servo demo, related to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/50 and https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/211
+
+  * ``lbr_bringup``: New launch mixin and launch file for MoveIt Servo
+  * ``lbr_moveit``: New keyboard driver to interface with MoveIt Servo
+
 Humble v2.1.1 (2024-09-27)
 --------------------------
 * Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563).

CITATION.cff

@@ -19,6 +19,6 @@ authors:
 
 
 title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots"
-version: 2.1.1
+version: 2.1.2
 doi: 10.48550/arXiv.2311.12709
-date-released: 2024-09-27
+date-released: 2024-10-18

lbr_bringup/config/moveit_servo.yaml

@@ -0,0 +1,72 @@
+# Please do e.g. refer to
+#   - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml
+#   - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml
+/**/servo_node:
+  ros__parameters:
+    moveit_servo:
+      ## Properties of incoming commands
+      command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+      scale:
+        # Scale parameters are only used if command_in_type=="unitless"
+        linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+        rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+        # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
+        joint: 0.5
+
+      # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+      # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+      ## Properties of outgoing commands
+      publish_period: 0.005 # #1/controller manager update rate [seconds]
+      low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+      # What type of topic does your robot driver expect?
+      # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+      command_out_type: std_msgs/Float64MultiArray
+
+      # What to publish? Can save some bandwidth as most robots only require positions or velocities
+      publish_joint_positions: true
+      publish_joint_velocities: false
+      publish_joint_accelerations: false
+
+      ## Plugins for smoothing outgoing commands
+      smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+      # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
+      # which other nodes can use as a source for information about the planning environment.
+      # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
+      # then is_primary_planning_scene_monitor needs to be set to false.
+      is_primary_planning_scene_monitor: true
+
+      ## MoveIt properties
+      move_group_name: arm # Often 'manipulator' or 'arm'
+      planning_frame: link_0 # The MoveIt planning frame. Often 'base_link' or 'world'
+
+      ## Other frames
+      ee_frame_name: link_ee # The name of the end effector link, used to return the EE pose
+      robot_link_command_frame: link_0 # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+      ## Stopping behaviour
+      incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
+      # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+      # Important because ROS may drop some messages and we need the robot to halt reliably.
+      num_outgoing_halt_msgs_to_publish: 4
+
+      ## Configure handling of singularities and joint limits
+      lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
+      hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+      joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+      leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+
+      ## Topic names
+      cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
+      joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+      joint_topic: /lbr/joint_states
+      status_topic: ~/status # Publish status to this topic
+      command_out_topic: /lbr/forward_position_controller/commands # Publish outgoing commands here
+
+      ## Collision checking for the entire robot body
+      check_collisions: true # Check collisions?
+      collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+      self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+      scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

lbr_bringup/launch/move_group.launch.py

@@ -5,7 +5,7 @@
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from lbr_bringup.description import LBRDescriptionMixin
-from lbr_bringup.move_group import LBRMoveGroupMixin
+from lbr_bringup.moveit import LBRMoveGroupMixin
 from lbr_bringup.rviz import RVizMixin
 
 

lbr_bringup/launch/moveit_servo.launch.py

@@ -0,0 +1,72 @@
+from typing import List
+
+from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
+from launch.actions import OpaqueFunction, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.moveit import LBRMoveGroupMixin, LBRMoveItServoMixin
+
+
+def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
+    ld = LaunchDescription()
+
+    moveit_servo_config = PathJoinSubstitution(
+        [FindPackageShare("lbr_bringup"), "config/moveit_servo.yaml"]
+    )
+    model = LaunchConfiguration("model").perform(context)
+    moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
+        robot_name=model,
+        package_name=f"{model}_moveit_config",
+    ).to_moveit_configs()
+
+    mode = LaunchConfiguration("mode").perform(context)
+    use_sim_time = False
+    if mode == "gazebo":
+        use_sim_time = True
+
+    # moveit servo node
+    servo_node = LBRMoveItServoMixin.node_moveit_servo(
+        parameters=[
+            moveit_configs.robot_description_kinematics,
+            moveit_configs.robot_description_semantic,
+            {"use_sim_time": use_sim_time},
+            moveit_servo_config,
+            {
+                "moveit_servo.use_gazebo": mode
+                == "gazebo",  # we configure this parameter dynamically
+            },
+        ],
+    )
+    ld.add_action(servo_node)
+
+    # call start servo after servo node start
+    ld.add_action(
+        RegisterEventHandler(
+            OnProcessStart(
+                target_action=servo_node,
+                on_start=[
+                    LBRMoveItServoMixin.call_start_servo_service(
+                        condition=IfCondition(
+                            LaunchConfiguration("default_enable_servo")
+                        )
+                    )
+                ],
+            ),
+        )
+    )
+
+    return ld.entities
+
+
+def generate_launch_description() -> LaunchDescription:
+    ld = LaunchDescription()
+
+    ld.add_action(LBRDescriptionMixin.arg_mode())
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRMoveItServoMixin.arg_default_enable_servo())
+
+    ld.add_action(OpaqueFunction(function=hidden_setup))
+    return ld

lbr_bringup/lbr_bringup/move_group.py → lbr_bringup/lbr_bringup/moveit.py

@@ -1,17 +1,17 @@
 import os
-from typing import Any, Dict, List
+from typing import Any, Dict, List, Optional, Union
 
 from ament_index_python import get_package_share_directory
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.substitutions import (
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from moveit_configs_utils import MoveItConfigs, MoveItConfigsBuilder
 
-# NOTE TO SELF:
-# due to individual moveit configs, put mixins into lbr_bringup rather than lbr_moveit_config
-# most of the configs are taken from Python package moveit_configs_utils.launches
-
 
 class LBRMoveGroupMixin:
     @staticmethod
@@ -118,3 +118,46 @@ def node_move_group(**kwargs) -> Node:
             output="screen",
             **kwargs,
         )
+
+
+class LBRMoveItServoMixin:
+    @staticmethod
+    def arg_default_enable_servo() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="default_enable_servo",
+            default_value="true",
+            description="Whether to enable the servo node by default.",
+        )
+
+    @staticmethod
+    def node_moveit_servo(
+        robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "robot_name", default="lbr"
+        ),
+        **kwargs,
+    ) -> Node:
+        return Node(
+            package="moveit_servo",
+            executable="servo_node_main",
+            output="screen",
+            namespace=robot_name,
+            **kwargs,
+        )
+
+    @staticmethod
+    def call_start_servo_service(
+        robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "robot_name", default="lbr"
+        ),
+        **kwargs,
+    ) -> ExecuteProcess:
+        return ExecuteProcess(
+            cmd=[
+                FindExecutable(name="ros2"),
+                "service",
+                "call",
+                PathJoinSubstitution([robot_name, "servo_node/start_servo"]),
+                "std_srvs/srv/Trigger",
+            ],
+            **kwargs,
+        )

lbr_bringup/package.xml

@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_bringup</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>LBR launch files.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
-  <license>Apache License 2.0</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
@@ -17,6 +17,8 @@
   <exec_depend>lbr_description</exec_depend>
   <exec_depend>lbr_fri_ros2</exec_depend>
   <exec_depend>lbr_ros2_control</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_servo</exec_depend>
   <exec_depend>rclpy</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros_gz_sim</exec_depend>

lbr_demos/doc/lbr_demos.rst

@@ -29,8 +29,8 @@ MoveIt
 .. toctree::
     :titlesonly:
 
-    lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst>
-    lbr_moveit2_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst>
+    lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst>
+    lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst>
 
 Integration
 -----------

lbr_demos/lbr_demos_advanced_cpp/package.xml

@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_advanced_cpp</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>Advanced C++ demos for the lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
-  <license>Apache License 2.0</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

lbr_demos/lbr_demos_advanced_py/package.xml

@@ -2,11 +2,11 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_advanced_py</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>Advanced Python demos for the lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <maintainer email="[email protected]">cmower</maintainer>
-  <license>Apache License 2.0</license>
+  <license>Apache-2.0</license>
 
   <exec_depend>lbr_description</exec_depend>
   <exec_depend>lbr_fri_idl</exec_depend>

lbr_demos/lbr_demos_advanced_py/setup.py

@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="2.1.1",
+    version="2.1.2",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),

lbr_demos/lbr_demos_cpp/package.xml

@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_cpp</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>C++ demos for lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
-  <license>Apache License 2.0</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

lbr_demos/lbr_demos_py/package.xml

@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_py</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>Python demos for lbr_ros2_control.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
-  <license>Apache License 2.0</license>
+  <license>Apache-2.0</license>
 
   <exec_depend>control_msgs</exec_depend>
   <exec_depend>lbr_bringup</exec_depend>

lbr_demos/lbr_demos_py/setup.py

@@ -4,7 +4,7 @@
 
 setup(
     name=package_name,
-    version="2.1.1",
+    version="2.1.2",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),