iterative_smoother.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <constraint_aware_spline_smoother/iterative_smoother.h>
00038 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00039 #include <arm_navigation_msgs/JointLimits.h>
00040 
00041 using namespace constraint_aware_spline_smoother;
00042 
00043 const double DEFAULT_VEL_MAX=1.0;
00044 const double DEFAULT_ACCEL_MAX=1.0;
00045 const double ROUNDING_THRESHOLD = 0.01;
00046 
00047 
00048 template <typename T>
00049 IterativeParabolicSmoother<T>::IterativeParabolicSmoother()
00050 : max_iterations_(100),
00051   max_time_change_per_it_(0.01)
00052 {}
00053 
00054 template <typename T>
00055 IterativeParabolicSmoother<T>::~IterativeParabolicSmoother()
00056 {}
00057 
00058 template <typename T>
00059 bool IterativeParabolicSmoother<T>::configure()
00060 {
00061   if (!spline_smoother::SplineSmoother<T>::getParam("max_iterations", max_iterations_))
00062   {
00063     ROS_WARN("Spline smoother, \"%s\", params has no attribute max_iterations.",
00064             spline_smoother::SplineSmoother<T>::getName().c_str());
00065   }
00066   ROS_DEBUG("Using a max_iterations value of %d",max_iterations_);
00067 
00068   if (!spline_smoother::SplineSmoother<T>::getParam("max_time_change_per_it", max_time_change_per_it_))
00069   {
00070     ROS_WARN("Spline smoother, \"%s\", params has no attribute max_time_change_per_it.",
00071             spline_smoother::SplineSmoother<T>::getName().c_str());
00072   }
00073   ROS_DEBUG("Using a max_time_change_per_it value of %f",max_time_change_per_it_);
00074 
00075  return true;
00076 }
00077 
00078 template <typename T>
00079 void IterativeParabolicSmoother<T>::printPoint(const trajectory_msgs::JointTrajectoryPoint& point, unsigned int i) const
00080 {
00081     ROS_DEBUG(  " time   [%i]= %f",i,point.time_from_start.toSec());
00082     if(point.positions.size() >= 7 )
00083     {
00084       ROS_DEBUG(" pos_   [%i]= %f %f %f %f %f %f %f",i,
00085         point.positions[0],point.positions[1],point.positions[2],point.positions[3],point.positions[4],point.positions[5],point.positions[6]);
00086     }
00087     if(point.velocities.size() >= 7 )
00088     {
00089       ROS_DEBUG("  vel_  [%i]= %f %f %f %f %f %f %f",i,
00090         point.velocities[0],point.velocities[1],point.velocities[2],point.velocities[3],point.velocities[4],point.velocities[5],point.velocities[6]);
00091     }
00092     if(point.accelerations.size() >= 7 )
00093     {
00094       ROS_DEBUG("   acc_ [%i]= %f %f %f %f %f %f %f",i,
00095         point.accelerations[0],point.accelerations[1],point.accelerations[2],point.accelerations[3],point.accelerations[4],point.accelerations[5],point.accelerations[6]);
00096     }
00097 }
00098 
00099 template <typename T>
00100 void IterativeParabolicSmoother<T>::printStats(const T& trajectory) const
00101 {
00102   ROS_DEBUG("jointNames= %s %s %s %s %s %s %s",
00103     trajectory.request.limits[0].joint_name.c_str(),trajectory.request.limits[1].joint_name.c_str(),trajectory.request.limits[2].joint_name.c_str(),
00104     trajectory.request.limits[3].joint_name.c_str(),trajectory.request.limits[4].joint_name.c_str(),trajectory.request.limits[5].joint_name.c_str(),
00105     trajectory.request.limits[6].joint_name.c_str());
00106   ROS_DEBUG("maxVelocities= %f %f %f %f %f %f %f",
00107     trajectory.request.limits[0].max_velocity,trajectory.request.limits[1].max_velocity,trajectory.request.limits[2].max_velocity,
00108     trajectory.request.limits[3].max_velocity,trajectory.request.limits[4].max_velocity,trajectory.request.limits[5].max_velocity,
00109     trajectory.request.limits[6].max_velocity);
00110   ROS_DEBUG("maxAccelerations= %f %f %f %f %f %f %f",
00111     trajectory.request.limits[0].max_acceleration,trajectory.request.limits[1].max_acceleration,trajectory.request.limits[2].max_acceleration,
00112     trajectory.request.limits[3].max_acceleration,trajectory.request.limits[4].max_acceleration,trajectory.request.limits[5].max_acceleration,
00113     trajectory.request.limits[6].max_acceleration);
00114   // for every point in time:
00115   for (unsigned int i=0; i<trajectory.request.trajectory.points.size(); ++i)
00116   {
00117     const trajectory_msgs::JointTrajectoryPoint& point = trajectory.request.trajectory.points[i];
00118     printPoint(point, i);
00119   }
00120 }
00121 
00122 // Applies velocity
00123 template <typename T>
00124 void IterativeParabolicSmoother<T>::applyVelocityConstraints(T& trajectory, std::vector<double> &time_diff) const
00125 {
00126   const unsigned int num_points = trajectory.request.trajectory.points.size();
00127   const unsigned int num_joints = trajectory.request.trajectory.joint_names.size();
00128 
00129   // Initial values
00130   for (unsigned int i=0; i<num_points; ++i)
00131   {
00132     trajectory_msgs::JointTrajectoryPoint& point = trajectory.request.trajectory.points[i];
00133     point.velocities.resize(num_joints);
00134     point.accelerations.resize(num_joints);
00135   }
00136 
00137   for (unsigned int i=0; i<num_points-1; ++i)
00138   {
00139     trajectory_msgs::JointTrajectoryPoint& point1 = trajectory.request.trajectory.points[i];
00140     trajectory_msgs::JointTrajectoryPoint& point2 = trajectory.request.trajectory.points[i+1];
00141 
00142     // Get velocity min/max
00143     for (unsigned int j=0; j<num_joints; ++j)
00144     {
00145       double v_max = 1.0;
00146 
00147       if( trajectory.request.limits[j].has_velocity_limits )
00148       {
00149         v_max = trajectory.request.limits[j].max_velocity;
00150       }
00151       double a_max = 1.0;
00152       if( trajectory.request.limits[j].has_velocity_limits )
00153       {
00154         a_max = trajectory.request.limits[j].max_acceleration;
00155       }
00156 
00157       const double dq1 = point1.positions[j];
00158       const double dq2 = point2.positions[j];
00159       const double t_min = std::abs(dq2-dq1) / v_max;
00160 
00161       if( t_min > time_diff[i] )
00162       {
00163         time_diff[i] = t_min;
00164       }
00165     }
00166   }
00167 }
00168 
00169 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
00170 // In the future we may want to solve to quadratic equation to get the exact timing interval.
00171 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
00172 template <typename T>
00173 double IterativeParabolicSmoother<T>::findT1( const double dq1, const double dq2, double dt1, const double dt2, const double a_max) const
00174 {
00175   const double mult_factor = 1.01;
00176   double v1 = (dq1)/dt1;
00177   double v2 = (dq2)/dt2;
00178   double a = 2*(v2-v1)/(dt1+dt2);
00179 
00180   while( std::abs( a ) > a_max )
00181   {
00182     v1 = (dq1)/dt1;
00183     v2 = (dq2)/dt2;
00184     a = 2*(v2-v1)/(dt1+dt2);
00185     dt1 *= mult_factor;
00186   }
00187 
00188   return dt1;
00189 }
00190 
00191 template <typename T>
00192 double IterativeParabolicSmoother<T>::findT2( const double dq1, const double dq2, const double dt1, double dt2, const double a_max) const
00193 {
00194   const double mult_factor = 1.01;
00195   double v1 = (dq1)/dt1;
00196   double v2 = (dq2)/dt2;
00197   double a = 2*(v2-v1)/(dt1+dt2);
00198 
00199   while( std::abs( a ) > a_max )
00200   {
00201     v1 = (dq1)/dt1;
00202     v2 = (dq2)/dt2;
00203     a = 2*(v2-v1)/(dt1+dt2);
00204     dt2 *= mult_factor;
00205   }
00206 
00207   return dt2;
00208 }
00209 
00210 // Takes the time differences, and updates the values in the trajectory.
00211 template <typename T>
00212 void updateTrajectory(T& trajectory, const std::vector<double>& time_diff )
00213 {
00214   double time_sum = 0.0;
00215   unsigned int num_joints = trajectory.request.trajectory.joint_names.size();
00216   const unsigned int num_points = trajectory.request.trajectory.points.size();
00217 
00218         // Error check
00219   if(time_diff.size() < 1)
00220                 return;
00221 
00222   // Times
00223   trajectory.request.trajectory.points[0].time_from_start = ros::Duration(0);
00224   for (unsigned int i=1; i<num_points; ++i)
00225   {
00226     time_sum += time_diff[i-1];
00227     trajectory.request.trajectory.points[i].time_from_start = ros::Duration(time_sum);
00228   }
00229 
00230   // Velocities
00231 /*
00232   for (unsigned int j=0; j<num_joints; ++j)
00233   {
00234     trajectory.request.trajectory.points[num_points-1].velocities[j] = 0.0;
00235   }
00236   for (unsigned int i=0; i<num_points-1; ++i)
00237   {
00238     trajectory_msgs::JointTrajectoryPoint& point1 = trajectory.request.trajectory.points[i];
00239     trajectory_msgs::JointTrajectoryPoint& point2 = trajectory.request.trajectory.points[i+1];
00240     for (unsigned int j=0; j<num_joints; ++j)
00241     {
00242       const double dq1 = point1.positions[j];
00243       const double dq2 = point2.positions[j];
00244       const double & dt1 = time_diff[i];
00245       const double v1 = (dq2-dq1)/(dt1);
00246       point1.velocities[j]= v1;
00247     }
00248   }
00249 */
00250   // Accelerations
00251   for (unsigned int i=0; i<num_points; ++i)
00252   {
00253     for (unsigned int j=0; j<num_joints; ++j)
00254     {
00255       double q1;
00256       double q2;
00257       double q3;
00258       double dt1;
00259       double dt2;
00260 
00261       if(i==0)
00262       { // First point
00263         q1 = trajectory.request.trajectory.points[i+1].positions[j];
00264         q2 = trajectory.request.trajectory.points[i].positions[j];
00265         q3 = trajectory.request.trajectory.points[i+1].positions[j];
00266         dt1 = time_diff[i];
00267         dt2 = time_diff[i];
00268       }
00269       else if(i < num_points-1)
00270       { // middle points
00271         q1 = trajectory.request.trajectory.points[i-1].positions[j];
00272         q2 = trajectory.request.trajectory.points[i].positions[j];
00273         q3 = trajectory.request.trajectory.points[i+1].positions[j];
00274         dt1 = time_diff[i-1];
00275         dt2 = time_diff[i];
00276       }
00277       else
00278       { // last point
00279         q1 = trajectory.request.trajectory.points[i-1].positions[j];
00280         q2 = trajectory.request.trajectory.points[i].positions[j];
00281         q3 = trajectory.request.trajectory.points[i-1].positions[j];
00282         dt1 = time_diff[i-1];
00283         dt2 = time_diff[i-1];
00284       }
00285 
00286       const double v1 = (q2-q1)/dt1;
00287       const double v2 = (q3-q2)/dt2;
00288       const double a = 2*(v2-v1)/(dt1+dt2);
00289       trajectory.request.trajectory.points[i].velocities[j] = (v2+v1)/2;
00290       trajectory.request.trajectory.points[i].accelerations[j] = a;
00291     }
00292   }
00293 }
00294 
00295 
00296 // Applies Acceleration constraints
00297 template <typename T>
00298 void IterativeParabolicSmoother<T>::applyAccelerationConstraints(const T& trajectory, std::vector<double> & time_diff) const
00299 {
00300   const unsigned int num_points = trajectory.request.trajectory.points.size();
00301   const unsigned int num_joints = trajectory.request.trajectory.joint_names.size();
00302   int num_updates = 0;
00303   int iteration= 0;
00304   bool backwards = false;
00305   double q1;
00306   double q2;
00307   double q3;
00308   double dt1;
00309   double dt2;
00310   double v1;
00311   double v2;
00312   double a;
00313 
00314   do
00315   {
00316     num_updates = 0;
00317     iteration++;
00318 
00319     // In this case we iterate through the joints on the outer loop.
00320     // This is so that any time interval increases have a chance to get propogated through the trajectory
00321     for (unsigned int j=0; j<num_joints; ++j)
00322     {
00323       // Loop forwards, then backwards
00324       for( int count=0; count<2; count++)
00325       {
00326         ROS_DEBUG("applyAcceleration: Iteration %i backwards=%i joint=%i", iteration, backwards, j);
00327         //updateTrajectory(trajectory, time_diff);
00328         //printStats(trajectory);
00329 
00330         for (unsigned int i=0; i<num_points-1; ++i)
00331         {
00332           unsigned int index = i;
00333           if(backwards)
00334           {
00335             index = (num_points-1)-i;
00336           }
00337 
00338           // Get acceleration limits
00339           double a_max = 1.0;
00340           if( trajectory.request.limits[j].has_acceleration_limits )
00341           {
00342             a_max = trajectory.request.limits[j].max_acceleration;
00343           }
00344 
00345           if(index==0)
00346           {     // First point
00347             q1 = trajectory.request.trajectory.points[index+1].positions[j];
00348             q2 = trajectory.request.trajectory.points[index].positions[j];
00349             q3 = trajectory.request.trajectory.points[index+1].positions[j];
00350             dt1 = time_diff[index];
00351             dt2 = time_diff[index];
00352             ROS_ASSERT(!backwards);
00353           }
00354           else if(index < num_points-1)
00355           { // middle points
00356             q1 = trajectory.request.trajectory.points[index-1].positions[j];
00357             q2 = trajectory.request.trajectory.points[index].positions[j];
00358             q3 = trajectory.request.trajectory.points[index+1].positions[j];
00359             dt1 = time_diff[index-1];
00360             dt2 = time_diff[index];
00361           }
00362           else
00363           { // last point - careful, there are only numpoints-1 time intervals
00364             q1 = trajectory.request.trajectory.points[index-1].positions[j];
00365             q2 = trajectory.request.trajectory.points[index].positions[j];
00366             q3 = trajectory.request.trajectory.points[index-1].positions[j];
00367             dt1 = time_diff[index-1];
00368             dt2 = time_diff[index-1];
00369             ROS_ASSERT(backwards);
00370           }
00371 
00372           v1 = (q2-q1)/dt1;
00373           v2 = (q3-q2)/dt2;
00374           a = 2*(v2-v1)/(dt1+dt2);
00375 
00376           if( std::abs( a ) > a_max + ROUNDING_THRESHOLD )
00377           {
00378             if(!backwards)
00379             {
00380               dt2 = std::min( dt2+max_time_change_per_it_, findT2( q2-q1, q3-q2, dt1, dt2, a_max) );
00381               time_diff[index] = dt2;
00382             }
00383             else
00384             {
00385               dt1 = std::min( dt1+max_time_change_per_it_, findT1( q2-q1, q3-q2, dt1, dt2, a_max) );
00386               time_diff[index-1] = dt1;
00387             }
00388             num_updates++;
00389 
00390             v1 = (q2-q1)/dt1;
00391             v2 = (q3-q2)/dt2;
00392             a = 2*(v2-v1)/(dt1+dt2);
00393           }
00394         }
00395         backwards = !backwards;
00396       }
00397     }
00398     ROS_DEBUG("applyAcceleration: num_updates=%i", num_updates);
00399   } while(num_updates > 0 && iteration < max_iterations_);
00400 }
00401 
00402 template <typename T>
00403 bool IterativeParabolicSmoother<T>::smooth(const T& trajectory_in,
00404                                    T& trajectory_out) const
00405 {
00406   bool success = true;
00407   trajectory_out = trajectory_in;       //copy
00408   const unsigned int num_points = trajectory_out.request.trajectory.points.size();
00409   std::vector<double> time_diff(num_points,0.0);        // the time difference between adjacent points
00410 
00411   applyVelocityConstraints(trajectory_out, time_diff);
00412   applyAccelerationConstraints(trajectory_out, time_diff);
00413 
00414   ROS_DEBUG("Velocity & Acceleration-Constrained Trajectory");
00415   updateTrajectory(trajectory_out, time_diff);
00416   printStats(trajectory_out);
00417 
00418   return success;
00419 }
00420 
00421 
00422 PLUGINLIB_DECLARE_CLASS(constraint_aware_spline_smoother,
00423                         IterativeParabolicSmootherFilterJointTrajectoryWithConstraints,
00424                         constraint_aware_spline_smoother::IterativeParabolicSmoother<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
00425                         filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>)


constraint_aware_spline_smoother
Author(s): Sachin Chitta
autogenerated on Mon Dec 2 2013 12:37:23