00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
00038 #include <moveit_msgs/JointLimits.h>
00039 #include <console_bridge/console.h>
00040 #include <moveit/robot_state/conversions.h>
00041
00042 namespace trajectory_processing
00043 {
00044 static const double DEFAULT_VEL_MAX = 1.0;
00045 static const double DEFAULT_ACCEL_MAX = 1.0;
00046 static const double ROUNDING_THRESHOLD = 0.01;
00047
00048 IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization(unsigned int max_iterations,
00049 double max_time_change_per_it)
00050 : max_iterations_(max_iterations), max_time_change_per_it_(max_time_change_per_it)
00051 {
00052 }
00053
00054 IterativeParabolicTimeParameterization::~IterativeParabolicTimeParameterization()
00055 {
00056 }
00057
00058 namespace
00059 {
00060 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
00061 {
00062 logDebug(" time [%i]= %f", i, point.time_from_start.toSec());
00063 if (point.positions.size() >= 7)
00064 {
00065 logDebug(" pos_ [%i]= %f %f %f %f %f %f %f", i, point.positions[0], point.positions[1], point.positions[2],
00066 point.positions[3], point.positions[4], point.positions[5], point.positions[6]);
00067 }
00068 if (point.velocities.size() >= 7)
00069 {
00070 logDebug(" vel_ [%i]= %f %f %f %f %f %f %f", i, point.velocities[0], point.velocities[1], point.velocities[2],
00071 point.velocities[3], point.velocities[4], point.velocities[5], point.velocities[6]);
00072 }
00073 if (point.accelerations.size() >= 7)
00074 {
00075 logDebug(" acc_ [%i]= %f %f %f %f %f %f %f", i, point.accelerations[0], point.accelerations[1],
00076 point.accelerations[2], point.accelerations[3], point.accelerations[4], point.accelerations[5],
00077 point.accelerations[6]);
00078 }
00079 }
00080
00081 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
00082 {
00083 logDebug("jointNames= %s %s %s %s %s %s %s", limits[0].joint_name.c_str(), limits[1].joint_name.c_str(),
00084 limits[2].joint_name.c_str(), limits[3].joint_name.c_str(), limits[4].joint_name.c_str(),
00085 limits[5].joint_name.c_str(), limits[6].joint_name.c_str());
00086 logDebug("maxVelocities= %f %f %f %f %f %f %f", limits[0].max_velocity, limits[1].max_velocity,
00087 limits[2].max_velocity, limits[3].max_velocity, limits[4].max_velocity, limits[5].max_velocity,
00088 limits[6].max_velocity);
00089 logDebug("maxAccelerations= %f %f %f %f %f %f %f", limits[0].max_acceleration, limits[1].max_acceleration,
00090 limits[2].max_acceleration, limits[3].max_acceleration, limits[4].max_acceleration,
00091 limits[5].max_acceleration, limits[6].max_acceleration);
00092
00093 for (std::size_t i = 0; i < trajectory.points.size(); ++i)
00094 printPoint(trajectory.points[i], i);
00095 }
00096 }
00097
00098
00099 void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
00100 std::vector<double>& time_diff,
00101 const double max_velocity_scaling_factor) const
00102 {
00103 const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00104 const std::vector<std::string>& vars = group->getVariableNames();
00105 const std::vector<int>& idx = group->getVariableIndexList();
00106 const robot_model::RobotModel& rmodel = group->getParentModel();
00107 const int num_points = rob_trajectory.getWayPointCount();
00108
00109 double velocity_scaling_factor = 1.0;
00110
00111 if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
00112 velocity_scaling_factor = max_velocity_scaling_factor;
00113 else if (max_velocity_scaling_factor == 0.0)
00114 logDebug("A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", velocity_scaling_factor);
00115 else
00116 logWarn("Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", max_velocity_scaling_factor,
00117 velocity_scaling_factor);
00118
00119 for (int i = 0; i < num_points - 1; ++i)
00120 {
00121 const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
00122 const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
00123
00124 for (std::size_t j = 0; j < vars.size(); ++j)
00125 {
00126 double v_max = DEFAULT_VEL_MAX;
00127 const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
00128 if (b.velocity_bounded_)
00129 v_max =
00130 std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor));
00131 const double dq1 = curr_waypoint->getVariablePosition(idx[j]);
00132 const double dq2 = next_waypoint->getVariablePosition(idx[j]);
00133 const double t_min = std::abs(dq2 - dq1) / v_max;
00134 if (t_min > time_diff[i])
00135 time_diff[i] = t_min;
00136 }
00137 }
00138 }
00139
00140
00141
00142
00143 double IterativeParabolicTimeParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2,
00144 const double a_max) const
00145 {
00146 const double mult_factor = 1.01;
00147 double v1 = (dq1) / dt1;
00148 double v2 = (dq2) / dt2;
00149 double a = 2.0 * (v2 - v1) / (dt1 + dt2);
00150
00151 while (std::abs(a) > a_max)
00152 {
00153 v1 = (dq1) / dt1;
00154 v2 = (dq2) / dt2;
00155 a = 2.0 * (v2 - v1) / (dt1 + dt2);
00156 dt1 *= mult_factor;
00157 }
00158
00159 return dt1;
00160 }
00161
00162 double IterativeParabolicTimeParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2,
00163 const double a_max) const
00164 {
00165 const double mult_factor = 1.01;
00166 double v1 = (dq1) / dt1;
00167 double v2 = (dq2) / dt2;
00168 double a = 2.0 * (v2 - v1) / (dt1 + dt2);
00169
00170 while (std::abs(a) > a_max)
00171 {
00172 v1 = (dq1) / dt1;
00173 v2 = (dq2) / dt2;
00174 a = 2.0 * (v2 - v1) / (dt1 + dt2);
00175 dt2 *= mult_factor;
00176 }
00177
00178 return dt2;
00179 }
00180
00181 namespace
00182 {
00183
00184
00185 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
00186 {
00187
00188 if (time_diff.empty())
00189 return;
00190
00191 double time_sum = 0.0;
00192
00193 robot_state::RobotStatePtr prev_waypoint;
00194 robot_state::RobotStatePtr curr_waypoint;
00195 robot_state::RobotStatePtr next_waypoint;
00196
00197 const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00198 const std::vector<std::string>& vars = group->getVariableNames();
00199 const std::vector<int>& idx = group->getVariableIndexList();
00200
00201 int num_points = rob_trajectory.getWayPointCount();
00202
00203 rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);
00204
00205
00206 for (int i = 1; i < num_points; ++i)
00207
00208 rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
00209
00210
00211 if (num_points <= 1)
00212 return;
00213
00214
00215 for (int i = 0; i < num_points; ++i)
00216 {
00217 curr_waypoint = rob_trajectory.getWayPointPtr(i);
00218
00219 if (i > 0)
00220 prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);
00221
00222 if (i < num_points - 1)
00223 next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
00224
00225 for (std::size_t j = 0; j < vars.size(); ++j)
00226 {
00227 double q1;
00228 double q2;
00229 double q3;
00230 double dt1;
00231 double dt2;
00232
00233 if (i == 0)
00234 {
00235
00236 q1 = next_waypoint->getVariablePosition(idx[j]);
00237 q2 = curr_waypoint->getVariablePosition(idx[j]);
00238 q3 = q1;
00239
00240 dt1 = dt2 = time_diff[i];
00241 }
00242 else if (i < num_points - 1)
00243 {
00244
00245 q1 = prev_waypoint->getVariablePosition(idx[j]);
00246 q2 = curr_waypoint->getVariablePosition(idx[j]);
00247 q3 = next_waypoint->getVariablePosition(idx[j]);
00248
00249 dt1 = time_diff[i - 1];
00250 dt2 = time_diff[i];
00251 }
00252 else
00253 {
00254
00255 q1 = prev_waypoint->getVariablePosition(idx[j]);
00256 q2 = curr_waypoint->getVariablePosition(idx[j]);
00257 q3 = q1;
00258
00259 dt1 = dt2 = time_diff[i - 1];
00260 }
00261
00262 double v1, v2, a;
00263
00264 bool start_velocity = false;
00265 if (dt1 == 0.0 || dt2 == 0.0)
00266 {
00267 v1 = 0.0;
00268 v2 = 0.0;
00269 a = 0.0;
00270 }
00271 else
00272 {
00273 if (i == 0)
00274 {
00275 if (curr_waypoint->hasVelocities())
00276 {
00277 start_velocity = true;
00278 v1 = curr_waypoint->getVariableVelocity(idx[j]);
00279 }
00280 }
00281 v1 = start_velocity ? v1 : (q2 - q1) / dt1;
00282
00283 v2 = start_velocity ? v1 : (q3 - q2) / dt2;
00284 a = 2.0 * (v2 - v1) / (dt1 + dt2);
00285 }
00286
00287 curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
00288 curr_waypoint->setVariableAcceleration(idx[j], a);
00289 }
00290 }
00291 }
00292 }
00293
00294
00295 void IterativeParabolicTimeParameterization::applyAccelerationConstraints(
00296 robot_trajectory::RobotTrajectory& rob_trajectory, std::vector<double>& time_diff,
00297 const double max_acceleration_scaling_factor) const
00298 {
00299 robot_state::RobotStatePtr prev_waypoint;
00300 robot_state::RobotStatePtr curr_waypoint;
00301 robot_state::RobotStatePtr next_waypoint;
00302
00303 const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00304 const std::vector<std::string>& vars = group->getVariableNames();
00305 const std::vector<int>& idx = group->getVariableIndexList();
00306 const robot_model::RobotModel& rmodel = group->getParentModel();
00307
00308 const int num_points = rob_trajectory.getWayPointCount();
00309 const unsigned int num_joints = group->getVariableCount();
00310 int num_updates = 0;
00311 int iteration = 0;
00312 bool backwards = false;
00313 double q1;
00314 double q2;
00315 double q3;
00316 double dt1;
00317 double dt2;
00318 double v1;
00319 double v2;
00320 double a;
00321
00322 double acceleration_scaling_factor = 1.0;
00323
00324 if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
00325 acceleration_scaling_factor = max_acceleration_scaling_factor;
00326 else if (max_acceleration_scaling_factor == 0.0)
00327 logDebug("A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
00328 acceleration_scaling_factor);
00329 else
00330 logWarn("Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
00331 max_acceleration_scaling_factor, acceleration_scaling_factor);
00332
00333 do
00334 {
00335 num_updates = 0;
00336 iteration++;
00337
00338
00339
00340 for (unsigned int j = 0; j < num_joints; ++j)
00341 {
00342
00343 for (int count = 0; count < 2; ++count)
00344 {
00345 for (int i = 0; i < num_points - 1; ++i)
00346 {
00347 int index = backwards ? (num_points - 1) - i : i;
00348
00349 curr_waypoint = rob_trajectory.getWayPointPtr(index);
00350
00351 if (index > 0)
00352 prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
00353
00354 if (index < num_points - 1)
00355 next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
00356
00357
00358 double a_max = DEFAULT_ACCEL_MAX;
00359 const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
00360 if (b.acceleration_bounded_)
00361 a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
00362 fabs(b.min_acceleration_ * acceleration_scaling_factor));
00363
00364 if (index == 0)
00365 {
00366
00367 q1 = next_waypoint->getVariablePosition(idx[j]);
00368 q2 = curr_waypoint->getVariablePosition(idx[j]);
00369 q3 = next_waypoint->getVariablePosition(idx[j]);
00370
00371 dt1 = dt2 = time_diff[index];
00372 assert(!backwards);
00373 }
00374 else if (index < num_points - 1)
00375 {
00376
00377 q1 = prev_waypoint->getVariablePosition(idx[j]);
00378 q2 = curr_waypoint->getVariablePosition(idx[j]);
00379 q3 = next_waypoint->getVariablePosition(idx[j]);
00380
00381 dt1 = time_diff[index - 1];
00382 dt2 = time_diff[index];
00383 }
00384 else
00385 {
00386
00387 q1 = prev_waypoint->getVariablePosition(idx[j]);
00388 q2 = curr_waypoint->getVariablePosition(idx[j]);
00389 q3 = prev_waypoint->getVariablePosition(idx[j]);
00390
00391 dt1 = dt2 = time_diff[index - 1];
00392 assert(backwards);
00393 }
00394
00395 if (dt1 == 0.0 || dt2 == 0.0)
00396 {
00397 v1 = 0.0;
00398 v2 = 0.0;
00399 a = 0.0;
00400 }
00401 else
00402 {
00403 bool start_velocity = false;
00404 if (index == 0)
00405 {
00406 if (curr_waypoint->hasVelocities())
00407 {
00408 start_velocity = true;
00409 v1 = curr_waypoint->getVariableVelocity(idx[j]);
00410 }
00411 }
00412 v1 = start_velocity ? v1 : (q2 - q1) / dt1;
00413 v2 = (q3 - q2) / dt2;
00414 a = 2.0 * (v2 - v1) / (dt1 + dt2);
00415 }
00416
00417 if (fabs(a) > a_max + ROUNDING_THRESHOLD)
00418 {
00419 if (!backwards)
00420 {
00421 dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
00422 time_diff[index] = dt2;
00423 }
00424 else
00425 {
00426 dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
00427 time_diff[index - 1] = dt1;
00428 }
00429 num_updates++;
00430
00431 if (dt1 == 0.0 || dt2 == 0.0)
00432 {
00433 v1 = 0.0;
00434 v2 = 0.0;
00435 a = 0.0;
00436 }
00437 else
00438 {
00439 v1 = (q2 - q1) / dt1;
00440 v2 = (q3 - q2) / dt2;
00441 a = 2 * (v2 - v1) / (dt1 + dt2);
00442 }
00443 }
00444 }
00445 backwards = !backwards;
00446 }
00447 }
00448
00449 } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
00450 }
00451
00452 bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
00453 const double max_velocity_scaling_factor,
00454 const double max_acceleration_scaling_factor) const
00455 {
00456 if (trajectory.empty())
00457 return true;
00458
00459 const robot_model::JointModelGroup* group = trajectory.getGroup();
00460 if (!group)
00461 {
00462 logError("It looks like the planner did not set the group the plan was computed for");
00463 return false;
00464 }
00465
00466
00467 trajectory.unwind();
00468
00469 const int num_points = trajectory.getWayPointCount();
00470 std::vector<double> time_diff(num_points - 1, 0.0);
00471
00472 applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
00473 applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
00474
00475 updateTrajectory(trajectory, time_diff);
00476 return true;
00477 }
00478 }