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
00037 #ifndef ITERATIVE_PARABOLIC_SMOOTHER_H_
00038 #define ITERATIVE_PARABOLIC_SMOOTHER_H_
00039
00040 #include <spline_smoother/spline_smoother.h>
00041 #include <trajectory_msgs/JointTrajectoryPoint.h>
00042 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00043 #include <arm_navigation_msgs/JointLimits.h>
00044
00045
00046 const double DEFAULT_VEL_MAX=1.0;
00047 const double DEFAULT_ACCEL_MAX=1.0;
00048 const double ROUNDING_THRESHOLD = 0.01;
00049
00050
00053 template <typename T>
00054 class IterativeParabolicSmoother : public spline_smoother::SplineSmoother<T>
00055 {
00056 public:
00057 IterativeParabolicSmoother();
00058 ~IterativeParabolicSmoother();
00059
00061 virtual bool configure();
00062
00065 virtual bool smooth(const T& trajectory_in, T& trajectory_out) const;
00066
00067 private:
00068 int max_iterations_;
00069 double max_time_change_per_it_;
00070
00071 void applyVelocityConstraints(T& trajectory, std::vector<double> &time_diff) const;
00072 void applyAccelerationConstraints(const T& trajectory, std::vector<double> & time_diff) const;
00073 double findT1( const double d1, const double d2, double t1, const double t2, const double a_max) const;
00074 double findT2( const double d1, const double d2, const double t1, double t2, const double a_max) const;
00075 void printStats(const T& trajectory) const;
00076 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, unsigned int i) const;
00077 };
00078
00079 template <typename T>
00080 IterativeParabolicSmoother<T>::IterativeParabolicSmoother()
00081 : max_iterations_(100),
00082 max_time_change_per_it_(0.01)
00083 {}
00084
00085 template <typename T>
00086 IterativeParabolicSmoother<T>::~IterativeParabolicSmoother()
00087 {}
00088
00089 template <typename T>
00090 bool IterativeParabolicSmoother<T>::configure()
00091 {
00092 if (!spline_smoother::SplineSmoother<T>::getParam("max_iterations", max_iterations_))
00093 {
00094 ROS_WARN("Spline smoother, \"%s\", params has no attribute max_iterations.",
00095 spline_smoother::SplineSmoother<T>::getName().c_str());
00096 }
00097 ROS_DEBUG("Using a max_iterations value of %d",max_iterations_);
00098
00099 if (!spline_smoother::SplineSmoother<T>::getParam("max_time_change_per_it", max_time_change_per_it_))
00100 {
00101 ROS_WARN("Spline smoother, \"%s\", params has no attribute max_time_change_per_it.",
00102 spline_smoother::SplineSmoother<T>::getName().c_str());
00103 }
00104 ROS_DEBUG("Using a max_time_change_per_it value of %f",max_time_change_per_it_);
00105
00106 return true;
00107 }
00108
00109 template <typename T>
00110 void IterativeParabolicSmoother<T>::printPoint(const trajectory_msgs::JointTrajectoryPoint& point, unsigned int i) const
00111 {
00112 ROS_DEBUG( " time [%i]= %f",i,point.time_from_start.toSec());
00113 if(point.positions.size() >= 7 )
00114 {
00115 ROS_DEBUG(" pos_ [%i]= %f %f %f %f %f %f %f",i,
00116 point.positions[0],point.positions[1],point.positions[2],point.positions[3],point.positions[4],point.positions[5],point.positions[6]);
00117 }
00118 if(point.velocities.size() >= 7 )
00119 {
00120 ROS_DEBUG(" vel_ [%i]= %f %f %f %f %f %f %f",i,
00121 point.velocities[0],point.velocities[1],point.velocities[2],point.velocities[3],point.velocities[4],point.velocities[5],point.velocities[6]);
00122 }
00123 if(point.accelerations.size() >= 7 )
00124 {
00125 ROS_DEBUG(" acc_ [%i]= %f %f %f %f %f %f %f",i,
00126 point.accelerations[0],point.accelerations[1],point.accelerations[2],point.accelerations[3],point.accelerations[4],point.accelerations[5],point.accelerations[6]);
00127 }
00128 }
00129
00130 template <typename T>
00131 void IterativeParabolicSmoother<T>::printStats(const T& trajectory) const
00132 {
00133 ROS_DEBUG("jointNames= %s %s %s %s %s %s %s",
00134 trajectory.limits[0].joint_name.c_str(),trajectory.limits[1].joint_name.c_str(),trajectory.limits[2].joint_name.c_str(),
00135 trajectory.limits[3].joint_name.c_str(),trajectory.limits[4].joint_name.c_str(),trajectory.limits[5].joint_name.c_str(),
00136 trajectory.limits[6].joint_name.c_str());
00137 ROS_DEBUG("maxVelocities= %f %f %f %f %f %f %f",
00138 trajectory.limits[0].max_velocity,trajectory.limits[1].max_velocity,trajectory.limits[2].max_velocity,
00139 trajectory.limits[3].max_velocity,trajectory.limits[4].max_velocity,trajectory.limits[5].max_velocity,
00140 trajectory.limits[6].max_velocity);
00141 ROS_DEBUG("maxAccelerations= %f %f %f %f %f %f %f",
00142 trajectory.limits[0].max_acceleration,trajectory.limits[1].max_acceleration,trajectory.limits[2].max_acceleration,
00143 trajectory.limits[3].max_acceleration,trajectory.limits[4].max_acceleration,trajectory.limits[5].max_acceleration,
00144 trajectory.limits[6].max_acceleration);
00145
00146 for (unsigned int i=0; i<trajectory.trajectory.points.size(); ++i)
00147 {
00148 const trajectory_msgs::JointTrajectoryPoint& point = trajectory.trajectory.points[i];
00149 printPoint(point, i);
00150 }
00151 }
00152
00153
00154 template <typename T>
00155 void IterativeParabolicSmoother<T>::applyVelocityConstraints(T& trajectory, std::vector<double> &time_diff) const
00156 {
00157 const unsigned int num_points = trajectory.trajectory.points.size();
00158 const unsigned int num_joints = trajectory.trajectory.joint_names.size();
00159
00160
00161 for (unsigned int i=0; i<num_points; ++i)
00162 {
00163 trajectory_msgs::JointTrajectoryPoint& point = trajectory.trajectory.points[i];
00164 point.velocities.resize(num_joints);
00165 point.accelerations.resize(num_joints);
00166 }
00167
00168 for (unsigned int i=0; i<num_points-1; ++i)
00169 {
00170 trajectory_msgs::JointTrajectoryPoint& point1 = trajectory.trajectory.points[i];
00171 trajectory_msgs::JointTrajectoryPoint& point2 = trajectory.trajectory.points[i+1];
00172
00173
00174 for (unsigned int j=0; j<num_joints; ++j)
00175 {
00176 double v_max = 1.0;
00177
00178 if( trajectory.limits[j].has_velocity_limits )
00179 {
00180 v_max = trajectory.limits[j].max_velocity;
00181 }
00182 double a_max = 1.0;
00183 if( trajectory.limits[j].has_velocity_limits )
00184 {
00185 a_max = trajectory.limits[j].max_acceleration;
00186 }
00187
00188 const double dq1 = point1.positions[j];
00189 const double dq2 = point2.positions[j];
00190 const double t_min = std::abs(dq2-dq1) / v_max;
00191
00192 if( t_min > time_diff[i] )
00193 {
00194 time_diff[i] = t_min;
00195 }
00196 }
00197 }
00198 }
00199
00200
00201
00202
00203 template <typename T>
00204 double IterativeParabolicSmoother<T>::findT1( const double dq1, const double dq2, double dt1, const double dt2, const double a_max) const
00205 {
00206 const double mult_factor = 1.01;
00207 double v1 = (dq1)/dt1;
00208 double v2 = (dq2)/dt2;
00209 double a = 2*(v2-v1)/(dt1+dt2);
00210
00211 while( std::abs( a ) > a_max )
00212 {
00213 v1 = (dq1)/dt1;
00214 v2 = (dq2)/dt2;
00215 a = 2*(v2-v1)/(dt1+dt2);
00216 dt1 *= mult_factor;
00217 }
00218
00219 return dt1;
00220 }
00221
00222 template <typename T>
00223 double IterativeParabolicSmoother<T>::findT2( const double dq1, const double dq2, const double dt1, double dt2, const double a_max) const
00224 {
00225 const double mult_factor = 1.01;
00226 double v1 = (dq1)/dt1;
00227 double v2 = (dq2)/dt2;
00228 double a = 2*(v2-v1)/(dt1+dt2);
00229
00230 while( std::abs( a ) > a_max )
00231 {
00232 v1 = (dq1)/dt1;
00233 v2 = (dq2)/dt2;
00234 a = 2*(v2-v1)/(dt1+dt2);
00235 dt2 *= mult_factor;
00236 }
00237
00238 return dt2;
00239 }
00240
00241
00242 template <typename T>
00243 void updateTrajectory(T& trajectory, const std::vector<double>& time_diff )
00244 {
00245 double time_sum = 0.0;
00246 unsigned int num_joints = trajectory.trajectory.joint_names.size();
00247 const unsigned int num_points = trajectory.trajectory.points.size();
00248
00249
00250 if(time_diff.size() < 1)
00251 return;
00252
00253
00254 trajectory.trajectory.points[0].time_from_start = ros::Duration(0);
00255 for (unsigned int i=1; i<num_points; ++i)
00256 {
00257 time_sum += time_diff[i-1];
00258 trajectory.trajectory.points[i].time_from_start = ros::Duration(time_sum);
00259 }
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282 for (unsigned int i=0; i<num_points; ++i)
00283 {
00284 for (unsigned int j=0; j<num_joints; ++j)
00285 {
00286 double q1;
00287 double q2;
00288 double q3;
00289 double dt1;
00290 double dt2;
00291
00292 if(i==0)
00293 {
00294 q1 = trajectory.trajectory.points[i+1].positions[j];
00295 q2 = trajectory.trajectory.points[i].positions[j];
00296 q3 = trajectory.trajectory.points[i+1].positions[j];
00297 dt1 = time_diff[i];
00298 dt2 = time_diff[i];
00299 }
00300 else if(i < num_points-1)
00301 {
00302 q1 = trajectory.trajectory.points[i-1].positions[j];
00303 q2 = trajectory.trajectory.points[i].positions[j];
00304 q3 = trajectory.trajectory.points[i+1].positions[j];
00305 dt1 = time_diff[i-1];
00306 dt2 = time_diff[i];
00307 }
00308 else
00309 {
00310 q1 = trajectory.trajectory.points[i-1].positions[j];
00311 q2 = trajectory.trajectory.points[i].positions[j];
00312 q3 = trajectory.trajectory.points[i-1].positions[j];
00313 dt1 = time_diff[i-1];
00314 dt2 = time_diff[i-1];
00315 }
00316
00317 const double v1 = (q2-q1)/dt1;
00318 const double v2 = (q3-q2)/dt2;
00319 const double a = 2*(v2-v1)/(dt1+dt2);
00320 trajectory.trajectory.points[i].velocities[j] = (v2+v1)/2;
00321 trajectory.trajectory.points[i].accelerations[j] = a;
00322 }
00323 }
00324 }
00325
00326
00327
00328 template <typename T>
00329 void IterativeParabolicSmoother<T>::applyAccelerationConstraints(const T& trajectory, std::vector<double> & time_diff) const
00330 {
00331 const unsigned int num_points = trajectory.trajectory.points.size();
00332 const unsigned int num_joints = trajectory.trajectory.joint_names.size();
00333 int num_updates = 0;
00334 int iteration= 0;
00335 bool backwards = false;
00336 double q1;
00337 double q2;
00338 double q3;
00339 double dt1;
00340 double dt2;
00341 double v1;
00342 double v2;
00343 double a;
00344
00345 do
00346 {
00347 num_updates = 0;
00348 iteration++;
00349
00350
00351
00352 for (unsigned int j=0; j<num_joints; ++j)
00353 {
00354
00355 for( int count=0; count<2; count++)
00356 {
00357 ROS_DEBUG("applyAcceleration: Iteration %i backwards=%i joint=%i", iteration, backwards, j);
00358
00359
00360
00361 for (unsigned int i=0; i<num_points-1; ++i)
00362 {
00363 unsigned int index = i;
00364 if(backwards)
00365 {
00366 index = (num_points-1)-i;
00367 }
00368
00369
00370 double a_max = 1.0;
00371 if( trajectory.limits[j].has_acceleration_limits )
00372 {
00373 a_max = trajectory.limits[j].max_acceleration;
00374 }
00375
00376 if(index==0)
00377 {
00378 q1 = trajectory.trajectory.points[index+1].positions[j];
00379 q2 = trajectory.trajectory.points[index].positions[j];
00380 q3 = trajectory.trajectory.points[index+1].positions[j];
00381 dt1 = time_diff[index];
00382 dt2 = time_diff[index];
00383 ROS_ASSERT(!backwards);
00384 }
00385 else if(index < num_points-1)
00386 {
00387 q1 = trajectory.trajectory.points[index-1].positions[j];
00388 q2 = trajectory.trajectory.points[index].positions[j];
00389 q3 = trajectory.trajectory.points[index+1].positions[j];
00390 dt1 = time_diff[index-1];
00391 dt2 = time_diff[index];
00392 }
00393 else
00394 {
00395 q1 = trajectory.trajectory.points[index-1].positions[j];
00396 q2 = trajectory.trajectory.points[index].positions[j];
00397 q3 = trajectory.trajectory.points[index-1].positions[j];
00398 dt1 = time_diff[index-1];
00399 dt2 = time_diff[index-1];
00400 ROS_ASSERT(backwards);
00401 }
00402
00403 v1 = (q2-q1)/dt1;
00404 v2 = (q3-q2)/dt2;
00405 a = 2*(v2-v1)/(dt1+dt2);
00406
00407 if( std::abs( a ) > a_max + ROUNDING_THRESHOLD )
00408 {
00409 if(!backwards)
00410 {
00411 dt2 = std::min( dt2+max_time_change_per_it_, findT2( q2-q1, q3-q2, dt1, dt2, a_max) );
00412 time_diff[index] = dt2;
00413 }
00414 else
00415 {
00416 dt1 = std::min( dt1+max_time_change_per_it_, findT1( q2-q1, q3-q2, dt1, dt2, a_max) );
00417 time_diff[index-1] = dt1;
00418 }
00419 num_updates++;
00420
00421 v1 = (q2-q1)/dt1;
00422 v2 = (q3-q2)/dt2;
00423 a = 2*(v2-v1)/(dt1+dt2);
00424 }
00425 }
00426 backwards = !backwards;
00427 }
00428 }
00429 ROS_DEBUG("applyAcceleration: num_updates=%i", num_updates);
00430 } while(num_updates > 0 && iteration < max_iterations_);
00431 }
00432
00433 template <typename T>
00434 bool IterativeParabolicSmoother<T>::smooth(const T& trajectory_in,
00435 T& trajectory_out) const
00436 {
00437 bool success = true;
00438 trajectory_out = trajectory_in;
00439 const unsigned int num_points = trajectory_out.trajectory.points.size();
00440 std::vector<double> time_diff(num_points,0.0);
00441
00442 applyVelocityConstraints(trajectory_out, time_diff);
00443 applyAccelerationConstraints(trajectory_out, time_diff);
00444
00445 ROS_DEBUG("Velocity & Acceleration-Constrained Trajectory");
00446 updateTrajectory(trajectory_out, time_diff);
00447 printStats(trajectory_out);
00448
00449 return success;
00450 }
00451
00452
00453
00454 #endif