From e9eaaba0ed1b98307f2182d766ae3e493b87b91b Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 17 Oct 2024 23:56:44 +0200 Subject: [PATCH] added moveit servo support (#50, #211) --- lbr_bringup/config/moveit_servo.yaml | 72 +++++++++++++++++++ lbr_bringup/launch/move_group.launch.py | 2 +- lbr_bringup/launch/moveit_servo.launch.py | 72 +++++++++++++++++++ .../lbr_bringup/{move_group.py => moveit.py} | 57 +++++++++++++-- lbr_bringup/package.xml | 2 + 5 files changed, 197 insertions(+), 8 deletions(-) create mode 100644 lbr_bringup/config/moveit_servo.yaml rename lbr_bringup/lbr_bringup/{move_group.py => moveit.py} (72%) 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