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 smoother_loader_ = std::make_unique<pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass>>(
143 "moveit_core", "online_signal_smoothing::SmoothingBaseClass");
144 smoother_ = smoother_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 || getCommandType() == CommandType::POSE) &&
353 servo_params_.halt_all_joints_in_cartesian_mode);
354
355 if (all_joint_halt)
356 {
357 // The velocities are initialized to zero by default, so we don't need to set it here.
358 bounded_state.positions = current_state.positions;
359 }
360 else
361 {
362 // Halt only the joints that are out of bounds
363 bounded_state.positions = target_state.positions;
364 bounded_state.velocities = target_state.velocities;
365 for (const auto idx : joint_variables_to_halt)
366 {
367 bounded_state.positions[idx] = current_state.positions[idx];
368 bounded_state.velocities[idx] = 0.0;
369 }
370 }
371
372 return bounded_state;
373}
374
375Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state)
376{
377 // Determine joint_name_group_index_map, if no subgroup is active, the map is empty
378 const auto& active_subgroup_name =
379 servo_params_.active_subgroup.empty() ? servo_params_.move_group_name : servo_params_.active_subgroup;
380 const auto& joint_name_group_index_map = (active_subgroup_name != servo_params_.move_group_name) ?
381 joint_name_to_index_maps_.at(servo_params_.active_subgroup) :
383
384 const int num_joints =
385 robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
386 Eigen::VectorXd joint_position_deltas(num_joints);
387 joint_position_deltas.setZero();
388
389 JointDeltaResult delta_result;
390
391 const CommandType expected_type = getCommandType();
392 if (command.index() == static_cast<size_t>(expected_type))
393 {
394 if (expected_type == CommandType::JOINT_JOG)
395 {
396 delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_,
397 joint_name_group_index_map);
398 servo_status_ = delta_result.first;
399 }
400 else if (expected_type == CommandType::TWIST)
401 {
402 // Transform the twist command to the planning frame, which is the base frame of the active subgroup's IK solver,
403 // before applying it. Additionally verify there is an IK solver, and that the transformation is successful.
404 const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name);
405 if (planning_frame_maybe.has_value())
406 {
407 const auto& planning_frame = *planning_frame_maybe;
408 const auto command_in_planning_frame_maybe = toPlanningFrame(std::get<TwistCommand>(command), planning_frame);
409 if (command_in_planning_frame_maybe.has_value())
410 {
411 delta_result = jointDeltaFromTwist(*command_in_planning_frame_maybe, robot_state, servo_params_,
412 planning_frame, joint_name_group_index_map);
413 servo_status_ = delta_result.first;
414 }
415 else
416 {
417 servo_status_ = StatusCode::INVALID;
418 RCLCPP_ERROR_STREAM(logger_, "Could not transform twist command to planning frame.");
419 }
420 }
421 else
422 {
423 servo_status_ = StatusCode::INVALID;
424 RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str());
425 }
426 }
427 else if (expected_type == CommandType::POSE)
428 {
429 // Transform the pose command to the planning frame, which is the base frame of the active subgroup's IK solver,
430 // before applying it. The end effector frame is also extracted as the tip frame of the IK solver.
431 // Additionally verify there is an IK solver, and that the transformation is successful.
432 const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name);
433 const auto ee_frame_maybe = getIKSolverTipFrame(robot_state, active_subgroup_name);
434 if (planning_frame_maybe.has_value() && ee_frame_maybe.has_value())
435 {
436 const auto& planning_frame = *planning_frame_maybe;
437 const auto command_in_planning_frame_maybe = toPlanningFrame(std::get<PoseCommand>(command), planning_frame);
438 if (command_in_planning_frame_maybe.has_value())
439 {
440 delta_result = jointDeltaFromPose(*command_in_planning_frame_maybe, robot_state, servo_params_,
441 planning_frame, *ee_frame_maybe, joint_name_group_index_map);
442 servo_status_ = delta_result.first;
443 }
444 else
445 {
446 servo_status_ = StatusCode::INVALID;
447 RCLCPP_ERROR_STREAM(logger_, "Could not transform pose command to planning frame.");
448 }
449 }
450 else
451 {
452 servo_status_ = StatusCode::INVALID;
453 RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str());
454 }
455 }
456
457 if (servo_status_ != StatusCode::INVALID)
458 {
459 joint_position_deltas = delta_result.second;
460 }
461 }
462 else
463 {
464 servo_status_ = StatusCode::INVALID;
465 RCLCPP_WARN_STREAM(logger_, "Incoming servo command type does not match known command types.");
466 }
467
468 return joint_position_deltas;
469}
470
471KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot_state, const ServoInput& command)
472{
473 // Set status to clear
474 servo_status_ = StatusCode::NO_WARNING;
475
476 // Update the parameters
477 updateParams();
478
479 // Get the joint model group info.
480 const moveit::core::JointModelGroup* joint_model_group =
481 robot_state->getJointModelGroup(servo_params_.move_group_name);
482
483 // Get necessary information about joints
484 const std::vector<std::string> joint_names = joint_model_group->getActiveJointModelNames();
485 const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds();
486 const int num_joints = joint_names.size();
487
488 // Extract current state from robot state
489 KinematicState current_state = extractRobotState(robot_state, servo_params_.move_group_name);
490 KinematicState target_state(num_joints);
491 target_state.joint_names = joint_names;
492
493 // Compute the change in joint position due to the incoming command
494 Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command, robot_state);
495
496 if (collision_velocity_scale_ > 0 && collision_velocity_scale_ < 1)
497 {
499 }
500 else if (collision_velocity_scale_ == 0)
501 {
502 servo_status_ = StatusCode::HALT_FOR_COLLISION;
503 }
504
505 // Continue rest of the computations only if the command is valid
506 // The computations can be skipped also in case we are halting.
507 if (servo_status_ != StatusCode::INVALID && servo_status_ != StatusCode::HALT_FOR_COLLISION)
508 {
509 // Compute the next joint positions based on the joint position deltas
510 target_state.positions = current_state.positions + joint_position_delta;
511
512 // Compute the joint velocities required to reach positions
513 target_state.velocities = joint_position_delta / servo_params_.publish_period;
514
515 // Scale down the velocity based on joint velocity limit or user defined scaling if applicable.
516 const double joint_velocity_limit_scale = jointLimitVelocityScalingFactor(
517 target_state.velocities, joint_bounds, servo_params_.override_velocity_scaling_factor);
518 if (joint_velocity_limit_scale < 1.0) // 1.0 means no scaling.
519 {
520 RCLCPP_DEBUG_STREAM(logger_, "Joint velocity limit scaling applied by a factor of " << joint_velocity_limit_scale);
521 }
522 target_state.velocities *= joint_velocity_limit_scale;
523
524 // Adjust joint position based on scaled down velocity
525 target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
526
527 // Apply collision scaling to the joint position delta
528 target_state.positions =
529 current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions);
530
531 // Compute velocities based on smoothed joint positions
532 target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period;
533
534 // Check if any joints are going past joint position limits.
535 const std::vector<size_t> joint_variables_to_halt =
536 jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
537
538 // Apply halting if any joints need to be halted.
539 if (!joint_variables_to_halt.empty())
540 {
541 servo_status_ = StatusCode::JOINT_BOUND;
542 target_state = haltJoints(joint_variables_to_halt, current_state, target_state);
543 }
544 }
545
546 // Apply smoothing to the positions if a smoother was provided.
547 doSmoothing(target_state);
548
549 return target_state;
550}
551
552std::optional<Eigen::Isometry3d> Servo::getPlanningToCommandFrameTransform(const std::string& command_frame,
553 const std::string& planning_frame) const
554{
555 const moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
556 if (robot_state->knowsFrameTransform(command_frame) && (robot_state->knowsFrameTransform(planning_frame)))
557 {
558 return robot_state->getGlobalLinkTransform(planning_frame).inverse() *
559 robot_state->getGlobalLinkTransform(command_frame);
560 }
561 else
562 {
563 try
564 {
565 return tf2::transformToEigen(
566 planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0)));
567 }
568 catch (tf2::TransformException& ex)
569 {
570 RCLCPP_ERROR(logger_, "Failed to get planning to command frame transform: %s", ex.what());
571 return std::nullopt;
572 }
573 }
574}
575
576std::optional<TwistCommand> Servo::toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const
577{
578 Eigen::VectorXd transformed_twist = command.velocities;
579
580 if (command.frame_id != planning_frame)
581 {
582 // Look up the transform between the planning and command frames.
583 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
584 if (!planning_to_command_tf_maybe.has_value())
585 {
586 return std::nullopt;
587 }
588 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
589
590 if (servo_params_.apply_twist_commands_about_ee_frame)
591 {
592 // If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
593 const auto planning_to_command_rotation = planning_to_command_tf.linear();
594 const Eigen::Vector3d translation_vector =
595 planning_to_command_rotation *
596 Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
597 const Eigen::Vector3d angular_vector =
598 planning_to_command_rotation *
599 Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);
600
601 // Update the values of the original command message to reflect the change in frame
602 transformed_twist.head<3>() = translation_vector;
603 transformed_twist.tail<3>() = angular_vector;
604 }
605 else
606 {
607 // If the twist command is applied about the planning frame, the spatial twist is calculated
608 // as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
609 // The above equation defines twist as [angular; linear], but in our convention it is
610 // [linear; angular] so the adjoint matrix is also reordered accordingly.
611 Eigen::MatrixXd adjoint(6, 6);
612
613 const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
614 const Eigen::Vector3d& translation = planning_to_command_tf.translation();
615
616 Eigen::Matrix3d skew_translation;
617 skew_translation.row(0) << 0, -translation(2), translation(1);
618 skew_translation.row(1) << translation(2), 0, -translation(0);
619 skew_translation.row(2) << -translation(1), translation(0), 0;
620
621 adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
622 adjoint.topRightCorner(3, 3) = rotation;
623 adjoint.bottomLeftCorner(3, 3) = rotation;
624 adjoint.bottomRightCorner(3, 3).setZero();
625
626 transformed_twist = adjoint * transformed_twist;
627 }
628 }
629
630 return TwistCommand{ planning_frame, transformed_twist };
631}
632
633std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const
634{
635 const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
636 if (!planning_to_command_tf_maybe)
637 {
638 return std::nullopt;
639 }
640
641 const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
642 return PoseCommand{ planning_frame, planning_to_command_tf * command.pose };
643}
644
645KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
646{
647 if (block_for_current_state)
648 {
649 bool have_current_state = false;
650 while (rclcpp::ok() && !have_current_state)
651 {
652 have_current_state =
653 planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), ROBOT_STATE_WAIT_TIME /* s */);
654 if (!have_current_state)
655 {
656 RCLCPP_WARN(logger_, "Waiting for the current state");
657 }
658 }
659 }
660 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
661 return extractRobotState(robot_state, servo_params_.move_group_name);
662}
663
664std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_state)
665{
666 auto target_state = halt_state;
667
668 // If all velocities are near zero, robot has decelerated to a stop.
669 bool stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
670
671 if (!stopped)
672 {
673 // set target velocity
674 target_state.velocities *= 0.0;
675
676 // scale velocity in case of obstacle
677 target_state.velocities *= collision_velocity_scale_;
678
679 for (long i = 0; i < halt_state.positions.size(); ++i)
680 {
681 target_state.positions[i] = halt_state.positions[i] + target_state.velocities[i] * servo_params_.publish_period;
682 const double vel = target_state.velocities[i];
683 target_state.velocities[i] = (std::abs(vel) > STOPPED_VELOCITY_EPS) ? vel : 0.0;
684 target_state.accelerations[i] =
685 (target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
686 }
687 }
688
689 // apply smoothing: this will change target position/velocity to make slow down gradual
690 doSmoothing(target_state);
691
692 return std::make_pair(stopped, target_state);
693}
694
695} // 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:645
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:664
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:471
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:143
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:86
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:224
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.
std::vector< std::string > joint_names
Eigen::VectorXd accelerations