From 441d35d80dd3d632f23f6bcbc056b1169040b191 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 27 Sep 2024 03:21:26 -0400 Subject: [PATCH] Fix Servo pose tracking tutorial (#973) --- .../config/panda_simulated_config.yaml | 45 ++++++++++--------- .../launch/pose_tracking_tutorial.launch.py | 17 ++++--- .../realtime_servo_tutorial.rst | 30 ++++++++++--- .../src/pose_tracking_tutorial.cpp | 15 ++++--- 4 files changed, 69 insertions(+), 38 deletions(-) diff --git a/doc/examples/realtime_servo/config/panda_simulated_config.yaml b/doc/examples/realtime_servo/config/panda_simulated_config.yaml index 35e8eedd25..2b6c0f5b51 100644 --- a/doc/examples/realtime_servo/config/panda_simulated_config.yaml +++ b/doc/examples/realtime_servo/config/panda_simulated_config.yaml @@ -6,13 +6,14 @@ # 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] +publish_period: 0.01 # 1/Nominal publish rate [seconds] +max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds] -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +command_in_type: "speed_units" # "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. + linear: 0.2 # 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 @@ -27,36 +28,36 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands use_smoothing: true -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin" # 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 +check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) ## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' - -## Other frames -ee_frame: panda_hand # The name of the end effector link, used to return the EE pose +move_group_name: panda_arm # Often 'manipulator' or 'arm' ## 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. +lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) +# Added as a buffer to joint variable position bounds [in that joint variable's respective units]. +# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. +# If moving quickly, make these values larger. +joint_limit_margins: [0.12] ## 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: /joint_states -status_topic: ~/status # Publish status to this topic -command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states # Get joint states from this tpoic +status_topic: ~/status # Publish status to this topic +command_out_topic: /panda_arm_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] +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/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py index b505908ca1..3bf2581a9d 100644 --- a/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py +++ b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py @@ -12,6 +12,8 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") + .joint_limits(file_path="config/hard_joint_limits.yaml") + .robot_description_kinematics() .to_moveit_configs() ) @@ -27,8 +29,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( @@ -96,10 +99,12 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], condition=UnlessCondition(launch_as_standalone_node), ), @@ -126,10 +131,12 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], output="screen", condition=IfCondition(launch_as_standalone_node), @@ -150,6 +157,6 @@ def generate_launch_description(): panda_arm_controller_spawner, servo_node, container, - launch.actions.TimerAction(period=10.0, actions=[demo_node]), + launch.actions.TimerAction(period=8.0, actions=[demo_node]), ] ) diff --git a/doc/examples/realtime_servo/realtime_servo_tutorial.rst b/doc/examples/realtime_servo/realtime_servo_tutorial.rst index 65069eb1bf..7428b5a807 100644 --- a/doc/examples/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/examples/realtime_servo/realtime_servo_tutorial.rst @@ -200,9 +200,11 @@ The user can use status for higher-level decision making. See :moveit_codedir:`moveit_servo/demos ` for complete examples of using the C++ interface. The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch `. - - ``ros2 launch moveit_servo demo_joint_jog.launch.py`` - - ``ros2 launch moveit_servo demo_twist.launch.py`` - - ``ros2 launch moveit_servo demo_pose.launch.py`` +.. code-block:: bash + + ros2 launch moveit_servo demo_joint_jog.launch.py + ros2 launch moveit_servo demo_twist.launch.py + ros2 launch moveit_servo demo_pose.launch.py Using the ROS API @@ -222,7 +224,11 @@ selected using the *command_out_type* parameter, and published on the topic spec The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType `. -From cli : ``ros2 service call //switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"`` +From the CLI: + +.. code-block:: bash + + ros2 service call //switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}" Programmatically: @@ -248,10 +254,22 @@ Similarly, servoing can be paused using the pause service ``/pause_se When using the ROS interface, the status of Servo is available on the topic ``//status``, see definition :moveit_msgs_codedir:`ServoStatus `. -Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``. +Launch ROS interface demo: + +.. code-block:: bash + + ros2 launch moveit_servo demo_ros_api.launch.py Once the demo is running, the robot can be teleoperated through the keyboard. -Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``. +Launch the keyboard demo: + +.. code-block:: bash + + ros2 run moveit_servo servo_keyboard_input An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example `. + +.. code-block:: bash + + ros2 launch moveit2_tutorials pose_tracking_tutorial.launch.py diff --git a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp index a32bf45cb4..d0692e9ced 100644 --- a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp +++ b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp @@ -208,7 +208,7 @@ int main(int argc, char* argv[]) executor->add_node(node); // Spin the node. - std::thread executor_thread([&]() { executor->spin(); }); + std::thread executor_thread([&executor]() { executor->spin(); }); visualization_msgs::msg::MarkerArray marray; std::vector path = getPath(); @@ -231,20 +231,25 @@ int main(int argc, char* argv[]) // Create the ROS message. auto request = std::make_shared(); request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE; - auto response = switch_input_client->async_send_request(request); + auto response_future = switch_input_client->async_send_request(request); + if (response_future.wait_for(std::chrono::duration(3.0)) == std::future_status::timeout) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Timed out waiting for MoveIt servo command switching request."); + } + const auto response = response_future.get(); if (response.get()->success) { - RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Pose"); + RCLCPP_INFO_STREAM(node->get_logger(), "Switched to command input type: Pose"); } else { - RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Pose"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not switch MoveIt servo command input type."); } // Follow the trajectory const double publish_period = 0.15; - rclcpp::WallRate rate(1 / publish_period); + rclcpp::WallRate rate(1.0 / publish_period); // The path needs to be reversed since the last point in the path is where we want to start. std::reverse(path.begin(), path.end());