Skip to content

Commit

Permalink
Fix Servo pose tracking tutorial (#973)
Browse files Browse the repository at this point in the history
  • Loading branch information
sea-bass authored Sep 27, 2024
1 parent 9e3bd6e commit 441d35d
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 38 deletions.
45 changes: 23 additions & 22 deletions doc/examples/realtime_servo/config/panda_simulated_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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]
Original file line number Diff line number Diff line change
Expand Up @@ -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()
)

Expand All @@ -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 = (
Expand Down Expand Up @@ -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),
),
Expand All @@ -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),
Expand All @@ -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]),
]
)
30 changes: 24 additions & 6 deletions doc/examples/realtime_servo/realtime_servo_tutorial.rst
Original file line number Diff line number Diff line change
Expand Up @@ -200,9 +200,11 @@ The user can use status for higher-level decision making.
See :moveit_codedir:`moveit_servo/demos <moveit_ros/moveit_servo/demos/cpp_interface>` for complete examples of using the C++ interface.
The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch <moveit_ros/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
Expand All @@ -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 <srv/ServoCommandType.srv>`.

From cli : ``ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"``
From the CLI:

.. code-block:: bash
ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"
Programmatically:

Expand All @@ -248,10 +254,22 @@ Similarly, servoing can be paused using the pause service ``<node_name>/pause_se

When using the ROS interface, the status of Servo is available on the topic ``/<node_name>/status``, see definition :moveit_msgs_codedir:`ServoStatus <msg/ServoStatus.msg>`.

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 <examples/realtime_servo/src/pose_tracking_tutorial.cpp>`.

.. code-block:: bash
ros2 launch moveit2_tutorials pose_tracking_tutorial.launch.py
15 changes: 10 additions & 5 deletions doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::Vector3d> path = getPath();
Expand All @@ -231,20 +231,25 @@ int main(int argc, char* argv[])
// Create the ROS message.
auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
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<double>(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());
Expand Down

0 comments on commit 441d35d

Please sign in to comment.