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