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 }
