velocityprofile_trap.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  velocityprofile_trap.cxx
00003 
00004                         velocityprofile_trap.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Erwin Aertbelien
00008     email                : erwin.aertbelien@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 /*****************************************************************************
00028  *  \author
00029  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00030  *
00031  *  \version
00032  *              ORO_Geometry V0.2
00033  *
00034  *      \par History
00035  *              - $log$
00036  *
00037  *      \par Release
00038  *              $Id: velocityprofile_trap.cpp,v 1.1.1.1.2.7 2003/07/24 13:26:15 psoetens Exp $
00039  *              $Name:  $
00040  ****************************************************************************/
00041 
00042 
00043 //#include "error.h"
00044 #include "velocityprofile_trap.hpp"
00045 
00046 namespace KDL {
00047 
00048 
00049 VelocityProfile_Trap::VelocityProfile_Trap(double _maxvel,double _maxacc):
00050           a1(0), a2(0), a3(0),
00051           b1(0), b2(0), b3(0),
00052           c1(0), c2(0), c3(0),
00053           duration(0), t1(0), t2(0),
00054                   maxvel(_maxvel),maxacc(_maxacc),
00055           startpos(0), endpos(0)
00056 
00057 {}
00058                 // constructs motion profile class with <maxvel> as parameter of the
00059                 // trajectory.
00060 
00061 void VelocityProfile_Trap::SetProfile(double pos1,double pos2) {
00062         startpos = pos1;
00063         endpos   = pos2;
00064         t1 = maxvel/maxacc;
00065         double s       = sign(endpos-startpos);
00066         double deltax1 = s*maxacc*sqr(t1)/2.0;
00067         double deltaT  = (endpos-startpos-2.0*deltax1) / (s*maxvel);
00068         if (deltaT > 0.0) {
00069                 // plan a complete profile :
00070                 duration = 2*t1+deltaT;
00071                 t2 = duration - t1;
00072         } else {
00073                 // plan an incomplete profile :
00074                 t1 = ::sqrt((endpos-startpos)/s/maxacc);
00075                 duration = t1*2.0;
00076                 t2=t1;
00077         }
00078         a3 = s*maxacc/2.0;
00079         a2 = 0;
00080         a1 = startpos;
00081 
00082         b3 = 0;
00083         b2 = a2+2*a3*t1 - 2.0*b3*t1;
00084         b1 = a1+t1*(a2+a3*t1) - t1*(b2+t1*b3);
00085 
00086         c3 = -s*maxacc/2.0;
00087         c2 = b2+2*b3*t2 - 2.0*c3*t2;
00088         c1 = b1+t2*(b2+b3*t2) - t2*(c2+t2*c3);
00089 }
00090 
00091 void VelocityProfile_Trap::SetProfileDuration(
00092         double pos1,double pos2,double newduration) {
00093         // duration should be longer than originally planned duration
00094     // Fastest :
00095         SetProfile(pos1,pos2);
00096     // Must be Slower  :
00097         double factor = duration/newduration;
00098     if (factor > 1)
00099         return; // do not exceed max
00100         a2*=factor;
00101         a3*=factor*factor;
00102         b2*=factor;
00103         b3*=factor*factor;
00104         c2*=factor;
00105         c3*=factor*factor;
00106         duration = newduration;
00107         t1 /= factor;
00108         t2 /= factor;
00109 }
00110 
00111 void VelocityProfile_Trap::SetProfileVelocity(
00112         double pos1,double pos2,double newvelocity) {
00113     // Max velocity
00114         SetProfile(pos1,pos2);
00115     // Must be Slower  :
00116         double factor = newvelocity;            // valid = [KDL::epsilon, 1.0]
00117         if (1.0 < factor) factor = 1.0;
00118         if (KDL::epsilon > factor) factor = KDL::epsilon;
00119         a2*=factor;
00120         a3*=factor*factor;
00121         b2*=factor;
00122         b3*=factor*factor;
00123         c2*=factor;
00124         c3*=factor*factor;
00125         duration = duration / factor;
00126         t1 /= factor;
00127         t2 /= factor;
00128 }
00129 
00130 void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc)
00131 {
00132     maxvel = _maxvel; maxacc = _maxacc;
00133 }
00134 
00135 double VelocityProfile_Trap::Duration() const {
00136         return duration;
00137 }
00138 
00139 double VelocityProfile_Trap::Pos(double time) const {
00140         if (time < 0) {
00141                 return startpos;
00142         } else if (time<t1) {
00143                 return a1+time*(a2+a3*time);
00144         } else if (time<t2) {
00145                 return b1+time*(b2+b3*time);
00146         } else if (time<=duration) {
00147                 return c1+time*(c2+c3*time);
00148         } else {
00149                 return endpos;
00150         }
00151 }
00152 double VelocityProfile_Trap::Vel(double time) const {
00153         if (time < 0) {
00154                 return 0;
00155         } else if (time<t1) {
00156                 return a2+2*a3*time;
00157         } else if (time<t2) {
00158                 return b2+2*b3*time;
00159         } else if (time<=duration) {
00160                 return c2+2*c3*time;
00161         } else {
00162                 return 0;
00163         }
00164 }
00165 
00166 double VelocityProfile_Trap::Acc(double time) const {
00167         if (time < 0) {
00168                 return 0;
00169         } else if (time<t1) {
00170                 return 2*a3;
00171         } else if (time<t2) {
00172                 return 2*b3;
00173         } else if (time<=duration) {
00174                 return 2*c3;
00175         } else {
00176                 return 0;
00177         }
00178 }
00179 
00180 VelocityProfile* VelocityProfile_Trap::Clone() const {
00181     VelocityProfile_Trap* res =  new VelocityProfile_Trap(maxvel,maxacc);
00182     res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00183         return res;
00184 }
00185 
00186 VelocityProfile_Trap::~VelocityProfile_Trap() {}
00187 
00188 
00189 void VelocityProfile_Trap::Write(std::ostream& os) const {
00190         os << "TRAPEZOIDAL[" << maxvel << "," << maxacc <<"]";
00191 }
00192 
00193 
00194 
00195 
00196 
00197 }
00198 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14