43#include <rclcpp/rclcpp.hpp>
47#pragma GCC diagnostic ignored "-Wold-style-cast"
51constexpr double ROBOT_STATE_WAIT_TIME = 5.0;
52constexpr double STOPPED_VELOCITY_EPS = 1e-4;
58Servo::Servo(
const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::ParamListener> servo_param_listener,
61 , logger_(
moveit::getLogger(
"moveit.ros.servo"))
62 , servo_param_listener_{ std::move(servo_param_listener) }
65 servo_params_ = servo_param_listener_->get_params();
67 if (!validateParams(servo_params_))
69 RCLCPP_ERROR_STREAM(logger_,
"Got invalid parameters, exiting.");
70 std::exit(EXIT_FAILURE);
73 if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(servo_params_.move_group_name,
74 ROBOT_STATE_WAIT_TIME))
76 RCLCPP_ERROR(logger_,
"Timeout waiting for current state");
77 std::exit(EXIT_FAILURE);
80 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
84 std::make_unique<CollisionMonitor>(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_));
85 collision_monitor_->start();
89 const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel()
90 ->getJointModelGroup(servo_params_.move_group_name)
91 ->getActiveJointModelNames();
93 for (
const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames())
96 if (sub_group_name == servo_params_.move_group_name)
100 const auto& subgroup_joint_names =
101 planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames();
105 for (
const auto& joint_name : subgroup_joint_names)
108 const auto move_group_iterator =
109 std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name);
111 new_map.insert(std::make_pair<std::string, std::size_t>(
112 std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator)));
115 joint_name_to_index_maps_.insert(
116 std::make_pair<std::string, JointNameToMoveGroupIndexMap>(std::string(sub_group_name), std::move(new_map)));
120 if (servo_params_.use_smoothing)
122 setSmoothingPlugin();
126 RCLCPP_WARN(logger_,
"No smoothing plugin loaded");
129 RCLCPP_INFO_STREAM(logger_,
"Servo initialized successfully");
137void Servo::setSmoothingPlugin()
142 pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass> smoothing_loader(
143 "moveit_core",
"online_signal_smoothing::SmoothingBaseClass");
144 smoother_ = smoothing_loader.createUniqueInstance(servo_params_.smoothing_filter_plugin_name);
146 catch (pluginlib::PluginlibException& ex)
148 RCLCPP_ERROR(logger_,
"Exception while loading the smoothing plugin '%s': '%s'",
149 servo_params_.smoothing_filter_plugin_name.c_str(), ex.what());
150 std::exit(EXIT_FAILURE);
154 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
155 const int num_joints =
156 robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
157 if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints))
159 RCLCPP_ERROR(logger_,
"Smoothing plugin could not be initialized");
160 std::exit(EXIT_FAILURE);
182 check_collision ? collision_monitor_->start() : collision_monitor_->stop();
185bool Servo::validateParams(
const servo::Params& servo_params)
187 bool params_valid =
true;
188 auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
189 auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
190 const std::string check_yaml_string =
" Check the parameters YAML file used to launch this node.";
191 if (joint_model_group ==
nullptr)
193 RCLCPP_ERROR_STREAM(logger_,
"The parameter 'move_group_name': `" << servo_params.move_group_name <<
'`'
194 <<
" is not valid." << check_yaml_string);
195 params_valid =
false;
198 if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold)
200 RCLCPP_ERROR_STREAM(logger_,
"The parameter 'hard_stop_singularity_threshold' "
201 "should be greater than the parameter 'lower_singularity_threshold'. But the "
202 "'hard_stop_singularity_threshold' is: '"
203 << servo_params.hard_stop_singularity_threshold
204 <<
"' and the 'lower_singularity_threshold' is: '"
205 << servo_params.lower_singularity_threshold <<
"'" << check_yaml_string);
206 params_valid =
false;
209 if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities &&
210 !servo_params.publish_joint_accelerations)
212 RCLCPP_ERROR_STREAM(logger_,
"At least one of the parameters: 'publish_joint_positions' / "
213 "'publish_joint_velocities' / "
214 "'publish_joint_accelerations' must be true. But they are all false."
215 << check_yaml_string);
216 params_valid =
false;
219 if ((servo_params.command_out_type ==
"std_msgs/Float64MultiArray") && servo_params.publish_joint_positions &&
220 servo_params.publish_joint_velocities)
223 logger_,
"When publishing a std_msgs/Float64MultiArray, "
224 "either the parameter 'publish_joint_positions' OR the parameter 'publish_joint_velocities' must "
225 "be set to true. But both are set to true."
226 << check_yaml_string);
227 params_valid =
false;
230 if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold)
232 RCLCPP_ERROR_STREAM(logger_,
"The parameter 'self_collision_proximity_threshold' should probably be less "
233 "than or equal to the parameter 'scene_collision_proximity_threshold'. But "
234 "'self_collision_proximity_threshold' is: '"
235 << servo_params.self_collision_proximity_threshold
236 <<
"' and 'scene_collision_proximity_threshold' is: '"
237 << servo_params.scene_collision_proximity_threshold <<
"'" << check_yaml_string);
238 params_valid =
false;
241 if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name &&
242 !joint_model_group->isSubgroup(servo_params.active_subgroup))
244 RCLCPP_ERROR_STREAM(logger_,
"The parameter 'active_subgroup': '"
245 << servo_params.active_subgroup
246 <<
"' does not name a valid subgroup of 'joint group': '"
247 << servo_params.move_group_name <<
"'" << check_yaml_string);
248 params_valid =
false;
251 const auto num_dofs = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount();
252 if (servo_params.joint_limit_margins.size() == 1u)
254 joint_limit_margins_.clear();
255 for (
size_t idx = 0; idx < num_dofs; ++idx)
257 joint_limit_margins_.push_back(servo_params.joint_limit_margins[0]);
260 else if (servo_params.joint_limit_margins.size() == num_dofs)
262 joint_limit_margins_ = servo_params.joint_limit_margins;
267 logger_,
"The parameter 'joint_limit_margins' must have either a single element or the same number of "
268 "elements as the degrees of freedom in the active joint group. The size of 'joint_limit_margins' is '"
269 << servo_params.joint_limit_margins.size() <<
"' but the number of degrees of freedom in group '"
270 << servo_params.move_group_name <<
"' is '" << num_dofs <<
"'." << check_yaml_string);
271 params_valid =
false;
278 "The publish period (%f sec) parameter must be less than 1/%d of the max expected latency parameter (%f sec).",
280 params_valid =
false;
286bool Servo::updateParams()
288 bool params_updated =
false;
289 if (servo_param_listener_->is_old(servo_params_))
291 const auto params = servo_param_listener_->get_params();
293 if (validateParams(params))
295 if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor)
297 RCLCPP_INFO_STREAM(logger_,
"override_velocity_scaling_factor changed to : "
298 << std::to_string(params.override_velocity_scaling_factor));
301 servo_params_ = params;
302 params_updated =
true;
306 RCLCPP_WARN_STREAM(logger_,
"Parameters will not be updated.");
309 return params_updated;
314 return servo_params_;
319 return servo_status_;
329 return expected_command_type_;
334 expected_command_type_ = command_type;
337KinematicState Servo::haltJoints(
const std::vector<size_t>& joint_variables_to_halt,
341 bounded_state.joint_names = target_state.
joint_names;
343 std::stringstream halting_joint_names;
344 for (
const auto idx : joint_variables_to_halt)
346 halting_joint_names << bounded_state.joint_names[idx] +
" ";
348 RCLCPP_WARN_STREAM(logger_,
"Joint position limit reached on joints: " << halting_joint_names.str());
350 const bool all_joint_halt =
357 bounded_state.positions = current_state.
positions;
362 bounded_state.positions = target_state.
positions;
363 bounded_state.velocities = target_state.
velocities;
364 for (
const auto idx : joint_variables_to_halt)
366 bounded_state.positions[idx] = current_state.
positions[idx];
367 bounded_state.velocities[idx] = 0.0;
371 return bounded_state;
374Eigen::VectorXd Servo::jointDeltaFromCommand(
const ServoInput& command,
const moveit::core::RobotStatePtr& robot_state)
377 const auto& active_subgroup_name =
378 servo_params_.active_subgroup.empty() ? servo_params_.move_group_name : servo_params_.active_subgroup;
379 const auto& joint_name_group_index_map = (active_subgroup_name != servo_params_.move_group_name) ?
380 joint_name_to_index_maps_.at(servo_params_.active_subgroup) :
383 const int num_joints =
384 robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
385 Eigen::VectorXd joint_position_deltas(num_joints);
386 joint_position_deltas.setZero();
391 if (command.index() ==
static_cast<size_t>(expected_type))
396 joint_name_group_index_map);
397 servo_status_ = delta_result.first;
404 if (planning_frame_maybe.has_value())
406 const auto& planning_frame = *planning_frame_maybe;
407 const auto command_in_planning_frame_maybe = toPlanningFrame(std::get<TwistCommand>(command), planning_frame);
408 if (command_in_planning_frame_maybe.has_value())
410 delta_result =
jointDeltaFromTwist(*command_in_planning_frame_maybe, robot_state, servo_params_,
411 planning_frame, joint_name_group_index_map);
412 servo_status_ = delta_result.first;
417 RCLCPP_ERROR_STREAM(logger_,
"Could not transform twist command to planning frame.");
423 RCLCPP_ERROR(logger_,
"No IK solver for planning group %s.", active_subgroup_name.c_str());
433 if (planning_frame_maybe.has_value() && ee_frame_maybe.has_value())
435 const auto& planning_frame = *planning_frame_maybe;
436 const auto command_in_planning_frame_maybe = toPlanningFrame(std::get<PoseCommand>(command), planning_frame);
437 if (command_in_planning_frame_maybe.has_value())
439 delta_result =
jointDeltaFromPose(*command_in_planning_frame_maybe, robot_state, servo_params_,
440 planning_frame, *ee_frame_maybe, joint_name_group_index_map);
441 servo_status_ = delta_result.first;
446 RCLCPP_ERROR_STREAM(logger_,
"Could not transform pose command to planning frame.");
452 RCLCPP_ERROR(logger_,
"No IK solver for planning group %s.", active_subgroup_name.c_str());
458 joint_position_deltas = delta_result.second;
464 RCLCPP_WARN_STREAM(logger_,
"Incoming servo command type does not match known command types.");
467 return joint_position_deltas;
480 robot_state->getJointModelGroup(servo_params_.move_group_name);
485 const int num_joints = joint_names.size();
493 Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command, robot_state);
495 if (collision_velocity_scale_ > 0 && collision_velocity_scale_ < 1)
499 else if (collision_velocity_scale_ == 0)
512 target_state.
velocities = joint_position_delta / servo_params_.publish_period;
516 target_state.
velocities, joint_bounds, servo_params_.override_velocity_scaling_factor);
517 if (joint_velocity_limit_scale < 1.0)
519 RCLCPP_DEBUG_STREAM(logger_,
"Joint velocity limit scaling applied by a factor of " << joint_velocity_limit_scale);
521 target_state.
velocities *= joint_velocity_limit_scale;
534 const std::vector<size_t> joint_variables_to_halt =
538 if (!joint_variables_to_halt.empty())
541 target_state = haltJoints(joint_variables_to_halt, current_state, target_state);
551std::optional<Eigen::Isometry3d> Servo::getPlanningToCommandFrameTransform(
const std::string& command_frame,
552 const std::string& planning_frame)
const
554 const moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
555 if (robot_state->knowsFrameTransform(command_frame) && (robot_state->knowsFrameTransform(planning_frame)))
557 return robot_state->getGlobalLinkTransform(planning_frame).inverse() *
558 robot_state->getGlobalLinkTransform(command_frame);
564 return tf2::transformToEigen(
565 planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0)));
567 catch (tf2::TransformException& ex)
569 RCLCPP_ERROR(logger_,
"Failed to get planning to command frame transform: %s", ex.what());
575std::optional<TwistCommand> Servo::toPlanningFrame(
const TwistCommand& command,
const std::string& planning_frame)
const
577 Eigen::VectorXd transformed_twist = command.velocities;
579 if (command.frame_id != planning_frame)
582 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
583 if (!planning_to_command_tf_maybe.has_value())
587 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
589 if (servo_params_.apply_twist_commands_about_ee_frame)
592 const auto planning_to_command_rotation = planning_to_command_tf.linear();
593 const Eigen::Vector3d translation_vector =
594 planning_to_command_rotation *
595 Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
596 const Eigen::Vector3d angular_vector =
597 planning_to_command_rotation *
598 Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);
601 transformed_twist.head<3>() = translation_vector;
602 transformed_twist.tail<3>() = angular_vector;
610 Eigen::MatrixXd adjoint(6, 6);
612 const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
613 const Eigen::Vector3d& translation = planning_to_command_tf.translation();
615 Eigen::Matrix3d skew_translation;
616 skew_translation.row(0) << 0, -translation(2), translation(1);
617 skew_translation.row(1) << translation(2), 0, -translation(0);
618 skew_translation.row(2) << -translation(1), translation(0), 0;
620 adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
621 adjoint.topRightCorner(3, 3) = rotation;
622 adjoint.bottomLeftCorner(3, 3) = rotation;
623 adjoint.bottomRightCorner(3, 3).setZero();
625 transformed_twist = adjoint * transformed_twist;
629 return TwistCommand{ planning_frame, transformed_twist };
632std::optional<PoseCommand> Servo::toPlanningFrame(
const PoseCommand& command,
const std::string& planning_frame)
const
634 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
635 if (!planning_to_command_tf_maybe)
640 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
641 return PoseCommand{ planning_frame, planning_to_command_tf * command.pose };
646 if (block_for_current_state)
648 bool have_current_state =
false;
649 while (rclcpp::ok() && !have_current_state)
651 have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
652 rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME );
653 if (!have_current_state)
655 RCLCPP_WARN(logger_,
"Waiting for the current state");
659 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
665 auto target_state = halt_state;
668 bool stopped = (target_state.
velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
676 target_state.
velocities *= collision_velocity_scale_;
678 for (
long i = 0; i < halt_state.
positions.size(); ++i)
681 const double vel = target_state.
velocities[i];
682 target_state.
velocities[i] = (std::abs(vel) > STOPPED_VELOCITY_EPS) ? vel : 0.0;
691 return std::make_pair(stopped, target_state);
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
void setCommandType(const CommandType &command_type)
Set the type of incoming servo command.
CommandType getCommandType() const
Get the type of command that servo is currently expecting.
KinematicState getCurrentRobotState(bool block_for_current_state) const
Get the current state of the robot as given by planning scene monitor. This may block if a current ro...
std::string getStatusMessage() const
Get the message associated with the current servo status.
std::pair< bool, KinematicState > smoothHalt(const KinematicState &halt_state)
Smoothly halt at a commanded state when command goes stale.
StatusCode getStatus() const
Get the current status of servo.
servo::Params & getParams()
Returns the most recent servo parameters.
void resetSmoothing(const KinematicState &state)
Resets the smoothing plugin, if set, to a specified state.
Servo(const rclcpp::Node::SharedPtr &node, std::shared_ptr< const servo::ParamListener > servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor)
void setCollisionChecking(const bool check_collision)
Start/Stop collision checking thread.
KinematicState getNextJointState(const moveit::core::RobotStatePtr &robot_state, const ServoInput &command)
Computes the joint state required to follow the given command.
void doSmoothing(KinematicState &state)
Applies smoothing to an input state, if a smoothing plugin is set.
std::vector< const JointModel::Bounds * > JointBoundsVector
JointDeltaResult jointDeltaFromTwist(const TwistCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
Compute the change in joint position for the given twist command.
const std::unordered_map< StatusCode, std::string > SERVO_STATUS_CODE_MAP({ { StatusCode::INVALID, "Invalid" }, { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } })
std::optional< std::string > getIKSolverTipFrame(const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.
JointDeltaResult jointDeltaFromJointJog(const JointJogCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
Compute the change in joint position for the given joint jog command.
std::optional< std::string > getIKSolverBaseFrame(const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
Get the base frame of the current active joint group or subgroup's IK solver.
std::variant< JointJogCommand, TwistCommand, PoseCommand > ServoInput
@ DECELERATE_FOR_COLLISION
double jointLimitVelocityScalingFactor(const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override)
Apply velocity scaling based on joint limits. If the robot model does not have velocity limits define...
std::pair< StatusCode, Eigen::VectorXd > JointDeltaResult
constexpr int MIN_POINTS_FOR_TRAJ_MSG
JointDeltaResult jointDeltaFromPose(const PoseCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const std::string &ee_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
Compute the change in joint position for the given pose command.
std::unordered_map< std::string, std::size_t > JointNameToMoveGroupIndexMap
std::vector< size_t > jointVariablesToHalt(const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, const std::vector< double > &margins)
Finds the joint variable indices corresponding to joints exceeding allowable position limits.
KinematicState extractRobotState(const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
Extract the state from a RobotStatePtr instance.
Main namespace for MoveIt.
Eigen::VectorXd positions
std::vector< std::string > joint_names
Eigen::VectorXd accelerations
Eigen::VectorXd velocities