53 const std::string
LOGNAME =
"cartesian_interpolator";
56 std::vector<RobotStatePtr>& traj,
const LinkModel* link,
58 const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
62 const double distance = translation.norm();
64 Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link);
67 pose.translation() += global_reference_frame ? translation : pose.linear() * translation;
71 validCallback, options);
75 std::vector<RobotStatePtr>& traj,
const LinkModel* link,
76 const Eigen::Isometry3d& target,
bool global_reference_frame,
77 const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
80 const Eigen::Isometry3d& link_offset)
86 const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
88 for (
const JointModel* joint : cjnt)
89 start_state->enforceBounds(joint);
92 Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link) * link_offset;
93 Eigen::Isometry3d offset = link_offset.inverse();
96 Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;
98 Eigen::Quaterniond start_quaternion(start_pose.linear());
99 Eigen::Quaterniond target_quaternion(rotated_target.linear());
101 if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
104 "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
105 "MaxEEFStep.translation components must be non-negative and at least one component must be "
106 "greater than zero");
110 double rotation_distance = start_quaternion.angularDistance(target_quaternion);
111 double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
114 std::size_t translation_steps = 0;
115 if (max_step.translation > 0.0)
116 translation_steps = floor(translation_distance / max_step.translation);
118 std::size_t rotation_steps = 0;
119 if (max_step.rotation > 0.0)
120 rotation_steps = floor(rotation_distance / max_step.rotation);
123 std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
128 std::vector<double> consistency_limits;
129 if (jump_threshold.prismatic > 0 || jump_threshold.revolute > 0)
130 for (
const JointModel* jm : group->getActiveJointModels())
133 switch (jm->getType())
136 limit = jump_threshold.revolute;
139 limit = jump_threshold.prismatic;
145 limit = jm->getMaximumExtent();
146 consistency_limits.push_back(limit);
150 traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
152 double last_valid_percentage = 0.0;
153 for (std::size_t i = 1; i <= steps; ++i)
155 double percentage = (double)i / (
double)steps;
157 Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
158 pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
162 if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
163 traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
167 last_valid_percentage = percentage;
172 return last_valid_percentage;
176 RobotState* start_state,
const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
const LinkModel* link,
181 double percentage_solved = 0.0;
182 for (std::size_t i = 0; i < waypoints.size(); ++i)
185 static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST;
186 std::vector<RobotStatePtr> waypoint_traj;
187 double wp_percentage_solved =
188 computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
189 NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset);
190 if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
192 percentage_solved = (double)(i + 1) / (double)waypoints.size();
193 std::vector<RobotStatePtr>::iterator
start = waypoint_traj.begin();
194 if (i > 0 && !waypoint_traj.empty())
195 std::advance(start, 1);
196 traj.insert(traj.end(),
start, waypoint_traj.end());
200 percentage_solved += wp_percentage_solved / (double)waypoints.size();
201 std::vector<RobotStatePtr>::iterator
start = waypoint_traj.begin();
202 if (i > 0 && !waypoint_traj.empty())
203 std::advance(start, 1);
204 traj.insert(traj.end(),
start, waypoint_traj.end());
211 return percentage_solved;
215 const JumpThreshold& jump_threshold)
217 double percentage_solved = 1.0;
218 if (traj.size() <= 1)
219 return percentage_solved;
221 if (jump_threshold.factor > 0.0)
224 double percentage_solved_absolute = 1.0;
225 if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
226 percentage_solved_absolute =
229 return std::min(percentage_solved, percentage_solved_absolute);
233 std::vector<RobotStatePtr>& traj,
234 double jump_threshold_factor)
239 "The computed trajectory is too short to detect jumps in joint-space "
240 "Need at least %zu steps, only got %zu. Try a lower max_step.",
244 std::vector<double> dist_vector;
245 dist_vector.reserve(traj.size() - 1);
246 double total_dist = 0.0;
247 for (std::size_t i = 1; i < traj.size(); ++i)
249 double dist_prev_point = traj[i]->distance(*traj[i - 1], group);
250 dist_vector.push_back(dist_prev_point);
251 total_dist += dist_prev_point;
254 double percentage = 1.0;
256 double thres = jump_threshold_factor * (total_dist / (double)dist_vector.size());
257 for (std::size_t i = 0; i < dist_vector.size(); ++i)
258 if (dist_vector[i] > thres)
261 percentage = (double)(i + 1) / (double)(traj.size());
270 std::vector<RobotStatePtr>& traj,
double revolute_threshold,
271 double prismatic_threshold)
273 bool check_revolute = revolute_threshold > 0.0;
274 bool check_prismatic = prismatic_threshold > 0.0;
276 double joint_threshold;
278 bool still_valid =
true;
279 const std::vector<const JointModel*>& joints = group->getActiveJointModels();
280 for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix)
282 for (
auto& joint : joints)
284 switch (joint->getType())
287 check_joint = check_revolute;
288 joint_threshold = revolute_threshold;
291 check_joint = check_prismatic;
292 joint_threshold = prismatic_threshold;
296 "Joint %s has not supported type %s. \n"
297 "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.",
298 joint->getName().c_str(), joint->getTypeName().c_str());
303 double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
307 distance, joint_threshold, joint->getName().c_str());
316 double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
317 traj.resize(traj_ix + 1);
318 return percent_valid;