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 }