39#if __has_include(<tf2/LinearMath/Quaternion.hpp>)
40#include <tf2/LinearMath/Quaternion.hpp>
42#include <tf2/LinearMath/Quaternion.h>
44#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
45#include <tf2_eigen/tf2_eigen.hpp>
46#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
58 const std::string& group_name,
const std::string& link_name,
59 const Eigen::Isometry3d& pose,
const std::string& frame_id,
60 const std::map<std::string, double>& seed,
61 std::map<std::string, double>& solution,
bool check_self_collision,
64 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
65 if (!robot_model->hasJointModelGroup(group_name))
67 RCLCPP_ERROR_STREAM(getLogger(),
"Robot model has no planning group named as " << group_name);
71 if (frame_id != robot_model->getModelFrame())
73 RCLCPP_ERROR_STREAM(getLogger(),
"Given frame (" << frame_id <<
") is unequal to model frame("
74 << robot_model->getModelFrame() <<
')');
82 if (check_self_collision)
86 const double* joint_group_variable_values) {
88 joint_group_variable_values);
94 if (rstate.setFromIK(jmg, pose, link_name, timeout, ik_constraint_function))
99 solution[joint_name] = rstate.getVariablePosition(joint_name);
105 RCLCPP_ERROR(getLogger(),
"Unable to find IK solution.");
113 const std::string& group_name,
const std::string& link_name,
114 const geometry_msgs::msg::Pose& pose,
const std::string& frame_id,
115 const std::map<std::string, double>& seed,
116 std::map<std::string, double>& solution,
bool check_self_collision,
117 const double timeout)
119 Eigen::Isometry3d pose_eigen;
120 tf2::convert<geometry_msgs::msg::Pose, Eigen::Isometry3d>(pose, pose_eigen);
121 return computePoseIK(scene, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision,
126 const std::map<std::string, double>& joint_state,
127 Eigen::Isometry3d& pose)
132 RCLCPP_ERROR_STREAM(getLogger(),
"The target link " << link_name <<
" is not known by robot.");
146 const std::map<std::string, double>& position_last,
const std::map<std::string, double>& velocity_last,
147 const std::map<std::string, double>& position_current,
double duration_last,
double duration_current,
150 const double epsilon = 10e-6;
151 if (duration_current <= epsilon)
153 RCLCPP_ERROR(getLogger(),
"Sample duration too small, cannot compute the velocity");
157 double velocity_current, acceleration_current;
159 for (
const auto& pos : position_current)
161 velocity_current = (pos.second - position_last.at(pos.first)) / duration_current;
163 if (!
joint_limits.verifyVelocityLimit(pos.first, velocity_current))
165 RCLCPP_ERROR_STREAM(getLogger(),
"Joint velocity limit of "
166 << pos.first <<
" violated. Set the velocity scaling factor lower!"
167 <<
" Actual joint velocity is " << velocity_current
168 <<
", while the limit is " <<
joint_limits.getLimit(pos.first).max_velocity
173 acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2;
175 if (fabs(velocity_last.at(pos.first)) <= fabs(velocity_current))
177 if (!
joint_limits.verifyAccelerationLimit(pos.first, acceleration_current))
179 RCLCPP_ERROR_STREAM(getLogger(),
"Joint acceleration limit of "
180 << pos.first <<
" violated. Set the acceleration scaling factor lower!"
181 <<
" Actual joint acceleration is " << acceleration_current
182 <<
", while the limit is "
183 <<
joint_limits.getLimit(pos.first).max_acceleration <<
". ");
190 if (!
joint_limits.verifyDecelerationLimit(pos.first, acceleration_current))
192 RCLCPP_ERROR_STREAM(getLogger(),
"Joint deceleration limit of "
193 << pos.first <<
" violated. Set the acceleration scaling factor lower!"
194 <<
" Actual joint deceleration is " << acceleration_current
195 <<
", while the limit is "
196 <<
joint_limits.getLimit(pos.first).max_deceleration <<
". ");
206 const planning_scene::PlanningSceneConstPtr& scene,
208 const std::string& group_name,
const std::string& link_name,
209 const std::map<std::string, double>& initial_joint_position,
double sampling_time,
210 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
211 bool check_self_collision)
213 RCLCPP_DEBUG(getLogger(),
"Generate joint trajectory from a Cartesian trajectory.");
215 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
217 rclcpp::Time generation_begin = clock.now();
220 const double epsilon = 10e-06;
221 std::vector<double> time_samples;
222 for (
double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time)
224 time_samples.push_back(t_sample);
226 time_samples.push_back(trajectory.Duration());
229 Eigen::Isometry3d pose_sample;
230 std::map<std::string, double> ik_solution_last, ik_solution, joint_velocity_last;
231 ik_solution_last = initial_joint_position;
232 for (
const auto& item : ik_solution_last)
234 joint_velocity_last[item.first] = 0.0;
237 for (std::vector<double>::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end();
240 tf2::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample);
242 if (!
computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
243 ik_solution, check_self_collision))
245 RCLCPP_ERROR(getLogger(),
"Failed to compute inverse kinematics solution for sampled Cartesian pose.");
246 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
247 joint_trajectory.points.clear();
252 double duration_current_sample = sampling_time;
254 if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1)
256 duration_current_sample = *time_iter - *(time_iter - 1);
258 if (time_samples.size() == 1)
260 duration_current_sample = *time_iter;
264 if (time_iter != time_samples.begin() &&
268 RCLCPP_ERROR_STREAM(getLogger(),
"Inverse kinematics solution at "
270 <<
"s violates the joint velocity/acceleration/deceleration limits.");
271 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
272 joint_trajectory.points.clear();
277 trajectory_msgs::msg::JointTrajectoryPoint point;
280 joint_trajectory.joint_names.clear();
281 for (
const auto& start_joint : initial_joint_position)
283 joint_trajectory.joint_names.push_back(start_joint.first);
286 point.time_from_start = rclcpp::Duration::from_seconds(*time_iter);
287 for (
const auto& joint_name : joint_trajectory.joint_names)
289 point.positions.push_back(ik_solution.at(joint_name));
291 if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1)
293 double joint_velocity =
294 (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample;
295 point.velocities.push_back(joint_velocity);
296 point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
297 (duration_current_sample + sampling_time) * 2);
298 joint_velocity_last[joint_name] = joint_velocity;
302 point.velocities.push_back(0.);
303 point.accelerations.push_back(0.);
304 joint_velocity_last[joint_name] = 0.;
309 joint_trajectory.points.push_back(point);
310 ik_solution_last = ik_solution;
313 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
314 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
315 RCLCPP_DEBUG_STREAM(getLogger(),
"Generate trajectory (N-Points: "
316 << joint_trajectory.points.size() <<
") took " << duration_ms <<
" ms | "
317 << duration_ms / joint_trajectory.points.size() <<
" ms per Point");
323 const planning_scene::PlanningSceneConstPtr& scene,
326 const std::string& link_name,
const std::map<std::string, double>& initial_joint_position,
327 const std::map<std::string, double>& initial_joint_velocity,
328 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
329 bool check_self_collision)
331 RCLCPP_DEBUG(getLogger(),
"Generate joint trajectory from a Cartesian trajectory.");
333 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
335 rclcpp::Time generation_begin = clock.now();
337 std::map<std::string, double> ik_solution_last = initial_joint_position;
338 std::map<std::string, double> joint_velocity_last = initial_joint_velocity;
339 double duration_last = 0;
340 double duration_current = 0;
341 joint_trajectory.joint_names.clear();
342 for (
const auto& joint_position : ik_solution_last)
344 joint_trajectory.joint_names.push_back(joint_position.first);
346 std::map<std::string, double> ik_solution;
347 for (
size_t i = 0; i < trajectory.
points.size(); ++i)
350 if (!
computePoseIK(scene, group_name, link_name, trajectory.
points.at(i).pose, robot_model->getModelFrame(),
351 ik_solution_last, ik_solution, check_self_collision))
353 RCLCPP_ERROR(getLogger(),
"Failed to compute inverse kinematics solution for sampled "
355 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
356 joint_trajectory.points.clear();
363 duration_current = trajectory.
points.front().time_from_start.seconds();
364 duration_last = duration_current;
369 trajectory.
points.at(i).time_from_start.seconds() - trajectory.
points.at(i - 1).time_from_start.seconds();
372 if (!
verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current,
379 RCLCPP_ERROR_STREAM(getLogger(),
"Inverse kinematics solution of the "
381 <<
"th sample violates the joint "
382 "velocity/acceleration/deceleration limits.");
383 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
384 joint_trajectory.points.clear();
390 trajectory_msgs::msg::JointTrajectoryPoint waypoint_joint;
391 waypoint_joint.time_from_start = trajectory.
points.at(i).time_from_start;
392 for (
const auto& joint_name : joint_trajectory.joint_names)
394 waypoint_joint.positions.push_back(ik_solution.at(joint_name));
395 double joint_velocity = (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current;
396 waypoint_joint.velocities.push_back(joint_velocity);
397 waypoint_joint.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
398 (duration_current + duration_last) * 2);
400 joint_velocity_last[joint_name] = joint_velocity;
404 joint_trajectory.points.push_back(waypoint_joint);
405 ik_solution_last = ik_solution;
406 duration_last = duration_current;
409 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
411 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
412 RCLCPP_DEBUG_STREAM(getLogger(),
"Generate trajectory (N-Points: "
413 << joint_trajectory.points.size() <<
") took " << duration_ms <<
" ms | "
414 << duration_ms / joint_trajectory.points.size() <<
" ms per Point");
420 const robot_trajectory::RobotTrajectoryPtr& first_trajectory,
421 const robot_trajectory::RobotTrajectoryPtr& second_trajectory,
double epsilon,
double& sampling_time)
425 std::size_t n1 = first_trajectory->getWayPointCount() - 1;
426 std::size_t n2 = second_trajectory->getWayPointCount() - 1;
427 if ((n1 < 2) && (n2 < 2))
429 RCLCPP_ERROR_STREAM(getLogger(),
"Both trajectories do not have enough points to determine sampling time.");
435 sampling_time = first_trajectory->getWayPointDurationFromPrevious(1);
439 sampling_time = second_trajectory->getWayPointDurationFromPrevious(1);
442 for (std::size_t i = 1; i < std::max(n1, n2); ++i)
446 if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
448 RCLCPP_ERROR_STREAM(getLogger(),
"First trajectory violates sampline time "
449 << sampling_time <<
" between points " << (i - 1) <<
"and " << i
457 if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
459 RCLCPP_ERROR_STREAM(getLogger(),
"Second trajectory violates sampline time "
460 << sampling_time <<
" between points " << (i - 1) <<
"and " << i
472 const std::string& joint_group_name,
double epsilon)
474 Eigen::VectorXd joint_position_1, joint_position_2;
479 if ((joint_position_1 - joint_position_2).norm() > epsilon)
481 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint positions of the two states are different. state1: "
482 << joint_position_1 <<
" state2: " << joint_position_2);
486 Eigen::VectorXd joint_velocity_1, joint_velocity_2;
491 if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon)
493 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint velocities of the two states are different. state1: "
494 << joint_velocity_1 <<
" state2: " << joint_velocity_2);
498 Eigen::VectorXd joint_acc_1, joint_acc_2;
503 if ((joint_acc_1 - joint_acc_2).norm() > epsilon)
505 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint accelerations of the two states are different. state1: "
506 << joint_acc_1 <<
" state2: " << joint_acc_2);
514 const std::string& group,
double EPSILON)
516 Eigen::VectorXd joint_variable;
518 if (joint_variable.norm() >
EPSILON)
520 RCLCPP_DEBUG(getLogger(),
"Joint velocities are not zero.");
524 if (joint_variable.norm() >
EPSILON)
526 RCLCPP_DEBUG(getLogger(),
"Joint accelerations are not zero.");
533 const Eigen::Vector3d& center_position,
double r,
534 const robot_trajectory::RobotTrajectoryPtr& traj,
535 bool inverseOrder, std::size_t& index)
537 RCLCPP_DEBUG(getLogger(),
"Start linear search for intersection point.");
539 const size_t waypoint_num = traj->getWayPointCount();
543 for (
size_t i = waypoint_num - 1; i > 0; --i)
545 if (
intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
546 traj->getWayPointPtr(i - 1)->getFrameTransform(link_name).translation(), r))
555 for (
size_t i = 0; i < waypoint_num - 1; ++i)
557 if (
intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
558 traj->getWayPointPtr(i + 1)->getFrameTransform(link_name).translation(), r))
570 const Eigen::Vector3d& p_current,
const Eigen::Vector3d& p_next,
573 return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r);
579 const double*
const ik_solution)
587 scene->checkSelfCollision(collision_req, collision_res, *rstate);
594 tf2::convert<geometry_msgs::msg::Quaternion, tf2::Quaternion>(quat, q);
595 quat = tf2::toMsg(q.normalized());
599 const geometry_msgs::msg::Quaternion& orientation,
600 const geometry_msgs::msg::Vector3& offset)
602 Eigen::Quaterniond quat;
603 tf2::fromMsg(orientation, quat);
606 tf2::fromMsg(position, v);
608 Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat;
610 tf2::fromMsg(offset, v);
611 pose.translation() -= quat * v;
617 return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position,
618 goal.orientation_constraints.front().orientation,
619 goal.position_constraints.front().target_point_offset);
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...
const std::string & getName() const
Get the name of the joint group.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void update(bool force=false)
Update all transforms.
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
rclcpp::Logger getLogger()
std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
bool computeLinkFK(moveit::core::RobotState &robot_state, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at a given robot state
bool linearSearchIntersectionPoint(const std::string &link_name, const Eigen::Vector3d ¢er_position, const double r, const robot_trajectory::RobotTrajectoryPtr &traj, bool inverseOrder, std::size_t &index)
Performs a linear search for the intersection point of the trajectory with the blending radius.
bool isStateColliding(const planning_scene::PlanningSceneConstPtr &scene, moveit::core::RobotState *state, const moveit::core::JointModelGroup *const group, const double *const ik_solution)
Checks if current robot state is in self collision.
bool isRobotStateStationary(const moveit::core::RobotState &state, const std::string &group, double EPSILON)
check if the robot state have zero velocity/acceleration
bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr &first_trajectory, const robot_trajectory::RobotTrajectoryPtr &second_trajectory, double EPSILON, double &sampling_time)
Determines the sampling time and checks that both trajectroies use the same sampling time.
bool intersectionFound(const Eigen::Vector3d &p_center, const Eigen::Vector3d &p_current, const Eigen::Vector3d &p_next, double r)
bool verifySampleJointLimits(const std::map< std::string, double > &position_last, const std::map< std::string, double > &velocity_last, const std::map< std::string, double > &position_current, double duration_last, double duration_current, const JointLimitsContainer &joint_limits)
verify the velocity/acceleration limits of current sample (based on backward difference computation) ...
bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const JointLimitsContainer &joint_limits, const KDL::Trajectory &trajectory, const std::string &group_name, const std::string &link_name, const std::map< std::string, double > &initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory &joint_trajectory, moveit_msgs::msg::MoveItErrorCodes &error_code, bool check_self_collision=false)
Generate joint trajectory from a KDL Cartesian trajectory.
bool computePoseIK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::string &link_name, const Eigen::Isometry3d &pose, const std::string &frame_id, const std::map< std::string, double > &seed, std::map< std::string, double > &solution, bool check_self_collision=true, const double timeout=0.0)
compute the inverse kinematics of a given pose, also check robot self collision
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool verbose
Flag indicating whether information about detected collisions should be reported.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.
std::vector< CartesianTrajectoryPoint > points
void normalizeQuaternion(geometry_msgs::msg::Quaternion &quat)
Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point &position, const geometry_msgs::msg::Quaternion &orientation, const geometry_msgs::msg::Vector3 &offset)
Adapt goal pose, defined by position+orientation, to consider offset.