diff --git a/lbr_bringup/config/moveit_servo.yaml b/lbr_bringup/config/moveit_servo.yaml
new file mode 100644
index 00000000..da58d276
--- /dev/null
+++ b/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.034 # 1/Nominal publish 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: trajectory_msgs/JointTrajectory
+
+ # What to publish? Can save some bandwidth as most robots only require positions or velocities
+ publish_joint_positions: true
+ publish_joint_velocities: true
+ 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/joint_trajectory_controller/joint_trajectory # 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]
diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py
index 8a966915..7d71292a 100644
--- a/lbr_bringup/launch/move_group.launch.py
+++ b/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
diff --git a/lbr_bringup/launch/moveit_servo.launch.py b/lbr_bringup/launch/moveit_servo.launch.py
index e69de29b..400a7258 100644
--- a/lbr_bringup/launch/moveit_servo.launch.py
+++ b/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
diff --git a/lbr_bringup/lbr_bringup/move_group.py b/lbr_bringup/lbr_bringup/moveit.py
similarity index 72%
rename from lbr_bringup/lbr_bringup/move_group.py
rename to lbr_bringup/lbr_bringup/moveit.py
index ef0b4682..f2a69fbf 100644
--- a/lbr_bringup/lbr_bringup/move_group.py
+++ b/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,
+ )
diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml
index 16a8554b..5b351434 100644
--- a/lbr_bringup/package.xml
+++ b/lbr_bringup/package.xml
@@ -17,6 +17,8 @@
lbr_description
lbr_fri_ros2
lbr_ros2_control
+ moveit_ros_move_group
+ moveit_servo
rclpy
robot_state_publisher
ros_gz_sim