Trajectory.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Georgia Tech Research Corporation
00003  * All rights reserved.
00004  *
00005  * Author: Tobias Kunz <tobias@gatech.edu>
00006  * Date: 10/2011
00007  *
00008  * Humanoid Robotics Lab      Georgia Institute of Technology
00009  * Director: Mike Stilman     http://www.golems.org
00010  *
00011  * Algorithm details and publications:
00012  * http://www.golems.org/node/1570
00013  *
00014  * This file is provided under the following "BSD-style" License:
00015  *   Redistribution and use in source and binary forms, with or
00016  *   without modification, are permitted provided that the following
00017  *   conditions are met:
00018  *   * Redistributions of source code must retain the above copyright
00019  *     notice, this list of conditions and the following disclaimer.
00020  *   * Redistributions in binary form must reproduce the above
00021  *     copyright notice, this list of conditions and the following
00022  *     disclaimer in the documentation and/or other materials provided
00023  *     with the distribution.
00024  *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
00025  *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
00026  *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00027  *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00029  *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00030  *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00031  *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
00032  *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
00033  *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *   POSSIBILITY OF SUCH DAMAGE.
00037  */
00038 
00039 #include "constraint_aware_spline_smoother/KunzStilman/Trajectory.h"
00040 
00041 using namespace std;
00042 using namespace Eigen;
00043 using namespace ParabolicBlend;
00044 
00045 Trajectory::Trajectory(const list<VectorXd> &_path, const VectorXd &maxVelocity, const VectorXd &maxAcceleration, double minWayPointSeparation) :
00046         path(_path.begin(), _path.end()),
00047         velocities(path.size() - 1),
00048         accelerations(path.size()),
00049         durations(path.size() - 1),
00050         blendDurations(path.size()),
00051         duration(0.0)
00052 {
00053   bool removeWayPoints = false;
00054         if(minWayPointSeparation > 0.0) {
00055                 removeWayPoints = true;
00056         }
00057 
00058         // remove waypoints that are too close together
00059         while(removeWayPoints) {
00060                 removeWayPoints = false;
00061 
00062                 vector<VectorXd>::iterator it = path.begin();
00063                 vector<VectorXd>::iterator next = it;
00064                 if(next != path.end())
00065                         next++;
00066                 while(next != path.end()) {
00067                         if((*it - *next).norm() < minWayPointSeparation) {
00068                                 removeWayPoints = true;
00069                                 vector<VectorXd>::iterator nextNext = next;
00070                                 nextNext++;
00071                                 if(it == path.begin()) {
00072                                         it = path.erase(next);
00073                                 }
00074                                 else if(nextNext == path.end()) {
00075                                         it = path.erase(it);
00076                                 }
00077                                 else {
00078                                         VectorXd newViaPoint = 0.5 * (*it + *next);
00079                                         it = path.erase(it);
00080                                         it = path.insert(it, newViaPoint);
00081                                         it++;
00082                                         it = path.erase(it);
00083                                 }
00084                                 next = it;
00085                                 if(next != path.end())
00086                                         next++;
00087                         }
00088                         else {
00089                                 it = next;
00090                                 next++;
00091                         }
00092                 }
00093 
00094                 velocities.resize(path.size() - 1);
00095                 accelerations.resize(path.size());
00096                 durations.resize(path.size() - 1);
00097                 blendDurations.resize(path.size());
00098         }
00099 
00100         // calculate time between waypoints and initial velocities of linear segments
00101         for(unsigned int i = 0; i < path.size() - 1; i++) {
00102                 durations[i] = 0.0;
00103                 for(int j = 0; j < path[i].size(); j++) {
00104                         durations[i] = max(durations[i], abs(path[i+1][j] - path[i][j]) / maxVelocity[j]);
00105                 }
00106                 velocities[i] = (path[i+1] - path[i]) / durations[i];
00107         }
00108 
00109         int numBlendsSlowedDown = numeric_limits<int>::max();
00110         while(numBlendsSlowedDown > 1) {
00111                 numBlendsSlowedDown = 0;
00112                 vector<double> slowDownFactors(path.size(), 1.0);
00113 
00114                 for(unsigned int i = 0; i < path.size(); i++) {
00115                         // calculate blend duration and acceleration
00116                         VectorXd previousVelocity = (i == 0) ? VectorXd::Zero(path[i].size()) : velocities[i-1];
00117                         VectorXd nextVelocity = (i == path.size() - 1) ? VectorXd::Zero(path[i].size()) : velocities[i];
00118                         blendDurations[i] = 0.0;
00119                         for(int j = 0; j < path[i].size(); j++) {
00120                                 blendDurations[i] = max(blendDurations[i], abs(nextVelocity[j] - previousVelocity[j]) / maxAcceleration[j]);
00121                                 accelerations[i] = (nextVelocity - previousVelocity) / blendDurations[i];
00122                         }
00123 
00124       // calculate slow down factor such that the blend phase replaces at most half of the neighboring linear segments
00125                         const double eps = 0.000001;
00126                         if((i > 0 && blendDurations[i] > durations[i-1] + eps && blendDurations[i-1] + blendDurations[i] > 2.0 * durations[i-1] + eps)
00127                                 || i < path.size() - 1 && blendDurations[i] > durations[i] + eps && blendDurations[i] + blendDurations[i+1] > 2.0 * durations[i] + eps)
00128                         {
00129                                 numBlendsSlowedDown++;
00130                                 const double maxDuration = min(i == 0 ? numeric_limits<double>::max() : durations[i-1],
00131                                         i == path.size() - 1 ? numeric_limits<double>::max() : durations[i]);
00132                                 slowDownFactors[i] = sqrt(maxDuration / blendDurations[i]);
00133                         }
00134                 }
00135 
00136     // apply slow down factors to linear segments
00137                 for(unsigned int i = 0; i < path.size() - 1; i++) {
00138                         velocities[i] *= min(slowDownFactors[i], slowDownFactors[i+1]);
00139                         durations[i] /= min(slowDownFactors[i], slowDownFactors[i+1]);
00140                 }
00141   }
00142         
00143   // calculate total time of trajectory
00144         for(unsigned int i = 0; i < path.size() - 1; i++) {
00145                 duration += durations[i];
00146         }
00147         duration += 0.5 * blendDurations.front() + 0.5 * blendDurations.back();
00148 }
00149 
00150 
00151 VectorXd Trajectory::getPosition(double time) const {
00152   if(time > duration) {
00153                 return path.back();
00154         }
00155         double t = time;
00156         if(t <= 0.5 * blendDurations[0]) {
00157                 return path[0] + 0.5 * t * t * accelerations[0];
00158         }
00159         else {
00160                 t -= 0.5 * blendDurations[0];
00161         }
00162   unsigned int i = 0;
00163         while(i < path.size() - 1 && t > durations[i]) {
00164                 t -= durations[i];
00165                 i++;
00166         }
00167   if(i == path.size() - 1) {
00168                 t = 0.5 * blendDurations.back() - t;
00169                 return path.back() + 0.5 * t * t * accelerations.back();
00170         }
00171 
00172         double switchingTime1 = 0.5 * blendDurations[i];
00173         double switchingTime2 = durations[i] - 0.5 * blendDurations[i+1];
00174 
00175         if(t < switchingTime1) {
00176                 t = switchingTime1 - t;
00177                 return path[i] + switchingTime1 * velocities[i] - t * velocities[i] + 0.5 * t * t * accelerations[i];
00178         }
00179         else if(t > switchingTime2) {
00180                 t -= switchingTime2;
00181                 return path[i] + switchingTime2 * velocities[i] + t * velocities[i] + 0.5 * t * t * accelerations[i+1];
00182         }
00183         else {
00184                 return path[i] + t * velocities[i];
00185         }
00186 }
00187 
00188 
00189 VectorXd Trajectory::getVelocity(double time) const {
00190   if(time > duration) {
00191                 return VectorXd::Zero(path.back().size());
00192         }
00193         double t = time;
00194         if(t <= 0.5 * blendDurations[0]) {
00195                 return t * accelerations[0];
00196         }
00197         else {
00198                 t -= 0.5 * blendDurations[0];
00199         }
00200   unsigned int i = 0;
00201         while(i < path.size() - 1 && t > durations[i]) {
00202                 t -= durations[i];
00203                 i++;
00204         }
00205   if(i == path.size() - 1) {
00206                 t = 0.5 * blendDurations.back() - t;
00207                 return - t * accelerations.back();
00208         }
00209 
00210         double switchingTime1 = 0.5 * blendDurations[i];
00211         double switchingTime2 = durations[i] - 0.5 * blendDurations[i+1];
00212 
00213   if(t < switchingTime1) {
00214                 t = switchingTime1 - t;
00215                 return velocities[i] - t * accelerations[i];
00216         }
00217         else if(t > switchingTime2) {
00218                 t -= switchingTime2;
00219                 return velocities[i] + t * accelerations[i+1];
00220         }
00221         else {
00222                 return velocities[i];
00223         }
00224 }
00225 
00226 
00227 double Trajectory::getDuration() const {
00228   return duration;
00229 }


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