$search
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 + a2*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