moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
servo.cpp
Go to the documentation of this file.
1/*******************************************************************************
2 * BSD 3-Clause License
3 *
4 * Copyright (c) 2019, Los Alamos National Security, LLC
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions are met:
9 *
10 * * Redistributions of source code must retain the above copyright notice, this
11 * list of conditions and the following disclaimer.
12 *
13 * * Redistributions in binary form must reproduce the above copyright notice,
14 * this list of conditions and the following disclaimer in the documentation
15 * and/or other materials provided with the distribution.
16 *
17 * * Neither the name of the copyright holder nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE
25 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 *******************************************************************************/
33
34/* Title : servo.cpp
35 * Project : moveit_servo
36 * Created : 05/17/2023
37 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim
38 */
39
43#include <rclcpp/rclcpp.hpp>
45
46// Disable -Wold-style-cast because all _THROTTLE macros trigger this
47#pragma GCC diagnostic ignored "-Wold-style-cast"
48
49namespace
50{
51constexpr double ROBOT_STATE_WAIT_TIME = 5.0; // seconds
52constexpr double STOPPED_VELOCITY_EPS = 1e-4;
53} // namespace
54
55namespace moveit_servo
56{
57
58Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::ParamListener> servo_param_listener,
59 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
60 : node_(node)
61 , logger_(moveit::getLogger("moveit.ros.servo"))
62 , servo_param_listener_{ std::move(servo_param_listener) }
63 , planning_scene_monitor_{ planning_scene_monitor }
64{
65 servo_params_ = servo_param_listener_->get_params();
66
67 if (!validateParams(servo_params_))
68 {
69 RCLCPP_ERROR_STREAM(logger_, "Got invalid parameters, exiting.");
70 std::exit(EXIT_FAILURE);
71 }
72
73 if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(servo_params_.move_group_name,
74 ROBOT_STATE_WAIT_TIME))
75 {
76 RCLCPP_ERROR(logger_, "Timeout waiting for current state");
77 std::exit(EXIT_FAILURE);
78 }
79
80 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
81
82 // Create the collision checker and start collision checking.
83 collision_monitor_ =
84 std::make_unique<CollisionMonitor>(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_));
85 collision_monitor_->start();
86
87 servo_status_ = StatusCode::NO_WARNING;
88
89 const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel()
90 ->getJointModelGroup(servo_params_.move_group_name)
91 ->getActiveJointModelNames();
92 // Create subgroup map
93 for (const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames())
94 {
95 // Skip move group
96 if (sub_group_name == servo_params_.move_group_name)
97 {
98 continue;
99 }
100 const auto& subgroup_joint_names =
101 planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames();
102
104 // For each joint name of the subgroup calculate the index in the move group joint vector
105 for (const auto& joint_name : subgroup_joint_names)
106 {
107 // Find subgroup joint name in move group joint names
108 const auto move_group_iterator =
109 std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name);
110 // Calculate position and add a new mapping of joint name to move group joint vector position
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)));
113 }
114 // Add new joint name to index map to existing maps
115 joint_name_to_index_maps_.insert(
116 std::make_pair<std::string, JointNameToMoveGroupIndexMap>(std::string(sub_group_name), std::move(new_map)));
117 }
118
119 // Load the smoothing plugin
120 if (servo_params_.use_smoothing)
121 {
122 setSmoothingPlugin();
123 }
124 else
125 {
126 RCLCPP_WARN(logger_, "No smoothing plugin loaded");
127 }
128
129 RCLCPP_INFO_STREAM(logger_, "Servo initialized successfully");
130}
131
133{
135}
136
137void Servo::setSmoothingPlugin()
138{
139 // Load the smoothing plugin
140 try
141 {
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);
145 }
146 catch (pluginlib::PluginlibException& ex)
147 {
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);
151 }
152
153 // Initialize the smoothing plugin
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))
158 {
159 RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized");
160 std::exit(EXIT_FAILURE);
161 }
162}
163
165{
166 if (smoother_)
167 {
168 smoother_->doSmoothing(state.positions, state.velocities, state.accelerations);
169 }
170}
171
173{
174 if (smoother_)
175 {
176 smoother_->reset(state.positions, state.velocities, state.accelerations);
177 }
178}
179
180void Servo::setCollisionChecking(const bool check_collision)
181{
182 check_collision ? collision_monitor_->start() : collision_monitor_->stop();
183}
184
185bool Servo::validateParams(const servo::Params& servo_params)
186{
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)
192 {
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;
196 }
197
198 if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold)
199 {
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;
207 }
208
209 if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities &&
210 !servo_params.publish_joint_accelerations)
211 {
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;
217 }
218
219 if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions &&
220 servo_params.publish_joint_velocities)
221 {
222 RCLCPP_ERROR_STREAM(
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;
228 }
229
230 if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold)
231 {
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;
239 }
240
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))
243 {
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;
249 }
250
251 const auto num_dofs = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount();
252 if (servo_params.joint_limit_margins.size() == 1u)
253 {
254 joint_limit_margins_.clear();
255 for (size_t idx = 0; idx < num_dofs; ++idx)
256 {
257 joint_limit_margins_.push_back(servo_params.joint_limit_margins[0]);
258 }
259 }
260 else if (servo_params.joint_limit_margins.size() == num_dofs)
261 {
262 joint_limit_margins_ = servo_params.joint_limit_margins;
263 }
264 else
265 {
266 RCLCPP_ERROR_STREAM(
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;
272 }
273
274 if (servo_params.max_expected_latency / MIN_POINTS_FOR_TRAJ_MSG < servo_params.publish_period)
275 {
276 RCLCPP_ERROR(
277 logger_,
278 "The publish period (%f sec) parameter must be less than 1/%d of the max expected latency parameter (%f sec).",
279 servo_params.publish_period, MIN_POINTS_FOR_TRAJ_MSG, servo_params.max_expected_latency);
280 params_valid = false;
281 }
282
283 return params_valid;
284}
285
286bool Servo::updateParams()
287{
288 bool params_updated = false;
289 if (servo_param_listener_->is_old(servo_params_))
290 {
291 const auto params = servo_param_listener_->get_params();
292
293 if (validateParams(params))
294 {
295 if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor)
296 {
297 RCLCPP_INFO_STREAM(logger_, "override_velocity_scaling_factor changed to : "
298 << std::to_string(params.override_velocity_scaling_factor));
299 }
300
301 servo_params_ = params;
302 params_updated = true;
303 }
304 else
305 {
306 RCLCPP_WARN_STREAM(logger_, "Parameters will not be updated.");
307 }
308 }
309 return params_updated;
310}
311
312servo::Params& Servo::getParams()
313{
314 return servo_params_;
315}
316
318{
319 return servo_status_;
320}
321
322std::string Servo::getStatusMessage() const
323{
324 return SERVO_STATUS_CODE_MAP.at(servo_status_);
325}
326
328{
329 return expected_command_type_;
330}
331
332void Servo::setCommandType(const CommandType& command_type)
333{
334 expected_command_type_ = command_type;
335}
336
337KinematicState Servo::haltJoints(const std::vector<size_t>& joint_variables_to_halt,
338 const KinematicState& current_state, const KinematicState& target_state) const
339{
340 KinematicState bounded_state(target_state.joint_names.size());
341 bounded_state.joint_names = target_state.joint_names;
342
343 std::stringstream halting_joint_names;
344 for (const auto idx : joint_variables_to_halt)
345 {
346 halting_joint_names << bounded_state.joint_names[idx] + " ";
347 }
348 RCLCPP_WARN_STREAM(logger_, "Joint position limit reached on joints: " << halting_joint_names.str());
349
350 const bool all_joint_halt =
351 (getCommandType() == CommandType::JOINT_JOG && servo_params_.halt_all_joints_in_joint_mode) ||
352 (getCommandType() == CommandType::TWIST && servo_params_.halt_all_joints_in_cartesian_mode);
353
354 if (all_joint_halt)
355 {
356 // The velocities are initialized to zero by default, so we don't need to set it here.
357 bounded_state.positions = current_state.positions;
358 }
359 else
360 {
361 // Halt only the joints that are out of bounds
362 bounded_state.positions = target_state.positions;
363 bounded_state.velocities = target_state.velocities;
364 for (const auto idx : joint_variables_to_halt)
365 {
366 bounded_state.positions[idx] = current_state.positions[idx];
367 bounded_state.velocities[idx] = 0.0;
368 }
369 }
370
371 return bounded_state;
372}
373
374Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state)
375{
376 // Determine joint_name_group_index_map, if no subgroup is active, the map is empty
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) :
382
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();
387
388 JointDeltaResult delta_result;
389
390 const CommandType expected_type = getCommandType();
391 if (command.index() == static_cast<size_t>(expected_type))
392 {
393 if (expected_type == CommandType::JOINT_JOG)
394 {
395 delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_,
396 joint_name_group_index_map);
397 servo_status_ = delta_result.first;
398 }
399 else if (expected_type == CommandType::TWIST)
400 {
401 // Transform the twist command to the planning frame, which is the base frame of the active subgroup's IK solver,
402 // before applying it. Additionally verify there is an IK solver, and that the transformation is successful.
403 const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name);
404 if (planning_frame_maybe.has_value())
405 {
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())
409 {
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;
413 }
414 else
415 {
416 servo_status_ = StatusCode::INVALID;
417 RCLCPP_ERROR_STREAM(logger_, "Could not transform twist command to planning frame.");
418 }
419 }
420 else
421 {
422 servo_status_ = StatusCode::INVALID;
423 RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str());
424 }
425 }
426 else if (expected_type == CommandType::POSE)
427 {
428 // Transform the pose command to the planning frame, which is the base frame of the active subgroup's IK solver,
429 // before applying it. The end effector frame is also extracted as the tip frame of the IK solver.
430 // Additionally verify there is an IK solver, and that the transformation is successful.
431 const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name);
432 const auto ee_frame_maybe = getIKSolverTipFrame(robot_state, active_subgroup_name);
433 if (planning_frame_maybe.has_value() && ee_frame_maybe.has_value())
434 {
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())
438 {
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;
442 }
443 else
444 {
445 servo_status_ = StatusCode::INVALID;
446 RCLCPP_ERROR_STREAM(logger_, "Could not transform pose command to planning frame.");
447 }
448 }
449 else
450 {
451 servo_status_ = StatusCode::INVALID;
452 RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str());
453 }
454 }
455
456 if (servo_status_ != StatusCode::INVALID)
457 {
458 joint_position_deltas = delta_result.second;
459 }
460 }
461 else
462 {
463 servo_status_ = StatusCode::INVALID;
464 RCLCPP_WARN_STREAM(logger_, "Incoming servo command type does not match known command types.");
465 }
466
467 return joint_position_deltas;
468}
469
470KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot_state, const ServoInput& command)
471{
472 // Set status to clear
473 servo_status_ = StatusCode::NO_WARNING;
474
475 // Update the parameters
476 updateParams();
477
478 // Get the joint model group info.
479 const moveit::core::JointModelGroup* joint_model_group =
480 robot_state->getJointModelGroup(servo_params_.move_group_name);
481
482 // Get necessary information about joints
483 const std::vector<std::string> joint_names = joint_model_group->getActiveJointModelNames();
484 const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds();
485 const int num_joints = joint_names.size();
486
487 // Extract current state from robot state
488 KinematicState current_state = extractRobotState(robot_state, servo_params_.move_group_name);
489 KinematicState target_state(num_joints);
490 target_state.joint_names = joint_names;
491
492 // Compute the change in joint position due to the incoming command
493 Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command, robot_state);
494
495 if (collision_velocity_scale_ > 0 && collision_velocity_scale_ < 1)
496 {
498 }
499 else if (collision_velocity_scale_ == 0)
500 {
501 servo_status_ = StatusCode::HALT_FOR_COLLISION;
502 }
503
504 // Continue rest of the computations only if the command is valid
505 // The computations can be skipped also in case we are halting.
506 if (servo_status_ != StatusCode::INVALID && servo_status_ != StatusCode::HALT_FOR_COLLISION)
507 {
508 // Compute the next joint positions based on the joint position deltas
509 target_state.positions = current_state.positions + joint_position_delta;
510
511 // Compute the joint velocities required to reach positions
512 target_state.velocities = joint_position_delta / servo_params_.publish_period;
513
514 // Scale down the velocity based on joint velocity limit or user defined scaling if applicable.
515 const double joint_velocity_limit_scale = jointLimitVelocityScalingFactor(
516 target_state.velocities, joint_bounds, servo_params_.override_velocity_scaling_factor);
517 if (joint_velocity_limit_scale < 1.0) // 1.0 means no scaling.
518 {
519 RCLCPP_DEBUG_STREAM(logger_, "Joint velocity limit scaling applied by a factor of " << joint_velocity_limit_scale);
520 }
521 target_state.velocities *= joint_velocity_limit_scale;
522
523 // Adjust joint position based on scaled down velocity
524 target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
525
526 // Apply collision scaling to the joint position delta
527 target_state.positions =
528 current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions);
529
530 // Compute velocities based on smoothed joint positions
531 target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period;
532
533 // Check if any joints are going past joint position limits.
534 const std::vector<size_t> joint_variables_to_halt =
535 jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
536
537 // Apply halting if any joints need to be halted.
538 if (!joint_variables_to_halt.empty())
539 {
540 servo_status_ = StatusCode::JOINT_BOUND;
541 target_state = haltJoints(joint_variables_to_halt, current_state, target_state);
542 }
543 }
544
545 // Apply smoothing to the positions if a smoother was provided.
546 doSmoothing(target_state);
547
548 return target_state;
549}
550
551std::optional<Eigen::Isometry3d> Servo::getPlanningToCommandFrameTransform(const std::string& command_frame,
552 const std::string& planning_frame) const
553{
554 const moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
555 if (robot_state->knowsFrameTransform(command_frame) && (robot_state->knowsFrameTransform(planning_frame)))
556 {
557 return robot_state->getGlobalLinkTransform(planning_frame).inverse() *
558 robot_state->getGlobalLinkTransform(command_frame);
559 }
560 else
561 {
562 try
563 {
564 return tf2::transformToEigen(
565 planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0)));
566 }
567 catch (tf2::TransformException& ex)
568 {
569 RCLCPP_ERROR(logger_, "Failed to get planning to command frame transform: %s", ex.what());
570 return std::nullopt;
571 }
572 }
573}
574
575std::optional<TwistCommand> Servo::toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const
576{
577 Eigen::VectorXd transformed_twist = command.velocities;
578
579 if (command.frame_id != planning_frame)
580 {
581 // Look up the transform between the planning and command frames.
582 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
583 if (!planning_to_command_tf_maybe.has_value())
584 {
585 return std::nullopt;
586 }
587 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
588
589 if (servo_params_.apply_twist_commands_about_ee_frame)
590 {
591 // If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
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]);
599
600 // Update the values of the original command message to reflect the change in frame
601 transformed_twist.head<3>() = translation_vector;
602 transformed_twist.tail<3>() = angular_vector;
603 }
604 else
605 {
606 // If the twist command is applied about the planning frame, the spatial twist is calculated
607 // as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
608 // The above equation defines twist as [angular; linear], but in our convention it is
609 // [linear; angular] so the adjoint matrix is also reordered accordingly.
610 Eigen::MatrixXd adjoint(6, 6);
611
612 const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
613 const Eigen::Vector3d& translation = planning_to_command_tf.translation();
614
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;
619
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();
624
625 transformed_twist = adjoint * transformed_twist;
626 }
627 }
628
629 return TwistCommand{ planning_frame, transformed_twist };
630}
631
632std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const
633{
634 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
635 if (!planning_to_command_tf_maybe)
636 {
637 return std::nullopt;
638 }
639
640 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
641 return PoseCommand{ planning_frame, planning_to_command_tf * command.pose };
642}
643
644KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
645{
646 if (block_for_current_state)
647 {
648 bool have_current_state = false;
649 while (rclcpp::ok() && !have_current_state)
650 {
651 have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
652 rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
653 if (!have_current_state)
654 {
655 RCLCPP_WARN(logger_, "Waiting for the current state");
656 }
657 }
658 }
659 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
660 return extractRobotState(robot_state, servo_params_.move_group_name);
661}
662
663std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_state)
664{
665 auto target_state = halt_state;
666
667 // If all velocities are near zero, robot has decelerated to a stop.
668 bool stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
669
670 if (!stopped)
671 {
672 // set target velocity
673 target_state.velocities *= 0.0;
674
675 // scale velocity in case of obstacle
676 target_state.velocities *= collision_velocity_scale_;
677
678 for (long i = 0; i < halt_state.positions.size(); ++i)
679 {
680 target_state.positions[i] = halt_state.positions[i] + target_state.velocities[i] * servo_params_.publish_period;
681 const double vel = target_state.velocities[i];
682 target_state.velocities[i] = (std::abs(vel) > STOPPED_VELOCITY_EPS) ? vel : 0.0;
683 target_state.accelerations[i] =
684 (target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
685 }
686 }
687
688 // apply smoothing: this will change target position/velocity to make slow down gradual
689 doSmoothing(target_state);
690
691 return std::make_pair(stopped, target_state);
692}
693
694} // namespace moveit_servo
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.
Definition servo.cpp:332
CommandType getCommandType() const
Get the type of command that servo is currently expecting.
Definition servo.cpp:327
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...
Definition servo.cpp:644
std::string getStatusMessage() const
Get the message associated with the current servo status.
Definition servo.cpp:322
std::pair< bool, KinematicState > smoothHalt(const KinematicState &halt_state)
Smoothly halt at a commanded state when command goes stale.
Definition servo.cpp:663
StatusCode getStatus() const
Get the current status of servo.
Definition servo.cpp:317
servo::Params & getParams()
Returns the most recent servo parameters.
Definition servo.cpp:312
void resetSmoothing(const KinematicState &state)
Resets the smoothing plugin, if set, to a specified state.
Definition servo.cpp:172
Servo(const rclcpp::Node::SharedPtr &node, std::shared_ptr< const servo::ParamListener > servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor)
Definition servo.cpp:58
void setCollisionChecking(const bool check_collision)
Start/Stop collision checking thread.
Definition servo.cpp:180
KinematicState getNextJointState(const moveit::core::RobotStatePtr &robot_state, const ServoInput &command)
Computes the joint state required to follow the given command.
Definition servo.cpp:470
void doSmoothing(KinematicState &state)
Applies smoothing to an input state, if a smoothing plugin is set.
Definition servo.cpp:164
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.
Definition command.cpp:147
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.
Definition common.cpp:72
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.
Definition command.cpp:82
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.
Definition common.cpp:57
std::variant< JointJogCommand, TwistCommand, PoseCommand > ServoInput
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...
Definition common.cpp:380
std::pair< StatusCode, Eigen::VectorXd > JointDeltaResult
Definition datatypes.hpp:85
constexpr int MIN_POINTS_FOR_TRAJ_MSG
Definition common.hpp:58
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.
Definition command.cpp:230
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.
Definition common.cpp:411
KinematicState extractRobotState(const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
Extract the state from a RobotStatePtr instance.
Definition common.cpp:516
Main namespace for MoveIt.
Definition exceptions.h:43
std::vector< std::string > joint_names
Eigen::VectorXd accelerations