velocityprofile_spline.cpp
Go to the documentation of this file.
00001 
00002 #include <limits>
00003 
00004 #include "velocityprofile_spline.hpp"
00005 
00006 namespace KDL {
00007 
00008 static inline void generatePowers(int n, double x, double* powers)
00009 {
00010   powers[0] = 1.0;
00011   for (int i=1; i<=n; i++)
00012   {
00013     powers[i] = powers[i-1]*x;
00014   }
00015   return;
00016 }
00017 
00018 VelocityProfile_Spline::VelocityProfile_Spline()
00019 {
00020   duration_ = 0.0;
00021 
00022   coeff_[0] = 0.0;
00023   coeff_[1] = 0.0;
00024   coeff_[2] = 0.0;
00025   coeff_[3] = 0.0;
00026   coeff_[4] = 0.0;
00027   coeff_[5] = 0.0;
00028 
00029   return;
00030 }
00031 
00032 VelocityProfile_Spline::VelocityProfile_Spline(const VelocityProfile_Spline &p)
00033 {
00034   duration_ = p.duration_;
00035 
00036   coeff_[0] = p.coeff_[0];
00037   coeff_[1] = p.coeff_[1];
00038   coeff_[2] = p.coeff_[2];
00039   coeff_[3] = p.coeff_[3];
00040   coeff_[4] = p.coeff_[4];
00041   coeff_[5] = p.coeff_[5];
00042   
00043   return;
00044 }
00045 
00046 VelocityProfile_Spline::~VelocityProfile_Spline()
00047 {
00048         return;
00049 }
00050 
00051 void VelocityProfile_Spline::SetProfile(double pos1, double pos2)
00052 {
00053   return;
00054 }
00055 
00056 void VelocityProfile_Spline::SetProfileDuration(double pos1, double pos2, double duration)
00057 {
00058   duration_ = duration;
00059   if (duration <= std::numeric_limits<double>::epsilon() )
00060   {
00061         coeff_[0] = pos1;
00062         coeff_[1] = 0.0;
00063         coeff_[2] = 0.0;
00064         coeff_[3] = 0.0;
00065         coeff_[4] = 0.0;
00066         coeff_[5] = 0.0;
00067   } else
00068   {
00069     coeff_[0] = pos1;
00070     coeff_[1] = (pos2 - pos1) / duration;
00071     coeff_[2] = 0.0;
00072     coeff_[3] = 0.0;
00073     coeff_[4] = 0.0;
00074     coeff_[5] = 0.0;
00075   }
00076   
00077   return;
00078 }
00079 
00080 void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double pos2, double vel2, double duration)
00081 {
00082   double T[4];
00083   duration_ = duration;
00084   generatePowers(3, duration, T);
00085 
00086   if (duration <= std::numeric_limits<double>::epsilon() )
00087   {
00088         coeff_[0] = pos2;
00089         coeff_[1] = vel2;
00090         coeff_[2] = 0.0;
00091         coeff_[3] = 0.0;
00092         coeff_[4] = 0.0;
00093         coeff_[5] = 0.0;
00094   } else
00095   {
00096     coeff_[0] = pos1;
00097     coeff_[1] = vel1;
00098     coeff_[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*T[1] - vel2*T[1]) / T[2];
00099     coeff_[3] = (2.0*pos1 - 2.0*pos2 + vel1*T[1] + vel2*T[1]) / T[3];
00100     coeff_[4] = 0.0;
00101     coeff_[5] = 0.0;
00102   }
00103   
00104   return;
00105 }
00106 
00107 void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration)
00108 {
00109   double T[6];
00110   generatePowers(5, duration, T);
00111 
00112   if (duration <= std::numeric_limits<double>::epsilon() )
00113   {
00114         coeff_[0] = pos2;
00115         coeff_[1] = vel2;
00116         coeff_[2] = 0.5 * acc2;
00117         coeff_[3] = 0.0;
00118         coeff_[4] = 0.0;
00119         coeff_[5] = 0.0;
00120   } else
00121   {
00122     coeff_[0] = pos1;
00123     coeff_[1] = vel1;
00124     coeff_[2] = 0.5*acc1;
00125     coeff_[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*T[2] + acc2*T[2] -
00126                        12.0*vel1*T[1] - 8.0*vel2*T[1]) / (2.0*T[3]);
00127     coeff_[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*T[2] - 2.0*acc2*T[2] +
00128                        16.0*vel1*T[1] + 14.0*vel2*T[1]) / (2.0*T[4]);
00129     coeff_[5] = (-12.0*pos1 + 12.0*pos2 - acc1*T[2] + acc2*T[2] -
00130                        6.0*vel1*T[1] - 6.0*vel2*T[1]) / (2.0*T[5]);
00131   }
00132   
00133   return;
00134 }
00135 
00136 double VelocityProfile_Spline::Duration() const
00137 {
00138   return duration_;
00139 }
00140 
00141 double VelocityProfile_Spline::Pos(double time) const
00142 {
00143   double t[6];
00144   double position;
00145   generatePowers(5, time, t);
00146 
00147   position = t[0]*coeff_[0] +
00148              t[1]*coeff_[1] +
00149              t[2]*coeff_[2] +
00150              t[3]*coeff_[3] +
00151              t[4]*coeff_[4] +
00152              t[5]*coeff_[5];
00153   return position;
00154 }
00155 
00156 double VelocityProfile_Spline::Vel(double time) const
00157 {
00158   double t[5];
00159   double velocity;
00160   generatePowers(4, time, t);
00161 
00162   velocity = t[0]*coeff_[1] +
00163              2.0*t[1]*coeff_[2] +
00164              3.0*t[2]*coeff_[3] +
00165              4.0*t[3]*coeff_[4] +
00166              5.0*t[4]*coeff_[5];
00167   return velocity;
00168 }
00169 
00170 double VelocityProfile_Spline::Acc(double time) const
00171 {
00172   double t[4];
00173   double acceleration;
00174   generatePowers(3, time, t);
00175 
00176   acceleration = 2.0*t[0]*coeff_[2] +
00177                  6.0*t[1]*coeff_[3] +
00178                  12.0*t[2]*coeff_[4] +
00179                  20.0*t[3]*coeff_[5];
00180   return acceleration;
00181 }
00182 
00183 void VelocityProfile_Spline::Write(std::ostream& os) const
00184 {
00185   os << "coefficients : [ " << coeff_[0] << " " << coeff_[1] << " " << coeff_[2] << " " << coeff_[3] << " " << coeff_[4] << " " << coeff_[5] << " ]";
00186   return;
00187 }
00188 
00189 VelocityProfile* VelocityProfile_Spline::Clone() const
00190 {
00191   return new VelocityProfile_Spline(*this);
00192 }
00193 }


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17