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;
00062         b2 = -a*t1;
00063         b1 = startpos + a*t1*t1/2;
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;
00074         b2 = a*t2;
00075         b1 = endpos - a*t2*t2/2;
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         duration                = s*(endpos-startpos)/maxvel+maxvel/maxacc/2.0;
00086         if (starting) {
00087                 t1 = 0;
00088                 t2 = maxvel/maxacc;
00089                 PlanProfile1(maxvel*s,maxacc*s);
00090         } else {
00091                 t1 = duration-maxvel/maxacc;
00092                 t2 = duration;
00093                 PlanProfile2(s*maxvel,s*maxacc);
00094         }
00095 }
00096 
00097 void VelocityProfile_TrapHalf::SetProfileDuration(
00098         double pos1,double pos2,double newduration)
00099 {
00100     SetProfile(pos1,pos2);
00101     double factor = duration/newduration;
00102 
00103     if ( factor > 1 )
00104         return;
00105 
00106         double s        = sign(endpos-startpos);
00107         double tmp      = 2.0*s*(endpos-startpos)/maxvel;
00108         double v        = s*maxvel;
00109         duration        = newduration;
00110         if (starting) {
00111                 if (tmp > duration) {
00112                         t1 = 0;
00113                         double a = v*v/2.0/(v*duration-(endpos-startpos));
00114                         t2 = v/a;
00115                         PlanProfile1(v,a);
00116                 } else {
00117                         t2 = duration;
00118                         double a = v*v/2.0/(endpos-startpos);
00119                         t1 = t2-v/a;
00120                         PlanProfile1(v,a);
00121                 }
00122         } else {
00123                 if (tmp > duration) {
00124                         t2 = duration;
00125                         double a = v*v/2.0/(v*duration-(endpos-startpos));
00126                         t1 = t2-v/a;
00127                         PlanProfile2(v,a);
00128                 } else {
00129                         double a = v*v/2.0/(endpos-startpos);
00130                         t1 = 0;
00131                         t2 = v/a;
00132                         PlanProfile2(v,a);
00133                 }
00134         }
00135 }
00136 
00137 double VelocityProfile_TrapHalf::Duration() const {
00138         return duration;
00139 }
00140 
00141 double VelocityProfile_TrapHalf::Pos(double time) const {
00142         if (time < 0) {
00143                 return startpos;
00144         } else if (time<t1) {
00145                 return a1+time*(a2+a3*time);
00146         } else if (time<t2) {
00147                 return b1+time*(b2+b3*time);
00148         } else if (time<=duration) {
00149                 return c1+time*(c2+c3*time);
00150         } else {
00151                 return endpos;
00152         }
00153 }
00154 double VelocityProfile_TrapHalf::Vel(double time) const {
00155         if (time < 0) {
00156                 return 0;
00157         } else if (time<t1) {
00158                 return a2+2*a3*time;
00159         } else if (time<t2) {
00160                 return b2+2*b3*time;
00161         } else if (time<=duration) {
00162                 return c2+2*c3*time;
00163         } else {
00164                 return 0;
00165         }
00166 }
00167 
00168 double VelocityProfile_TrapHalf::Acc(double time) const {
00169         if (time < 0) {
00170                 return 0;
00171         } else if (time<t1) {
00172                 return 2*a3;
00173         } else if (time<t2) {
00174                 return 2*b3;
00175         } else if (time<=duration) {
00176                 return 2*c3;
00177         } else {
00178                 return 0;
00179         }
00180 }
00181 
00182 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
00183     VelocityProfile_TrapHalf* res =  new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
00184     res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00185         return res;
00186 }
00187 
00188 VelocityProfile_TrapHalf::~VelocityProfile_TrapHalf() {}
00189 
00190 
00191 void VelocityProfile_TrapHalf::Write(std::ostream& os) const {
00192         os << "TRAPEZOIDALHALF[" << maxvel << "," << maxacc << "," << starting << "]";
00193 }
00194 
00195 
00196 
00197 
00198 
00199 }
00200 


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