polynomial.cc
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without
00005 modification, are permitted provided that the following conditions are met:
00006 
00007 * Redistributions of source code must retain the above copyright notice, this
00008   list of conditions and the following disclaimer.
00009 
00010 * Redistributions in binary form must reproduce the above copyright notice,
00011   this list of conditions and the following disclaimer in the documentation
00012   and/or other materials provided with the distribution.
00013 
00014 * Neither the name of the copyright holder nor the names of its
00015   contributors may be used to endorse or promote products derived from
00016   this software without specific prior written permission.
00017 
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 ******************************************************************************/
00029 
00030 #include <towr/variables/polynomial.h>
00031 
00032 #include <cassert>
00033 #include <cmath>
00034 
00035 
00036 namespace towr {
00037 
00038 Polynomial::Polynomial (int order, int dim)
00039 {
00040   int n_coeff = order+1;
00041   for (int c=A; c<n_coeff; ++c) {
00042     coeff_ids_.push_back(static_cast<Coefficients>(c));
00043     coeff_.push_back(VectorXd::Zero(dim));
00044   }
00045 }
00046 
00047 State Polynomial::GetPoint(double t_local) const
00048 {
00049   // sanity checks
00050   if (t_local < 0.0)
00051     assert(false);//("spliner.cc called with dt<0")
00052 
00053   int n_dim = coeff_.front().size();
00054   State out(n_dim, 3);
00055 
00056   for (auto d : {kPos, kVel, kAcc})
00057     for (Coefficients c : coeff_ids_)
00058       out.at(d) += GetDerivativeWrtCoeff(t_local, d, c)*coeff_.at(c);
00059 
00060   return out;
00061 }
00062 
00063 double
00064 Polynomial::GetDerivativeWrtCoeff (double t, Dx deriv, Coefficients c) const
00065 {
00066   switch (deriv) {
00067     case kPos:   return               std::pow(t,c);         break;
00068     case kVel:   return c>=B? c*      std::pow(t,c-1) : 0.0; break;
00069     case kAcc:   return c>=C? c*(c-1)*std::pow(t,c-2) : 0.0; break;
00070     default: assert(false); // derivative not defined
00071   }
00072 }
00073 
00074 
00075 
00076 CubicHermitePolynomial::CubicHermitePolynomial (int dim)
00077     : Polynomial(3,dim),
00078       n0_(dim),
00079       n1_(dim)
00080 {
00081   T_ = 0.0;
00082 }
00083 
00084 void
00085 CubicHermitePolynomial::SetNodes (const Node& n0, const Node& n1)
00086 {
00087   n0_ = n0;
00088   n1_ = n1;
00089 }
00090 
00091 void
00092 CubicHermitePolynomial::SetDuration(double duration)
00093 {
00094   T_ = duration;
00095 }
00096 
00097 void
00098 CubicHermitePolynomial::UpdateCoeff()
00099 {
00100   coeff_[A] =  n0_.p();
00101   coeff_[B] =  n0_.v();
00102   coeff_[C] = -( 3*(n0_.p() - n1_.p()) +  T_*(2*n0_.v() + n1_.v()) ) / std::pow(T_,2);
00103   coeff_[D] =  ( 2*(n0_.p() - n1_.p()) +  T_*(  n0_.v() + n1_.v()) ) / std::pow(T_,3);
00104 }
00105 
00106 double
00107 CubicHermitePolynomial::GetDerivativeWrtStartNode (Dx dfdt,
00108                                              Dx node_derivative, // pos or velocity node
00109                                              double t_local) const
00110 {
00111   switch (dfdt) {
00112     case kPos:
00113       return GetDerivativeOfPosWrtStartNode(node_derivative, t_local);
00114     case kVel:
00115       return GetDerivativeOfVelWrtStartNode(node_derivative, t_local);
00116     case kAcc:
00117       return GetDerivativeOfAccWrtStartNode(node_derivative, t_local);
00118     default:
00119       assert(false); // derivative not yet implemented
00120   }
00121 }
00122 
00123 double
00124 CubicHermitePolynomial::GetDerivativeWrtEndNode (Dx dfdt,
00125                                            Dx node_derivative, // pos or velocity node
00126                                            double t_local) const
00127 {
00128   switch (dfdt) {
00129     case kPos:
00130       return GetDerivativeOfPosWrtEndNode(node_derivative, t_local);
00131     case kVel:
00132       return GetDerivativeOfVelWrtEndNode(node_derivative, t_local);
00133     case kAcc:
00134       return GetDerivativeOfAccWrtEndNode(node_derivative, t_local);
00135     default:
00136       assert(false); // derivative not yet implemented
00137   }
00138 }
00139 
00140 double
00141 CubicHermitePolynomial::GetDerivativeOfPosWrtStartNode(Dx node_value,
00142                                                        double t) const
00143 {
00144   double t2 = std::pow(t,2);
00145   double t3 = std::pow(t,3);
00146   double T  = T_;
00147   double T2 = std::pow(T_,2);
00148   double T3 = std::pow(T_,3);
00149 
00150   switch (node_value) {
00151     case kPos: return (2*t3)/T3 - (3*t2)/T2 + 1;
00152     case kVel: return t - (2*t2)/T + t3/T2;
00153     default: assert(false); // only derivative wrt nodes values calculated
00154   }
00155 }
00156 
00157 double
00158 CubicHermitePolynomial::GetDerivativeOfVelWrtStartNode (Dx node_value,
00159                                                   double t) const
00160 {
00161   double t2 = std::pow(t,2);
00162   double T  = T_;
00163   double T2 = std::pow(T_,2);
00164   double T3 = std::pow(T_,3);
00165 
00166   switch (node_value) {
00167     case kPos: return (6*t2)/T3 - (6*t)/T2;
00168     case kVel: return (3*t2)/T2 - (4*t)/T + 1;
00169     default: assert(false); // only derivative wrt nodes values calculated
00170   }
00171 }
00172 
00173 double
00174 CubicHermitePolynomial::GetDerivativeOfAccWrtStartNode (Dx node_value,
00175                                                   double t) const
00176 {
00177   double T  = T_;
00178   double T2 = std::pow(T_,2);
00179   double T3 = std::pow(T_,3);
00180 
00181   switch (node_value) {
00182     case kPos: return (12*t)/T3 - 6/T2;
00183     case kVel: return (6*t)/T2 - 4/T;
00184     default: assert(false); // only derivative wrt nodes values calculated
00185   }
00186 }
00187 
00188 double
00189 CubicHermitePolynomial::GetDerivativeOfPosWrtEndNode (Dx node_value,
00190                                                 double t) const
00191 {
00192   double t2 = std::pow(t,2);
00193   double t3 = std::pow(t,3);
00194   double T  = T_;
00195   double T2 = std::pow(T_,2);
00196   double T3 = std::pow(T_,3);
00197 
00198   switch (node_value) {
00199     case kPos: return (3*t2)/T2 - (2*t3)/T3;
00200     case kVel: return t3/T2 - t2/T;
00201     default: assert(false); // only derivative wrt nodes values calculated
00202   }
00203 }
00204 
00205 double
00206 CubicHermitePolynomial::GetDerivativeOfVelWrtEndNode (Dx node_value,
00207                                                 double t) const
00208 {
00209   double t2 = std::pow(t,2);
00210   double T  = T_;
00211   double T2 = std::pow(T_,2);
00212   double T3 = std::pow(T_,3);
00213 
00214   switch (node_value) {
00215     case kPos: return (6*t)/T2 - (6*t2)/T3;
00216     case kVel: return (3*t2)/T2 - (2*t)/T;
00217     default: assert(false); // only derivative wrt nodes values calculated
00218   }
00219 }
00220 
00221 double
00222 CubicHermitePolynomial::GetDerivativeOfAccWrtEndNode (Dx node_value,
00223                                                 double t) const
00224 {
00225   double T  = T_;
00226   double T2 = std::pow(T_,2);
00227   double T3 = std::pow(T_,3);
00228 
00229   switch (node_value) {
00230     case kPos: return 6/T2 - (12*t)/T3;
00231     case kVel: return (6*t)/T2 - 2/T;
00232     default: assert(false); // only derivative wrt nodes values calculated
00233   }
00234 }
00235 
00236 Eigen::VectorXd
00237 CubicHermitePolynomial::GetDerivativeOfPosWrtDuration(double t) const
00238 {
00239   VectorXd x0 = n0_.p();
00240   VectorXd x1 = n1_.p();
00241   VectorXd v0 = n0_.v();
00242   VectorXd v1 = n1_.v();
00243 
00244   double t2 = std::pow(t,2);
00245   double t3 = std::pow(t,3);
00246   double T  = T_;
00247   double T2 = std::pow(T_,2);
00248   double T3 = std::pow(T_,3);
00249   double T4 = std::pow(T_,4);
00250 
00251   VectorXd deriv = (t3*(v0 + v1))/T3
00252                  - (t2*(2*v0 + v1))/T2
00253                  - (3*t3*(2*x0 - 2*x1 + T*v0 + T*v1))/T4
00254                  + (2*t2*(3*x0 - 3*x1 + 2*T*v0 + T*v1))/T3;
00255 
00256   return deriv;
00257 }
00258 
00259 } // namespace towr


towr_core
Author(s): Alexander W. Winkler
autogenerated on Mon Apr 9 2018 03:12:44