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::SetMax(double _maxvel,double _maxacc)
00112 {
00113     maxvel = _maxvel; maxacc = _maxacc;
00114 }
00115 
00116 double VelocityProfile_Trap::Duration() const {
00117         return duration;
00118 }
00119 
00120 double VelocityProfile_Trap::Pos(double time) const {
00121         if (time < 0) {
00122                 return startpos;
00123         } else if (time<t1) {
00124                 return a1+time*(a2+a3*time);
00125         } else if (time<t2) {
00126                 return b1+time*(b2+b3*time);
00127         } else if (time<=duration) {
00128                 return c1+time*(c2+c3*time);
00129         } else {
00130                 return endpos;
00131         }
00132 }
00133 double VelocityProfile_Trap::Vel(double time) const {
00134         if (time < 0) {
00135                 return 0;
00136         } else if (time<t1) {
00137                 return a2+2*a3*time;
00138         } else if (time<t2) {
00139                 return b2+2*b3*time;
00140         } else if (time<=duration) {
00141                 return c2+2*c3*time;
00142         } else {
00143                 return 0;
00144         }
00145 }
00146 
00147 double VelocityProfile_Trap::Acc(double time) const {
00148         if (time < 0) {
00149                 return 0;
00150         } else if (time<t1) {
00151                 return 2*a3;
00152         } else if (time<t2) {
00153                 return 2*b3;
00154         } else if (time<=duration) {
00155                 return 2*c3;
00156         } else {
00157                 return 0;
00158         }
00159 }
00160 
00161 VelocityProfile* VelocityProfile_Trap::Clone() const {
00162     VelocityProfile_Trap* res =  new VelocityProfile_Trap(maxvel,maxacc);
00163     res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00164         return res;
00165 }
00166 
00167 VelocityProfile_Trap::~VelocityProfile_Trap() {}
00168 
00169 
00170 void VelocityProfile_Trap::Write(std::ostream& os) const {
00171         os << "TRAPEZOIDAL[" << maxvel << "," << maxacc <<"]";
00172 }
00173 
00174 
00175 
00176 
00177 
00178 }
00179 


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25