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


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:23