Skip to content

Adjusted the logic of the twist command of moveit_servo #3609

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Demonmasterlqx wants to merge 11 commits into
base: main
Choose a base branch
from
Open

Adjusted the logic of the twist command of moveit_servo #3609

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
e0376cc
Fix servo_node does not wait for planning scene monitor to receive fi…
Demonmasterlqx Nov 8, 2025
9965d9e
Remove the update flag in CurrentStateMonitor::jointStateCallback sin…
Demonmasterlqx Nov 8, 2025
de0c9f7
Merge remote-tracking branch 'origin/main'
Demonmasterlqx Nov 8, 2025
640f913
format code
Demonmasterlqx Nov 8, 2025
3d8a734
remove the repetitive waiting code in moveit_servo
Demonmasterlqx Nov 8, 2025
98a0239
fix twist command logict in moveit_servo
Demonmasterlqx Nov 8, 2025
284e510
Merge branch 'main' into main
Demonmasterlqx Nov 11, 2025
65df338
Merge branch 'moveit:main' into main
Demonmasterlqx Nov 24, 2025
9fecf09
Merge branch 'moveit:main' into main
Demonmasterlqx Nov 26, 2025
967f322
Merge branch 'moveit:main' into main
Demonmasterlqx Nov 27, 2025
09ba483
Merge branch 'moveit:main' into main
Demonmasterlqx Dec 24, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions moveit_ros/moveit_servo/config/servo_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,13 @@ servo:
must be passed to the node during launch time."
}

end_effector_frame: {
type: string,
default_value: "",
description: "The end effector frame for the active planning group. \
If not set, it will be overridden by command-frame of incoming commands"
}

active_subgroup: {
type: string,
default_value: "",
Expand Down
3 changes: 3 additions & 0 deletions moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,6 +223,9 @@ class Servo

// Variables

// The end effector frame for the active planning group.
std::string end_effector_frame_;

StatusCode servo_status_;
// This needs to be threadsafe so it can be updated in realtime.
std::atomic<CommandType> expected_command_type_;
Expand Down
98 changes: 55 additions & 43 deletions moveit_ros/moveit_servo/src/servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,8 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P

moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();

end_effector_frame_ = servo_params_.end_effector_frame;

// Create the collision checker and start collision checking.
collision_monitor_ =
std::make_unique<CollisionMonitor>(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_));
Expand Down Expand Up @@ -576,54 +578,64 @@ std::optional<TwistCommand> Servo::toPlanningFrame(const TwistCommand& command,
{
Eigen::VectorXd transformed_twist = command.velocities;

if (command.frame_id != planning_frame)
// Look up the transform between the planning and command frames.
const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
if (!planning_to_command_tf_maybe.has_value())
{
return std::nullopt;
}
const auto& planning_to_command_tf = *planning_to_command_tf_maybe;

if (servo_params_.apply_twist_commands_about_ee_frame)
{
// Look up the transform between the planning and command frames.
const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
if (!planning_to_command_tf_maybe.has_value())

const auto planning_to_ee_frame_maybe = getPlanningToCommandFrameTransform(
(end_effector_frame_.empty() ? command.frame_id : end_effector_frame_),
planning_frame);
if (!planning_to_ee_frame_maybe.has_value())
{
return std::nullopt;
}
const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
const auto& planning_to_ee_frame = *planning_to_ee_frame_maybe;
RCLCPP_DEBUG(logger_, "Applying twist about end effector frame: %s",
(end_effector_frame_.empty() ? command.frame_id.c_str() : end_effector_frame_.c_str()));

// If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
const auto planning_to_command_rotation = planning_to_command_tf.linear();
const auto planning_to_ee_frame_rotation = planning_to_ee_frame.linear();
const Eigen::Vector3d translation_vector =
planning_to_command_rotation *
Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
const Eigen::Vector3d angular_vector =
planning_to_ee_frame_rotation *
Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);

// Update the values of the original command message to reflect the change in frame
transformed_twist.head<3>() = translation_vector;
transformed_twist.tail<3>() = angular_vector;
}
else
{
// If the twist command is applied about the planning frame, the spatial twist is calculated
// as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
// The above equation defines twist as [angular; linear], but in our convention it is
// [linear; angular] so the adjoint matrix is also reordered accordingly.
Eigen::MatrixXd adjoint(6, 6);

if (servo_params_.apply_twist_commands_about_ee_frame)
{
// If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
const auto planning_to_command_rotation = planning_to_command_tf.linear();
const Eigen::Vector3d translation_vector =
planning_to_command_rotation *
Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
const Eigen::Vector3d angular_vector =
planning_to_command_rotation *
Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);

// Update the values of the original command message to reflect the change in frame
transformed_twist.head<3>() = translation_vector;
transformed_twist.tail<3>() = angular_vector;
}
else
{
// If the twist command is applied about the planning frame, the spatial twist is calculated
// as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
// The above equation defines twist as [angular; linear], but in our convention it is
// [linear; angular] so the adjoint matrix is also reordered accordingly.
Eigen::MatrixXd adjoint(6, 6);

const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
const Eigen::Vector3d& translation = planning_to_command_tf.translation();

Eigen::Matrix3d skew_translation;
skew_translation.row(0) << 0, -translation(2), translation(1);
skew_translation.row(1) << translation(2), 0, -translation(0);
skew_translation.row(2) << -translation(1), translation(0), 0;

adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
adjoint.topRightCorner(3, 3) = rotation;
adjoint.bottomLeftCorner(3, 3) = rotation;
adjoint.bottomRightCorner(3, 3).setZero();

transformed_twist = adjoint * transformed_twist;
}
const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
const Eigen::Vector3d& translation = planning_to_command_tf.translation();

Eigen::Matrix3d skew_translation;
skew_translation.row(0) << 0, -translation(2), translation(1);
skew_translation.row(1) << translation(2), 0, -translation(0);
skew_translation.row(2) << -translation(1), translation(0), 0;

adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
adjoint.topRightCorner(3, 3) = rotation;
adjoint.bottomLeftCorner(3, 3) = rotation;
adjoint.bottomRightCorner(3, 3).setZero();

transformed_twist = adjoint * transformed_twist;
}

return TwistCommand{ planning_frame, transformed_twist };
Expand Down
9 changes: 5 additions & 4 deletions moveit_ros/moveit_servo/src/servo_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -344,12 +344,13 @@ void ServoNode::servoLoop()
// while Servo uses RCL_ROS_TIME
while (servo_node_start >
convertClockType(planning_scene_monitor_->getLastUpdateTime(), servo_node_start.get_clock_type()))
{
RCLCPP_INFO(node_->get_logger(), "Waiting for planning scene monitor to receive robot state update.");
rclcpp::sleep_for(std::chrono::seconds(1));
}
{
RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
// Output the time of planning_scene_monitor_->getLastUpdateTime()
RCLCPP_INFO(node_->get_logger(), "Last planning scene update time: %f",
planning_scene_monitor_->getLastUpdateTime().seconds());
// Output the current time
RCLCPP_INFO(node_->get_logger(), "Current time: %f", node_->now().seconds());
rclcpp::sleep_for(std::chrono::seconds(1));
}
KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */);
Expand Down
30 changes: 29 additions & 1 deletion moveit_ros/moveit_servo/src/utils/common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,6 +197,20 @@ composeTrajectoryMessage(const servo::Params& servo_params, const std::deque<Kin
add_point(joint_trajectory, joint_cmd_rolling_window[i]);
}

// If it is not empty, add a point with a speed of 0 to ensure that the robot can stop

if (!joint_cmd_rolling_window.empty())
{
auto& last_state = joint_cmd_rolling_window.back();
KinematicState stop_state = last_state;
for (long i = 0; i < stop_state.velocities.size(); ++i)
{
stop_state.velocities[i] = 0.0;
}
stop_state.time_stamp += rclcpp::Duration(0, 100000); // add 0.1s to ensure stop
add_point(joint_trajectory, stop_state);
}

return joint_trajectory;
}

Expand Down Expand Up @@ -501,7 +515,21 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const
}
else
{
planning_scene_monitor->requestPlanningSceneState();
int cnt = 0;
while (!planning_scene_monitor->requestPlanningSceneState())
{
RCLCPP_WARN_THROTTLE(
node->get_logger(), *node->get_clock(), 5000,
"Waiting for planning scene monitor to get the planning scene from the primary planning scene monitor...");
rclcpp::sleep_for(std::chrono::milliseconds(100));
cnt++;
if (cnt > 120)
{
RCLCPP_ERROR(node->get_logger(),
"Failed to get planning scene from primary planning scene monitor after 12 seconds.");
break;
}
}
}

planning_scene_monitor->setPlanningScenePublishingFrequency(PLANNING_SCENE_PUBLISHING_FREQUENCY);
Expand Down
4 changes: 1 addition & 3 deletions moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -404,12 +404,10 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState:
}

// callbacks, if needed
if (update)
{
if (update){
for (JointStateUpdateCallback& update_callback : update_callbacks_)
update_callback(joint_state);
}

// notify waitForCurrentState() *after* potential update callbacks
state_update_condition_.notify_all();
}
Expand Down