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   duration_ = duration;
00111   generatePowers(5, duration, T);
00112 
00113   if (duration <= std::numeric_limits<double>::epsilon() )
00114   {
00115         coeff_[0] = pos2;
00116         coeff_[1] = vel2;
00117         coeff_[2] = 0.5 * acc2;
00118         coeff_[3] = 0.0;
00119         coeff_[4] = 0.0;
00120         coeff_[5] = 0.0;
00121   } else
00122   {
00123     coeff_[0] = pos1;
00124     coeff_[1] = vel1;
00125     coeff_[2] = 0.5*acc1;
00126     coeff_[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*T[2] + acc2*T[2] -
00127                        12.0*vel1*T[1] - 8.0*vel2*T[1]) / (2.0*T[3]);
00128     coeff_[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*T[2] - 2.0*acc2*T[2] +
00129                        16.0*vel1*T[1] + 14.0*vel2*T[1]) / (2.0*T[4]);
00130     coeff_[5] = (-12.0*pos1 + 12.0*pos2 - acc1*T[2] + acc2*T[2] -
00131                        6.0*vel1*T[1] - 6.0*vel2*T[1]) / (2.0*T[5]);
00132   }
00133   
00134   return;
00135 }
00136 
00137 double VelocityProfile_Spline::Duration() const
00138 {
00139   return duration_;
00140 }
00141 
00142 double VelocityProfile_Spline::Pos(double time) const
00143 {
00144   double t[6];
00145   double position;
00146   generatePowers(5, time, t);
00147 
00148   position = t[0]*coeff_[0] +
00149              t[1]*coeff_[1] +
00150              t[2]*coeff_[2] +
00151              t[3]*coeff_[3] +
00152              t[4]*coeff_[4] +
00153              t[5]*coeff_[5];
00154   return position;
00155 }
00156 
00157 double VelocityProfile_Spline::Vel(double time) const
00158 {
00159   double t[5];
00160   double velocity;
00161   generatePowers(4, time, t);
00162 
00163   velocity = t[0]*coeff_[1] +
00164              2.0*t[1]*coeff_[2] +
00165              3.0*t[2]*coeff_[3] +
00166              4.0*t[3]*coeff_[4] +
00167              5.0*t[4]*coeff_[5];
00168   return velocity;
00169 }
00170 
00171 double VelocityProfile_Spline::Acc(double time) const
00172 {
00173   double t[4];
00174   double acceleration;
00175   generatePowers(3, time, t);
00176 
00177   acceleration = 2.0*t[0]*coeff_[2] +
00178                  6.0*t[1]*coeff_[3] +
00179                  12.0*t[2]*coeff_[4] +
00180                  20.0*t[3]*coeff_[5];
00181   return acceleration;
00182 }
00183 
00184 void VelocityProfile_Spline::Write(std::ostream& os) const
00185 {
00186   os << "coefficients : [ " << coeff_[0] << " " << coeff_[1] << " " << coeff_[2] << " " << coeff_[3] << " " << coeff_[4] << " " << coeff_[5] << " ]";
00187   return;
00188 }
00189 
00190 VelocityProfile* VelocityProfile_Spline::Clone() const
00191 {
00192   return new VelocityProfile_Spline(*this);
00193 }
00194 }


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28