38#include <tf2/LinearMath/Quaternion.h>
39#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
40#include <tf2_eigen/tf2_eigen.hpp>
41#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
53 const std::string& group_name,
const std::string& link_name,
54 const Eigen::Isometry3d& pose,
const std::string& frame_id,
55 const std::map<std::string, double>& seed,
56 std::map<std::string, double>& solution,
bool check_self_collision,
59 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
60 if (!robot_model->hasJointModelGroup(group_name))
62 RCLCPP_ERROR_STREAM(getLogger(),
"Robot model has no planning group named as " << group_name);
66 if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name))
68 RCLCPP_ERROR_STREAM(getLogger(),
69 "No valid IK solver exists for " << link_name <<
" in planning group " << group_name);
73 if (frame_id != robot_model->getModelFrame())
75 RCLCPP_ERROR_STREAM(getLogger(),
"Given frame (" << frame_id <<
") is unequal to model frame("
76 << robot_model->getModelFrame() <<
')');
86 const double* joint_group_variable_values) {
88 joint_group_variable_values);
92 if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function))
95 for (
const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames())
97 solution[joint_name] = rstate.getVariablePosition(joint_name);
103 RCLCPP_ERROR(getLogger(),
"Unable to find IK solution.");
111 const std::string& group_name,
const std::string& link_name,
112 const geometry_msgs::msg::Pose& pose,
const std::string& frame_id,
113 const std::map<std::string, double>& seed,
114 std::map<std::string, double>& solution,
bool check_self_collision,
115 const double timeout)
117 Eigen::Isometry3d pose_eigen;
118 tf2::convert<geometry_msgs::msg::Pose, Eigen::Isometry3d>(pose, pose_eigen);
119 return computePoseIK(scene, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision,
124 const std::string& link_name,
125 const std::map<std::string, double>& joint_state,
126 Eigen::Isometry3d& pose)
131 if (!rstate.knowsFrameTransform(link_name))
133 RCLCPP_ERROR_STREAM(getLogger(),
"The target link " << link_name <<
" is not known by robot.");
137 rstate.setVariablePositions(joint_state);
141 pose = rstate.getFrameTransform(link_name);
147 const std::map<std::string, double>& position_last,
const std::map<std::string, double>& velocity_last,
148 const std::map<std::string, double>& position_current,
double duration_last,
double duration_current,
151 const double epsilon = 10e-6;
152 if (duration_current <= epsilon)
154 RCLCPP_ERROR(getLogger(),
"Sample duration too small, cannot compute the velocity");
158 double velocity_current, acceleration_current;
160 for (
const auto& pos : position_current)
162 velocity_current = (pos.second - position_last.at(pos.first)) / duration_current;
164 if (!
joint_limits.verifyVelocityLimit(pos.first, velocity_current))
166 RCLCPP_ERROR_STREAM(getLogger(),
"Joint velocity limit of "
167 << pos.first <<
" violated. Set the velocity scaling factor lower!"
168 <<
" Actual joint velocity is " << velocity_current
169 <<
", while the limit is " <<
joint_limits.getLimit(pos.first).max_velocity
174 acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2;
176 if (fabs(velocity_last.at(pos.first)) <= fabs(velocity_current))
178 if (!
joint_limits.verifyAccelerationLimit(pos.first, acceleration_current))
180 RCLCPP_ERROR_STREAM(getLogger(),
"Joint acceleration limit of "
181 << pos.first <<
" violated. Set the acceleration scaling factor lower!"
182 <<
" Actual joint acceleration is " << acceleration_current
183 <<
", while the limit is "
184 <<
joint_limits.getLimit(pos.first).max_acceleration <<
". ");
191 if (!
joint_limits.verifyDecelerationLimit(pos.first, acceleration_current))
193 RCLCPP_ERROR_STREAM(getLogger(),
"Joint deceleration limit of "
194 << pos.first <<
" violated. Set the acceleration scaling factor lower!"
195 <<
" Actual joint deceleration is " << acceleration_current
196 <<
", while the limit is "
197 <<
joint_limits.getLimit(pos.first).max_deceleration <<
". ");
207 const planning_scene::PlanningSceneConstPtr& scene,
209 const std::string& group_name,
const std::string& link_name,
210 const std::map<std::string, double>& initial_joint_position,
double sampling_time,
211 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
212 bool check_self_collision)
214 RCLCPP_DEBUG(getLogger(),
"Generate joint trajectory from a Cartesian trajectory.");
216 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
218 rclcpp::Time generation_begin = clock.now();
221 const double epsilon = 10e-06;
222 std::vector<double> time_samples;
223 for (
double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time)
225 time_samples.push_back(t_sample);
227 time_samples.push_back(trajectory.Duration());
230 Eigen::Isometry3d pose_sample;
231 std::map<std::string, double> ik_solution_last, ik_solution, joint_velocity_last;
232 ik_solution_last = initial_joint_position;
233 for (
const auto& item : ik_solution_last)
235 joint_velocity_last[item.first] = 0.0;
238 for (std::vector<double>::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end();
241 tf2::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample);
243 if (!
computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
244 ik_solution, check_self_collision))
246 RCLCPP_ERROR(getLogger(),
"Failed to compute inverse kinematics solution for sampled Cartesian pose.");
247 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
248 joint_trajectory.points.clear();
253 double duration_current_sample = sampling_time;
255 if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1)
257 duration_current_sample = *time_iter - *(time_iter - 1);
259 if (time_samples.size() == 1)
261 duration_current_sample = *time_iter;
265 if (time_iter != time_samples.begin() &&
269 RCLCPP_ERROR_STREAM(getLogger(),
"Inverse kinematics solution at "
271 <<
"s violates the joint velocity/acceleration/deceleration limits.");
272 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
273 joint_trajectory.points.clear();
278 trajectory_msgs::msg::JointTrajectoryPoint point;
281 joint_trajectory.joint_names.clear();
282 for (
const auto& start_joint : initial_joint_position)
284 joint_trajectory.joint_names.push_back(start_joint.first);
287 point.time_from_start = rclcpp::Duration::from_seconds(*time_iter);
288 for (
const auto& joint_name : joint_trajectory.joint_names)
290 point.positions.push_back(ik_solution.at(joint_name));
292 if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1)
294 double joint_velocity =
295 (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample;
296 point.velocities.push_back(joint_velocity);
297 point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
298 (duration_current_sample + sampling_time) * 2);
299 joint_velocity_last[joint_name] = joint_velocity;
303 point.velocities.push_back(0.);
304 point.accelerations.push_back(0.);
305 joint_velocity_last[joint_name] = 0.;
310 joint_trajectory.points.push_back(point);
311 ik_solution_last = ik_solution;
314 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
315 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
316 RCLCPP_DEBUG_STREAM(getLogger(),
"Generate trajectory (N-Points: "
317 << joint_trajectory.points.size() <<
") took " << duration_ms <<
" ms | "
318 << duration_ms / joint_trajectory.points.size() <<
" ms per Point");
324 const planning_scene::PlanningSceneConstPtr& scene,
327 const std::string& link_name,
const std::map<std::string, double>& initial_joint_position,
328 const std::map<std::string, double>& initial_joint_velocity,
329 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
330 bool check_self_collision)
332 RCLCPP_DEBUG(getLogger(),
"Generate joint trajectory from a Cartesian trajectory.");
334 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
336 rclcpp::Time generation_begin = clock.now();
338 std::map<std::string, double> ik_solution_last = initial_joint_position;
339 std::map<std::string, double> joint_velocity_last = initial_joint_velocity;
340 double duration_last = 0;
341 double duration_current = 0;
342 joint_trajectory.joint_names.clear();
343 for (
const auto& joint_position : ik_solution_last)
345 joint_trajectory.joint_names.push_back(joint_position.first);
347 std::map<std::string, double> ik_solution;
348 for (
size_t i = 0; i < trajectory.
points.size(); ++i)
351 if (!
computePoseIK(scene, group_name, link_name, trajectory.
points.at(i).pose, robot_model->getModelFrame(),
352 ik_solution_last, ik_solution, check_self_collision))
354 RCLCPP_ERROR(getLogger(),
"Failed to compute inverse kinematics solution for sampled "
356 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
357 joint_trajectory.points.clear();
364 duration_current = trajectory.
points.front().time_from_start.seconds();
365 duration_last = duration_current;
370 trajectory.
points.at(i).time_from_start.seconds() - trajectory.
points.at(i - 1).time_from_start.seconds();
373 if (!
verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current,
380 RCLCPP_ERROR_STREAM(getLogger(),
"Inverse kinematics solution of the "
382 <<
"th sample violates the joint "
383 "velocity/acceleration/deceleration limits.");
384 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
385 joint_trajectory.points.clear();
391 trajectory_msgs::msg::JointTrajectoryPoint waypoint_joint;
392 waypoint_joint.time_from_start = trajectory.
points.at(i).time_from_start;
393 for (
const auto& joint_name : joint_trajectory.joint_names)
395 waypoint_joint.positions.push_back(ik_solution.at(joint_name));
396 double joint_velocity = (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current;
397 waypoint_joint.velocities.push_back(joint_velocity);
398 waypoint_joint.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
399 (duration_current + duration_last) * 2);
401 joint_velocity_last[joint_name] = joint_velocity;
405 joint_trajectory.points.push_back(waypoint_joint);
406 ik_solution_last = ik_solution;
407 duration_last = duration_current;
410 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
412 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
413 RCLCPP_DEBUG_STREAM(getLogger(),
"Generate trajectory (N-Points: "
414 << joint_trajectory.points.size() <<
") took " << duration_ms <<
" ms | "
415 << duration_ms / joint_trajectory.points.size() <<
" ms per Point");
421 const robot_trajectory::RobotTrajectoryPtr& first_trajectory,
422 const robot_trajectory::RobotTrajectoryPtr& second_trajectory,
double epsilon,
double& sampling_time)
426 std::size_t n1 = first_trajectory->getWayPointCount() - 1;
427 std::size_t n2 = second_trajectory->getWayPointCount() - 1;
428 if ((n1 < 2) && (n2 < 2))
430 RCLCPP_ERROR_STREAM(getLogger(),
"Both trajectories do not have enough points to determine sampling time.");
436 sampling_time = first_trajectory->getWayPointDurationFromPrevious(1);
440 sampling_time = second_trajectory->getWayPointDurationFromPrevious(1);
443 for (std::size_t i = 1; i < std::max(n1, n2); ++i)
447 if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
449 RCLCPP_ERROR_STREAM(getLogger(),
"First trajectory violates sampline time "
450 << sampling_time <<
" between points " << (i - 1) <<
"and " << i
458 if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
460 RCLCPP_ERROR_STREAM(getLogger(),
"Second trajectory violates sampline time "
461 << sampling_time <<
" between points " << (i - 1) <<
"and " << i
473 const std::string& joint_group_name,
double epsilon)
475 Eigen::VectorXd joint_position_1, joint_position_2;
480 if ((joint_position_1 - joint_position_2).norm() > epsilon)
482 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint positions of the two states are different. state1: "
483 << joint_position_1 <<
" state2: " << joint_position_2);
487 Eigen::VectorXd joint_velocity_1, joint_velocity_2;
492 if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon)
494 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint velocities of the two states are different. state1: "
495 << joint_velocity_1 <<
" state2: " << joint_velocity_2);
499 Eigen::VectorXd joint_acc_1, joint_acc_2;
504 if ((joint_acc_1 - joint_acc_2).norm() > epsilon)
506 RCLCPP_DEBUG_STREAM(getLogger(),
"Joint accelerations of the two states are different. state1: "
507 << joint_acc_1 <<
" state2: " << joint_acc_2);
515 const std::string& group,
double EPSILON)
517 Eigen::VectorXd joint_variable;
519 if (joint_variable.norm() >
EPSILON)
521 RCLCPP_DEBUG(getLogger(),
"Joint velocities are not zero.");
525 if (joint_variable.norm() >
EPSILON)
527 RCLCPP_DEBUG(getLogger(),
"Joint accelerations are not zero.");
534 const Eigen::Vector3d& center_position,
double r,
535 const robot_trajectory::RobotTrajectoryPtr& traj,
536 bool inverseOrder, std::size_t& index)
538 RCLCPP_DEBUG(getLogger(),
"Start linear search for intersection point.");
540 const size_t waypoint_num = traj->getWayPointCount();
544 for (
size_t i = waypoint_num - 1; i > 0; --i)
546 if (
intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
547 traj->getWayPointPtr(i - 1)->getFrameTransform(link_name).translation(), r))
556 for (
size_t i = 0; i < waypoint_num - 1; ++i)
558 if (
intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
559 traj->getWayPointPtr(i + 1)->getFrameTransform(link_name).translation(), r))
571 const Eigen::Vector3d& p_current,
const Eigen::Vector3d& p_next,
574 return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r);
578 const planning_scene::PlanningSceneConstPtr& scene,
581 const double*
const ik_solution)
583 if (!test_for_self_collision)
594 scene->checkSelfCollision(collision_req, collision_res, *rstate);
601 tf2::convert<geometry_msgs::msg::Quaternion, tf2::Quaternion>(quat, q);
602 quat = tf2::toMsg(q.normalized());
606 const geometry_msgs::msg::Quaternion& orientation,
607 const geometry_msgs::msg::Vector3& offset)
609 Eigen::Quaterniond quat;
610 tf2::fromMsg(orientation, quat);
613 tf2::fromMsg(position, v);
615 Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat;
617 tf2::fromMsg(offset, v);
618 pose.translation() -= quat * v;
624 return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position,
625 goal.orientation_constraints.front().orientation,
626 goal.position_constraints.front().target_point_offset);
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...
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 ...
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 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 bool test_for_self_collision, 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.
bool computeLinkFK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at give robot state
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.