spline_parameterization.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, Delft University of Technology
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 Delft University of Technology nor the names
00018  *     of its contributors may be used to endorse or promote products
00019  *     derived 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: Ruben Burger (r.b.burger@tudelft.nl)
00036  *
00037  * Based on iterative_time_parameterization.cpp
00038  */
00039 
00040 #include <moveit/trajectory_processing/spline_parameterization.h>
00041 #include <moveit/trajectory_processing/spline.h>
00042 #include <moveit_msgs/JointLimits.h>
00043 #include <console_bridge/console.h>
00044 #include <moveit/robot_state/conversions.h>
00045 
00046 namespace trajectory_processing
00047 {
00048 static const double DEFAULT_VEL_MAX = 1.0;
00049 static const double DEFAULT_ACCEL_MAX = 1.0;
00050 static const double ROUNDING_THRESHOLD = 0.01;
00051 
00052 SplineParameterization::SplineParameterization(unsigned int max_iterations, double max_time_change_per_it)
00053   : max_iterations_(max_iterations), max_time_change_per_it_(max_time_change_per_it)
00054 {
00055 }
00056 
00057 SplineParameterization::~SplineParameterization()
00058 {
00059 }
00060 
00061 namespace
00062 {
00063 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
00064 {
00065   logDebug(" time   [%i]= %f", i, point.time_from_start.toSec());
00066   if (point.positions.size() >= 7)
00067   {
00068     logDebug(" pos_   [%i]= %f %f %f %f %f %f %f", i, point.positions[0], point.positions[1], point.positions[2],
00069              point.positions[3], point.positions[4], point.positions[5], point.positions[6]);
00070   }
00071   if (point.velocities.size() >= 7)
00072   {
00073     logDebug("  vel_  [%i]= %f %f %f %f %f %f %f", i, point.velocities[0], point.velocities[1], point.velocities[2],
00074              point.velocities[3], point.velocities[4], point.velocities[5], point.velocities[6]);
00075   }
00076   if (point.accelerations.size() >= 7)
00077   {
00078     logDebug("   acc_ [%i]= %f %f %f %f %f %f %f", i, point.accelerations[0], point.accelerations[1],
00079              point.accelerations[2], point.accelerations[3], point.accelerations[4], point.accelerations[5],
00080              point.accelerations[6]);
00081   }
00082 }
00083 
00084 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
00085 {
00086   logDebug("jointNames= %s %s %s %s %s %s %s", limits[0].joint_name.c_str(), limits[1].joint_name.c_str(),
00087            limits[2].joint_name.c_str(), limits[3].joint_name.c_str(), limits[4].joint_name.c_str(),
00088            limits[5].joint_name.c_str(), limits[6].joint_name.c_str());
00089   logDebug("maxVelocities= %f %f %f %f %f %f %f", limits[0].max_velocity, limits[1].max_velocity,
00090            limits[2].max_velocity, limits[3].max_velocity, limits[4].max_velocity, limits[5].max_velocity,
00091            limits[6].max_velocity);
00092   logDebug("maxAccelerations= %f %f %f %f %f %f %f", limits[0].max_acceleration, limits[1].max_acceleration,
00093            limits[2].max_acceleration, limits[3].max_acceleration, limits[4].max_acceleration,
00094            limits[5].max_acceleration, limits[6].max_acceleration);
00095   // for every point in time:
00096   for (std::size_t i = 0; i < trajectory.points.size(); ++i)
00097     printPoint(trajectory.points[i], i);
00098 }
00099 }
00100 
00101 // Applies velocity
00102 void SplineParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
00103                                                       std::vector<double>& time_diff,
00104                                                       const double max_velocity_scaling_factor) const
00105 {
00106   const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00107   const std::vector<std::string>& vars = group->getVariableNames();
00108   const std::vector<int>& idx = group->getVariableIndexList();
00109   const robot_model::RobotModel& rmodel = group->getParentModel();
00110   const int num_points = rob_trajectory.getWayPointCount();
00111 
00112   double velocity_scaling_factor = 1.0;
00113 
00114   if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
00115     velocity_scaling_factor = max_velocity_scaling_factor;
00116   else if (max_velocity_scaling_factor == 0.0)
00117     logDebug("A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", velocity_scaling_factor);
00118   else
00119     logWarn("Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", max_velocity_scaling_factor,
00120             velocity_scaling_factor);
00121 
00122   for (int i = 0; i < num_points - 1; ++i)
00123   {
00124     const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
00125     const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
00126 
00127     for (std::size_t j = 0; j < vars.size(); ++j)
00128     {
00129       double v_max = DEFAULT_VEL_MAX;
00130       const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
00131       if (b.velocity_bounded_)
00132         v_max =
00133             std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor));
00134       const double dq1 = curr_waypoint->getVariablePosition(idx[j]);
00135       const double dq2 = next_waypoint->getVariablePosition(idx[j]);
00136       const double t_min = std::abs(dq2 - dq1) / v_max;
00137       if (t_min > time_diff[i])
00138         time_diff[i] = t_min;
00139     }
00140   }
00141 }
00142 
00143 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
00144 // In the future we may want to solve to quadratic equation to get the exact timing interval.
00145 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
00146 double SplineParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2,
00147                                       const double a_max) const
00148 {
00149   const double mult_factor = 1.01;
00150   double v1 = (dq1) / dt1;
00151   double v2 = (dq2) / dt2;
00152   double a = 2.0 * (v2 - v1) / (dt1 + dt2);
00153 
00154   while (std::abs(a) > a_max)
00155   {
00156     v1 = (dq1) / dt1;
00157     v2 = (dq2) / dt2;
00158     a = 2.0 * (v2 - v1) / (dt1 + dt2);
00159     dt1 *= mult_factor;
00160   }
00161 
00162   return dt1;
00163 }
00164 
00165 double SplineParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2,
00166                                       const double a_max) const
00167 {
00168   const double mult_factor = 1.01;
00169   double v1 = (dq1) / dt1;
00170   double v2 = (dq2) / dt2;
00171   double a = 2.0 * (v2 - v1) / (dt1 + dt2);
00172 
00173   while (std::abs(a) > a_max)
00174   {
00175     v1 = (dq1) / dt1;
00176     v2 = (dq2) / dt2;
00177     a = 2.0 * (v2 - v1) / (dt1 + dt2);
00178     dt2 *= mult_factor;
00179   }
00180 
00181   return dt2;
00182 }
00183 
00184 namespace
00185 {
00186 // Takes the time differences, and updates the timestamps, velocities and accelerations
00187 // in the trajectory.
00188 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
00189 {
00190   // Error check
00191   if (time_diff.empty())
00192     return;
00193 
00194   robot_state::RobotStatePtr curr_waypoint;
00195 
00196   const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00197   const std::vector<std::string>& vars = group->getVariableNames();
00198   const std::vector<int>& idx = group->getVariableIndexList();
00199 
00200   int num_points = rob_trajectory.getWayPointCount();
00201 
00202   // Adding a 1ns offset for prefixing a 0-acceleration state
00203   rob_trajectory.setWayPointDurationFromPrevious(0, 1e-9);
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   // Fitting a cubic spline
00215   for (std::size_t j = 0; j < vars.size(); ++j)
00216   {
00217     std::vector<double> x, y;
00218     for (int i = 0; i < num_points; ++i)
00219     {
00220       curr_waypoint = rob_trajectory.getWayPointPtr(i);
00221       y.push_back(curr_waypoint->getVariablePosition(idx[j]));
00222       x.push_back(rob_trajectory.getWaypointDurationFromStart(i));
00223     }
00224 
00225     tk::spline s;
00226     s.set_boundary(tk::spline::first_deriv, 0.0, tk::spline::first_deriv, 0.0, false);
00227     s.set_points(x, y);
00228 
00229     for (int i = 0; i < num_points; ++i)
00230     {
00231       curr_waypoint = rob_trajectory.getWayPointPtr(i);
00232       curr_waypoint->setVariableVelocity(idx[j], s.deriv(1, x[i]));
00233       curr_waypoint->setVariableAcceleration(idx[j], s.deriv(2, x[i]));
00234     }
00235   }
00236 
00237   // Add prefix waypoint at 50ms to ensure it is not in the past
00238   rob_trajectory.addPrefixWayPoint(rob_trajectory.getFirstWayPoint(), 0.05);
00239 
00240   // Set accelerations to 0 on first waypoint
00241   std::vector<double> zero_vector(vars.size(), 0.0);
00242   rob_trajectory.getWayPointPtr(0)->setVariableAccelerations(zero_vector);
00243 }
00244 }
00245 
00246 // Applies Acceleration constraints
00247 void SplineParameterization::applyAccelerationConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
00248                                                           std::vector<double>& time_diff,
00249                                                           const double max_acceleration_scaling_factor) const
00250 {
00251   robot_state::RobotStatePtr prev_waypoint;
00252   robot_state::RobotStatePtr curr_waypoint;
00253   robot_state::RobotStatePtr next_waypoint;
00254 
00255   const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
00256   const std::vector<std::string>& vars = group->getVariableNames();
00257   const std::vector<int>& idx = group->getVariableIndexList();
00258   const robot_model::RobotModel& rmodel = group->getParentModel();
00259 
00260   const int num_points = rob_trajectory.getWayPointCount();
00261   const unsigned int num_joints = group->getVariableCount();
00262   int num_updates = 0;
00263   int iteration = 0;
00264   bool backwards = false;
00265   double q1;
00266   double q2;
00267   double q3;
00268   double dt1;
00269   double dt2;
00270   double v1;
00271   double v2;
00272   double a;
00273 
00274   double acceleration_scaling_factor = 1.0;
00275 
00276   if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
00277     acceleration_scaling_factor = max_acceleration_scaling_factor;
00278   else if (max_acceleration_scaling_factor == 0.0)
00279     logDebug("A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
00280              acceleration_scaling_factor);
00281   else
00282     logWarn("Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
00283             max_acceleration_scaling_factor, acceleration_scaling_factor);
00284 
00285   do
00286   {
00287     num_updates = 0;
00288     iteration++;
00289 
00290     // In this case we iterate through the joints on the outer loop.
00291     // This is so that any time interval increases have a chance to get propogated through the trajectory
00292     for (unsigned int j = 0; j < num_joints; ++j)
00293     {
00294       // Loop forwards, then backwards
00295       for (int count = 0; count < 2; ++count)
00296       {
00297         for (int i = 0; i < num_points - 1; ++i)
00298         {
00299           int index = backwards ? (num_points - 1) - i : i;
00300 
00301           curr_waypoint = rob_trajectory.getWayPointPtr(index);
00302 
00303           if (index > 0)
00304             prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
00305 
00306           if (index < num_points - 1)
00307             next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
00308 
00309           // Get acceleration limits
00310           double a_max = DEFAULT_ACCEL_MAX;
00311           const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
00312           if (b.acceleration_bounded_)
00313             a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
00314                              fabs(b.min_acceleration_ * acceleration_scaling_factor));
00315 
00316           if (index == 0)
00317           {
00318             // First point
00319             q1 = next_waypoint->getVariablePosition(idx[j]);
00320             q2 = curr_waypoint->getVariablePosition(idx[j]);
00321             q3 = next_waypoint->getVariablePosition(idx[j]);
00322 
00323             dt1 = dt2 = time_diff[index];
00324             assert(!backwards);
00325           }
00326           else if (index < num_points - 1)
00327           {
00328             // middle points
00329             q1 = prev_waypoint->getVariablePosition(idx[j]);
00330             q2 = curr_waypoint->getVariablePosition(idx[j]);
00331             q3 = next_waypoint->getVariablePosition(idx[j]);
00332 
00333             dt1 = time_diff[index - 1];
00334             dt2 = time_diff[index];
00335           }
00336           else
00337           {
00338             // last point - careful, there are only numpoints-1 time intervals
00339             q1 = prev_waypoint->getVariablePosition(idx[j]);
00340             q2 = curr_waypoint->getVariablePosition(idx[j]);
00341             q3 = prev_waypoint->getVariablePosition(idx[j]);
00342 
00343             dt1 = dt2 = time_diff[index - 1];
00344             assert(backwards);
00345           }
00346 
00347           if (dt1 == 0.0 || dt2 == 0.0)
00348           {
00349             v1 = 0.0;
00350             v2 = 0.0;
00351             a = 0.0;
00352           }
00353           else
00354           {
00355             bool start_velocity = false;
00356             if (index == 0)
00357             {
00358               if (curr_waypoint->hasVelocities())
00359               {
00360                 start_velocity = true;
00361                 v1 = curr_waypoint->getVariableVelocity(idx[j]);
00362               }
00363             }
00364             v1 = start_velocity ? v1 : (q2 - q1) / dt1;
00365             v2 = (q3 - q2) / dt2;
00366             a = 2.0 * (v2 - v1) / (dt1 + dt2);
00367           }
00368 
00369           if (fabs(a) > a_max + ROUNDING_THRESHOLD)
00370           {
00371             if (!backwards)
00372             {
00373               dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
00374               time_diff[index] = dt2;
00375             }
00376             else
00377             {
00378               dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
00379               time_diff[index - 1] = dt1;
00380             }
00381             num_updates++;
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               v1 = (q2 - q1) / dt1;
00392               v2 = (q3 - q2) / dt2;
00393               a = 2 * (v2 - v1) / (dt1 + dt2);
00394             }
00395           }
00396         }
00397         backwards = !backwards;
00398       }
00399     }
00400     // logDebug("applyAcceleration: num_updates=%i", num_updates);
00401   } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
00402 }
00403 
00404 bool SplineParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
00405                                                const double max_velocity_scaling_factor,
00406                                                const double max_acceleration_scaling_factor) const
00407 {
00408   if (trajectory.empty())
00409     return true;
00410 
00411   const robot_model::JointModelGroup* group = trajectory.getGroup();
00412   if (!group)
00413   {
00414     logError("It looks like the planner did not set the group the plan was computed for");
00415     return false;
00416   }
00417 
00418   // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
00419   trajectory.unwind();
00420 
00421   const int num_points = trajectory.getWayPointCount();
00422   std::vector<double> time_diff(num_points - 1, 0.0);  // the time difference between adjacent points
00423 
00424   applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
00425   applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
00426 
00427   updateTrajectory(trajectory, time_diff);
00428   return true;
00429 }
00430 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36