velocityprofile_traphalf.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  velocityprofile_traphalf.cxx
00003 
00004                         velocityprofile_traphalf.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_traphalf.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
00039  *              $Name:  $
00040  ****************************************************************************/
00041 
00042 
00043 //#include "error.h"
00044 #include "velocityprofile_traphalf.hpp"
00045 
00046 namespace KDL {
00047 
00048 
00049 VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc,bool _starting):
00050                   maxvel(_maxvel),maxacc(_maxacc),starting(_starting) {}
00051 
00052 void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting)
00053 {
00054     maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
00055 }
00056 
00057 void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
00058         a3 = 0;
00059         a2 = 0;
00060         a1 = startpos;
00061         b3 = a/2.0;
00062         b2 = -a*t1;
00063         b1 = startpos + a*t1*t1/2.0;
00064         c3 = 0;
00065         c2 = v;
00066         c1 = endpos - v*duration;
00067 }
00068 
00069 void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) {
00070         a3 = 0;
00071         a2 = v;
00072         a1 = startpos;
00073         b3 = -a/2.0;
00074         b2 = a*t2;
00075         b1 = endpos - a*t2*t2/2.0;
00076         c3 = 0;
00077         c2 = 0;
00078         c1 = endpos;
00079 }
00080 
00081 void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
00082         startpos        = pos1;
00083         endpos          = pos2;
00084         double s        = sign(endpos-startpos);
00085         // check that the maxvel is obtainable
00086         double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc));
00087         duration                = s*(endpos-startpos)/vel+vel/maxacc/2.0;
00088         if (starting) {
00089                 t1 = 0;
00090                 t2 = vel/maxacc;
00091                 PlanProfile1(vel*s,maxacc*s);
00092         } else {
00093                 t1 = duration-vel/maxacc;
00094                 t2 = duration;
00095                 PlanProfile2(s*vel,s*maxacc);
00096         }
00097 }
00098 
00099 void VelocityProfile_TrapHalf::SetProfileDuration(
00100         double pos1,double pos2,double newduration)
00101 {
00102     SetProfile(pos1,pos2);
00103     double factor = duration/newduration;
00104 
00105     if ( factor > 1 )
00106         return;
00107 
00108         double s        = sign(endpos-startpos);
00109         double tmp      = 2.0*s*(endpos-startpos)/maxvel;
00110         double v        = s*maxvel;
00111         duration        = newduration;
00112         if (starting) {
00113                 if (tmp > duration) {
00114                         t1 = 0;
00115                         double a = v*v/2.0/(v*duration-(endpos-startpos));
00116                         t2 = v/a;
00117                         PlanProfile1(v,a);
00118                 } else {
00119                         t2 = duration;
00120                         double a = v*v/2.0/(endpos-startpos);
00121                         t1 = t2-v/a;
00122                         PlanProfile1(v,a);
00123                 }
00124         } else {
00125                 if (tmp > duration) {
00126                         t2 = duration;
00127                         double a = v*v/2.0/(v*duration-(endpos-startpos));
00128                         t1 = t2-v/a;
00129                         PlanProfile2(v,a);
00130                 } else {
00131                         double a = v*v/2.0/(endpos-startpos);
00132                         t1 = 0;
00133                         t2 = v/a;
00134                         PlanProfile2(v,a);
00135                 }
00136         }
00137 }
00138 
00139 double VelocityProfile_TrapHalf::Duration() const {
00140         return duration;
00141 }
00142 
00143 double VelocityProfile_TrapHalf::Pos(double time) const {
00144         if (time < 0) {
00145                 return startpos;
00146         } else if (time<t1) {
00147                 return a1+time*(a2+a3*time);
00148         } else if (time<t2) {
00149                 return b1+time*(b2+b3*time);
00150         } else if (time<=duration) {
00151                 return c1+time*(c2+c3*time);
00152         } else {
00153                 return endpos;
00154         }
00155 }
00156 double VelocityProfile_TrapHalf::Vel(double time) const {
00157         if (time < 0) {
00158                 return 0;
00159         } else if (time<t1) {
00160                 return a2+2*a3*time;
00161         } else if (time<t2) {
00162                 return b2+2*b3*time;
00163         } else if (time<=duration) {
00164                 return c2+2*c3*time;
00165         } else {
00166                 return 0;
00167         }
00168 }
00169 
00170 double VelocityProfile_TrapHalf::Acc(double time) const {
00171         if (time < 0) {
00172                 return 0;
00173         } else if (time<t1) {
00174                 return 2*a3;
00175         } else if (time<t2) {
00176                 return 2*b3;
00177         } else if (time<=duration) {
00178                 return 2*c3;
00179         } else {
00180                 return 0;
00181         }
00182 }
00183 
00184 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
00185     VelocityProfile_TrapHalf* res =  new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
00186     res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00187         return res;
00188 }
00189 
00190 VelocityProfile_TrapHalf::~VelocityProfile_TrapHalf() {}
00191 
00192 
00193 void VelocityProfile_TrapHalf::Write(std::ostream& os) const {
00194         os << "TRAPEZOIDALHALF[" << maxvel << "," << maxacc << "," << starting << "]";
00195 }
00196 
00197 
00198 
00199 
00200 
00201 }
00202 


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