40 #include <Eigen/Geometry>
49 const std::string
LOGNAME =
"trajectory_processing.time_optimal_trajectory_generation";
50 constexpr
double EPS = 0.000001;
65 s = std::max(0.0, std::min(1.0,
s));
76 return Eigen::VectorXd::Zero(
start_.size());
81 return std::list<double>();
97 CircularPathSegment(
const Eigen::VectorXd& start,
const Eigen::VectorXd& intersection,
const Eigen::VectorXd& end,
100 if ((intersection -
start).norm() < 0.000001 || (end - intersection).norm() < 0.000001)
105 x = Eigen::VectorXd::Zero(
start.size());
106 y = Eigen::VectorXd::Zero(
start.size());
110 const Eigen::VectorXd start_direction = (intersection -
start).normalized();
111 const Eigen::VectorXd end_direction = (end - intersection).normalized();
112 const double start_dot_end = start_direction.dot(end_direction);
115 if (start_dot_end > 0.999999 || start_dot_end < -0.999999)
120 x = Eigen::VectorXd::Zero(
start.size());
121 y = Eigen::VectorXd::Zero(
start.size());
125 const double angle = acos(start_dot_end);
126 const double start_distance = (
start - intersection).norm();
127 const double end_distance = (end - intersection).norm();
130 double distance = std::min(start_distance, end_distance);
136 center = intersection + (end_direction - start_direction).normalized() *
radius / cos(0.5 *
angle);
161 std::list<double> switching_points;
162 const double dim =
x.size();
163 for (
unsigned int i = 0; i < dim; ++i)
165 double switching_angle = atan2(
y[i],
x[i]);
166 if (switching_angle < 0.0)
168 switching_angle +=
M_PI;
170 const double switching_point = switching_angle *
radius;
173 switching_points.push_back(switching_point);
176 switching_points.sort();
177 return switching_points;
192 Path::Path(
const std::list<Eigen::VectorXd>& path,
double max_deviation) : length_(0.0)
196 std::list<Eigen::VectorXd>::const_iterator path_iterator1 = path.begin();
197 std::list<Eigen::VectorXd>::const_iterator path_iterator2 = path_iterator1;
199 std::list<Eigen::VectorXd>::const_iterator path_iterator3;
200 Eigen::VectorXd start_config = *path_iterator1;
201 while (path_iterator2 != path.end())
203 path_iterator3 = path_iterator2;
205 if (max_deviation > 0.0 && path_iterator3 != path.end())
209 0.5 * (*path_iterator2 + *path_iterator3), max_deviation);
210 Eigen::VectorXd end_config = blend_segment->
getConfig(0.0);
211 if ((end_config - start_config).norm() > 0.000001)
213 path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, end_config));
221 path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *path_iterator2));
222 start_config = *path_iterator2;
224 path_iterator1 = path_iterator2;
232 path_segment->position_ =
length_;
233 std::list<double> local_switching_points = path_segment->getSwitchingPoints();
234 for (std::list<double>::const_iterator
point = local_switching_points.begin();
235 point != local_switching_points.end(); ++
point)
239 length_ += path_segment->getLength();
247 Path::Path(
const Path& path) : length_(path.length_), switching_points_(path.switching_points_)
249 for (
const std::unique_ptr<PathSegment>& path_segment : path.
path_segments_)
262 std::list<std::unique_ptr<PathSegment>>::const_iterator it =
path_segments_.begin();
263 std::list<std::unique_ptr<PathSegment>>::const_iterator
next = it;
270 s -= (*it)->position_;
294 std::list<std::pair<double, bool>>::const_iterator it =
switching_points_.begin();
301 discontinuity =
true;
304 discontinuity = it->second;
316 , max_velocity_(max_velocity)
317 , max_acceleration_(max_acceleration)
318 , joint_num_(max_velocity.size())
320 , time_step_(time_step)
321 , cached_time_(
std::numeric_limits<double>::max())
327 double before_acceleration;
345 std::list<TrajectoryStep>::iterator previous =
trajectory_.begin();
346 std::list<TrajectoryStep>::iterator it = previous;
352 previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0);
365 double& before_acceleration,
double& after_acceleration)
368 double acceleration_before_acceleration, acceleration_after_acceleration;
369 bool acceleration_reached_end;
372 acceleration_reached_end =
374 acceleration_before_acceleration, acceleration_after_acceleration);
375 }
while (!acceleration_reached_end &&
379 double velocity_before_acceleration, velocity_after_acceleration;
380 bool velocity_reached_end;
384 velocity_before_acceleration, velocity_after_acceleration);
386 !velocity_reached_end && velocity_switching_point.
path_pos_ <= acceleration_switching_point.
path_pos_ &&
390 if (acceleration_reached_end && velocity_reached_end)
394 else if (!acceleration_reached_end &&
395 (velocity_reached_end || acceleration_switching_point.
path_pos_ <= velocity_switching_point.
path_pos_))
397 next_switching_point = acceleration_switching_point;
398 before_acceleration = acceleration_before_acceleration;
399 after_acceleration = acceleration_after_acceleration;
404 next_switching_point = velocity_switching_point;
405 before_acceleration = velocity_before_acceleration;
406 after_acceleration = velocity_after_acceleration;
412 double& before_acceleration,
double& after_acceleration)
414 double switching_path_pos = path_pos;
415 double switching_path_vel;
430 switching_path_vel = std::min(before_path_vel, after_path_vel);
434 if ((before_path_vel > after_path_vel ||
437 (before_path_vel < after_path_vel ||
getMinMaxPhaseSlope(switching_path_pos +
EPS, switching_path_vel,
true) <
446 before_acceleration = 0.0;
447 after_acceleration = 0.0;
457 next_switching_point =
TrajectoryStep(switching_path_pos, switching_path_vel);
462 double& before_acceleration,
double& after_acceleration)
464 const double step_size = 0.001;
465 const double accuracy = 0.000001;
468 path_pos -= step_size;
471 path_pos += step_size;
487 double before_path_pos = path_pos - step_size;
488 double after_path_pos = path_pos;
489 while (after_path_pos - before_path_pos > accuracy)
491 path_pos = (before_path_pos + after_path_pos) / 2.0;
495 before_path_pos = path_pos;
499 after_path_pos = path_pos;
512 double path_pos = trajectory.back().path_pos_;
513 double path_vel = trajectory.back().path_vel_;
516 std::list<std::pair<double, bool>>::iterator next_discontinuity = switching_points.begin();
520 while ((next_discontinuity != switching_points.end()) &&
521 (next_discontinuity->first <= path_pos || !next_discontinuity->second))
523 ++next_discontinuity;
526 double old_path_pos = path_pos;
527 double old_path_vel = path_vel;
530 path_pos +=
time_step_ * 0.5 * (old_path_vel + path_vel);
532 if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first)
536 if (path_pos - next_discontinuity->first <
EPS)
540 path_vel = old_path_vel +
541 (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos);
542 path_pos = next_discontinuity->first;
550 else if (path_vel < 0.0)
571 trajectory.pop_back();
572 double before = trajectory.back().
path_pos_;
573 double before_path_vel = trajectory.back().path_vel_;
575 double after_path_vel = overshoot.
path_vel_;
576 while (after - before >
EPS)
578 const double midpoint = 0.5 * (before + after);
579 double midpoint_path_vel = 0.5 * (before_path_vel + after_path_vel);
592 after_path_vel = midpoint_path_vel;
597 before_path_vel = midpoint_path_vel;
604 if (after > next_discontinuity->first)
608 else if (
getMinMaxPhaseSlope(trajectory.back().path_pos_, trajectory.back().path_vel_,
true) >
629 std::list<TrajectoryStep>::iterator start2 = start_trajectory.end();
631 std::list<TrajectoryStep>::iterator start1 = start2;
633 std::list<TrajectoryStep> trajectory;
635 assert(start1->path_pos_ <= path_pos);
637 while (start1 != start_trajectory.begin() || path_pos >= 0.0)
639 if (start1->path_pos_ <= path_pos)
643 path_pos -=
time_step_ * 0.5 * (path_vel + trajectory.front().path_vel_);
645 slope = (trajectory.front().path_vel_ - path_vel) / (trajectory.front().path_pos_ - path_pos);
663 const double start_slope = (start2->path_vel_ - start1->path_vel_) / (start2->path_pos_ - start1->path_pos_);
664 const double intersection_path_pos =
665 (start1->path_vel_ - path_vel + slope * path_pos - start_slope * start1->path_pos_) / (slope - start_slope);
666 if (std::max(start1->path_pos_, path_pos) -
EPS <= intersection_path_pos &&
667 intersection_path_pos <=
EPS + std::min(start2->path_pos_, trajectory.front().path_pos_))
669 const double intersection_path_vel =
670 start1->path_vel_ + start_slope * (intersection_path_pos - start1->path_pos_);
671 start_trajectory.erase(start2, start_trajectory.end());
672 start_trajectory.push_back(
TrajectoryStep(intersection_path_pos, intersection_path_vel));
673 start_trajectory.splice(start_trajectory.end(), trajectory);
687 double factor = max ? 1.0 : -1.0;
688 double max_path_acceleration = std::numeric_limits<double>::max();
691 if (config_deriv[i] != 0.0)
693 max_path_acceleration =
694 std::min(max_path_acceleration,
max_acceleration_[i] / std::abs(config_deriv[i]) -
695 factor * config_deriv2[i] * path_vel * path_vel / config_deriv[i]);
698 return factor * max_path_acceleration;
708 double max_path_velocity = std::numeric_limits<double>::infinity();
713 if (config_deriv[i] != 0.0)
715 for (
unsigned int j = i + 1; j <
joint_num_; ++j)
717 if (config_deriv[j] != 0.0)
719 double a_ij = config_deriv2[i] / config_deriv[i] - config_deriv2[j] / config_deriv[j];
722 max_path_velocity = std::min(max_path_velocity, sqrt((
max_acceleration_[i] / std::abs(config_deriv[i]) +
729 else if (config_deriv2[i] != 0.0)
731 max_path_velocity = std::min(max_path_velocity, sqrt(
max_acceleration_[i] / std::abs(config_deriv2[i])));
734 return max_path_velocity;
740 double max_path_velocity = std::numeric_limits<double>::max();
743 max_path_velocity = std::min(max_path_velocity,
max_velocity_[i] / std::abs(tangent[i]));
745 return max_path_velocity;
757 double max_path_velocity = std::numeric_limits<double>::max();
758 unsigned int active_constraint;
761 const double this_max_path_velocity =
max_velocity_[i] / std::abs(tangent[i]);
762 if (this_max_path_velocity < max_path_velocity)
764 max_path_velocity = this_max_path_velocity;
765 active_constraint = i;
769 (tangent[active_constraint] * std::abs(tangent[active_constraint]));
786 std::list<TrajectoryStep>::const_iterator last =
trajectory_.end();
808 std::list<TrajectoryStep>::const_iterator previous = it;
811 double time_step = it->time_ - previous->time_;
812 const double acceleration =
813 2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
815 time_step = time - previous->time_;
816 const double path_pos =
817 previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
825 std::list<TrajectoryStep>::const_iterator previous = it;
828 double time_step = it->time_ - previous->time_;
829 const double acceleration =
830 2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
832 const double path_pos =
833 previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
834 const double path_vel = previous->path_vel_ + time_step * acceleration;
842 std::list<TrajectoryStep>::const_iterator previous = it;
845 double time_step = it->time_ - previous->time_;
846 const double acceleration =
847 2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
849 const double path_pos =
850 previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
851 const double path_vel = previous->path_vel_ + time_step * acceleration;
852 Eigen::VectorXd path_acc =
855 path_acc /= time_step;
860 const double min_angle_change)
861 : path_tolerance_(path_tolerance), resample_dt_(resample_dt), min_angle_change_(min_angle_change)
867 const std::unordered_map<std::string, double>& acceleration_limits,
const double max_velocity_scaling_factor,
868 const double max_acceleration_scaling_factor)
const
870 if (trajectory.
empty())
885 auto validate_limit = [](
const char* type,
double value,
const std::string&
name) {
886 if (value <= std::numeric_limits<double>::epsilon())
899 Eigen::VectorXd max_velocity(num_joints);
900 Eigen::VectorXd max_acceleration(num_joints);
901 for (
size_t j = 0; j < num_joints; ++j)
903 const auto&
name = vars[j];
907 auto it = velocity_limits.find(
name);
908 if (it != velocity_limits.end())
910 if (!validate_limit(
"velocity", it->second,
name))
912 max_velocity[j] = it->second * velocity_scaling_factor;
929 it = acceleration_limits.find(
name);
930 if (it != acceleration_limits.end())
932 if (!validate_limit(
"acceleration", it->second,
name))
934 max_acceleration[j] = it->second * acceleration_scaling_factor;
942 acceleration_scaling_factor;
958 "There is a combination of revolute and prismatic joints in the robot model. "
959 "TOTG's `path_tolerance` parameter is applied to both types ignoring their different units.");
967 std::list<Eigen::VectorXd> points;
968 for (
size_t p = 0; p < num_points; ++p)
970 moveit::core::RobotStatePtr waypoint = trajectory.
getWayPointPtr(p);
971 Eigen::VectorXd new_point(num_joints);
973 bool diverse_point = (p == 0);
975 for (
size_t j = 0; j < num_joints; ++j)
977 new_point[j] = waypoint->getVariablePosition(idx[j]);
981 diverse_point =
true;
987 points.push_back(new_point);
991 else if (p == num_points - 1)
993 points.back() = new_point;
998 if (points.size() == 1)
1024 for (
size_t sample = 0; sample <= sample_count; ++sample)
1028 Eigen::VectorXd position = parameterized.
getPosition(
t);
1029 Eigen::VectorXd velocity = parameterized.
getVelocity(
t);
1032 for (
size_t j = 0; j < num_joints; ++j)
1048 const std::vector<const moveit::core::JointModel*>& joint_models = group->
getActiveJointModels();
1050 bool have_prismatic =
1052 return joint_model->getType() == moveit::core::JointModel::JointType::PRISMATIC;
1055 bool have_revolute =
1057 return joint_model->getType() == moveit::core::JointModel::JointType::REVOLUTE;
1060 return have_prismatic && have_revolute;
1065 double scaling_factor = std::clamp(requested_scaling_factor, 1e-7, 1.0);
1066 if (requested_scaling_factor != scaling_factor)
1069 requested_scaling_factor, scaling_factor);
1071 return scaling_factor;