iterative_time_parameterization.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 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 
00035 /* Author: Ken Anderson */
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   // for every point in time:
00093   for (std::size_t i = 0; i < trajectory.points.size(); ++i)
00094     printPoint(trajectory.points[i], i);
00095 }
00096 }
00097 
00098 // Applies velocity
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 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
00141 // In the future we may want to solve to quadratic equation to get the exact timing interval.
00142 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
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 // Takes the time differences, and updates the timestamps, velocities and accelerations
00184 // in the trajectory.
00185 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
00186 {
00187   // Error check
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   // Times
00206   for (int i = 1; i < num_points; ++i)
00207     // Update the time between the waypoints in the robot_trajectory.
00208     rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
00209 
00210   // Return if there is only one point in the trajectory!
00211   if (num_points <= 1)
00212     return;
00213 
00214   // Accelerations
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         // First point
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         // middle points
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         // last point
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         // v2 = (q3-q2)/dt2;
00283         v2 = start_velocity ? v1 : (q3 - q2) / dt2;  // Needed to ensure continuous velocity for first point
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 // Applies Acceleration constraints
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     // In this case we iterate through the joints on the outer loop.
00339     // This is so that any time interval increases have a chance to get propogated through the trajectory
00340     for (unsigned int j = 0; j < num_joints; ++j)
00341     {
00342       // Loop forwards, then backwards
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           // Get acceleration limits
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             // First point
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             // middle points
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             // last point - careful, there are only numpoints-1 time intervals
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     // logDebug("applyAcceleration: num_updates=%i", num_updates);
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   // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
00467   trajectory.unwind();
00468 
00469   const int num_points = trajectory.getWayPointCount();
00470   std::vector<double> time_diff(num_points - 1, 0.0);  // the time difference between adjacent points
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49