iterative_smoother.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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 #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   // for every point in time:
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 // Applies velocity
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   // Initial values
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     // Get velocity min/max
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 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
00201 // In the future we may want to solve to quadratic equation to get the exact timing interval.
00202 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
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 // Takes the time differences, and updates the values in the trajectory.
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     // Error check
00250   if(time_diff.size() < 1)
00251         return;
00252 
00253   // Times
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   // Velocities
00262 /*
00263   for (unsigned int j=0; j<num_joints; ++j)
00264   {
00265     trajectory.trajectory.points[num_points-1].velocities[j] = 0.0;
00266   }
00267   for (unsigned int i=0; i<num_points-1; ++i)
00268   {
00269     trajectory_msgs::JointTrajectoryPoint& point1 = trajectory.trajectory.points[i];
00270     trajectory_msgs::JointTrajectoryPoint& point2 = trajectory.trajectory.points[i+1];
00271     for (unsigned int j=0; j<num_joints; ++j)
00272     {
00273       const double dq1 = point1.positions[j];
00274       const double dq2 = point2.positions[j];
00275       const double & dt1 = time_diff[i];
00276       const double v1 = (dq2-dq1)/(dt1);
00277       point1.velocities[j]= v1;
00278     }
00279   }
00280 */
00281   // Accelerations
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       { // First point
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       { // middle points
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       { // last point
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 // Applies Acceleration constraints
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     // In this case we iterate through the joints on the outer loop.
00351     // This is so that any time interval increases have a chance to get propogated through the trajectory
00352     for (unsigned int j=0; j<num_joints; ++j)
00353     {
00354       // Loop forwards, then backwards
00355       for( int count=0; count<2; count++)
00356       {
00357         ROS_DEBUG("applyAcceleration: Iteration %i backwards=%i joint=%i", iteration, backwards, j);
00358         //updateTrajectory(trajectory, time_diff);
00359         //printStats(trajectory);
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           // Get acceleration limits
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           { // First point
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           { // middle points
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           { // last point - careful, there are only numpoints-1 time intervals
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;   //copy
00439   const unsigned int num_points = trajectory_out.trajectory.points.size();
00440   std::vector<double> time_diff(num_points,0.0);    // the time difference between adjacent points
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


coverage_3d_arm_navigation
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:56