polynomial.cc
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
31 
32 #include <cassert>
33 #include <cmath>
34 
35 
36 namespace towr {
37 
38 Polynomial::Polynomial (int order, int dim)
39 {
40  int n_coeff = order+1;
41  for (int c=A; c<n_coeff; ++c) {
42  coeff_ids_.push_back(static_cast<Coefficients>(c));
43  coeff_.push_back(VectorXd::Zero(dim));
44  }
45 }
46 
47 State Polynomial::GetPoint(double t_local) const
48 {
49  // sanity checks
50  if (t_local < 0.0)
51  assert(false);//("spliner.cc called with dt<0")
52 
53  int n_dim = coeff_.front().size();
54  State out(n_dim, 3);
55 
56  for (auto d : {kPos, kVel, kAcc})
57  for (Coefficients c : coeff_ids_)
58  out.at(d) += GetDerivativeWrtCoeff(t_local, d, c)*coeff_.at(c);
59 
60  return out;
61 }
62 
63 double
65 {
66  switch (deriv) {
67  case kPos: return std::pow(t,c); break;
68  case kVel: return c>=B? c* std::pow(t,c-1) : 0.0; break;
69  case kAcc: return c>=C? c*(c-1)*std::pow(t,c-2) : 0.0; break;
70  default: assert(false); // derivative not defined
71  }
72 }
73 
74 
75 
77  : Polynomial(3,dim),
78  n0_(dim),
79  n1_(dim)
80 {
81  T_ = 0.0;
82 }
83 
84 void
86 {
87  n0_ = n0;
88  n1_ = n1;
89 }
90 
91 void
93 {
94  T_ = duration;
95 }
96 
97 void
99 {
100  coeff_[A] = n0_.p();
101  coeff_[B] = n0_.v();
102  coeff_[C] = -( 3*(n0_.p() - n1_.p()) + T_*(2*n0_.v() + n1_.v()) ) / std::pow(T_,2);
103  coeff_[D] = ( 2*(n0_.p() - n1_.p()) + T_*( n0_.v() + n1_.v()) ) / std::pow(T_,3);
104 }
105 
106 double
108  Dx node_derivative, // pos or velocity node
109  double t_local) const
110 {
111  switch (dfdt) {
112  case kPos:
113  return GetDerivativeOfPosWrtStartNode(node_derivative, t_local);
114  case kVel:
115  return GetDerivativeOfVelWrtStartNode(node_derivative, t_local);
116  case kAcc:
117  return GetDerivativeOfAccWrtStartNode(node_derivative, t_local);
118  default:
119  assert(false); // derivative not yet implemented
120  }
121 }
122 
123 double
125  Dx node_derivative, // pos or velocity node
126  double t_local) const
127 {
128  switch (dfdt) {
129  case kPos:
130  return GetDerivativeOfPosWrtEndNode(node_derivative, t_local);
131  case kVel:
132  return GetDerivativeOfVelWrtEndNode(node_derivative, t_local);
133  case kAcc:
134  return GetDerivativeOfAccWrtEndNode(node_derivative, t_local);
135  default:
136  assert(false); // derivative not yet implemented
137  }
138 }
139 
140 double
142  double t) const
143 {
144  double t2 = std::pow(t,2);
145  double t3 = std::pow(t,3);
146  double T = T_;
147  double T2 = std::pow(T_,2);
148  double T3 = std::pow(T_,3);
149 
150  switch (node_value) {
151  case kPos: return (2*t3)/T3 - (3*t2)/T2 + 1;
152  case kVel: return t - (2*t2)/T + t3/T2;
153  default: assert(false); // only derivative wrt nodes values calculated
154  }
155 }
156 
157 double
159  double t) const
160 {
161  double t2 = std::pow(t,2);
162  double T = T_;
163  double T2 = std::pow(T_,2);
164  double T3 = std::pow(T_,3);
165 
166  switch (node_value) {
167  case kPos: return (6*t2)/T3 - (6*t)/T2;
168  case kVel: return (3*t2)/T2 - (4*t)/T + 1;
169  default: assert(false); // only derivative wrt nodes values calculated
170  }
171 }
172 
173 double
175  double t) const
176 {
177  double T = T_;
178  double T2 = std::pow(T_,2);
179  double T3 = std::pow(T_,3);
180 
181  switch (node_value) {
182  case kPos: return (12*t)/T3 - 6/T2;
183  case kVel: return (6*t)/T2 - 4/T;
184  default: assert(false); // only derivative wrt nodes values calculated
185  }
186 }
187 
188 double
190  double t) const
191 {
192  double t2 = std::pow(t,2);
193  double t3 = std::pow(t,3);
194  double T = T_;
195  double T2 = std::pow(T_,2);
196  double T3 = std::pow(T_,3);
197 
198  switch (node_value) {
199  case kPos: return (3*t2)/T2 - (2*t3)/T3;
200  case kVel: return t3/T2 - t2/T;
201  default: assert(false); // only derivative wrt nodes values calculated
202  }
203 }
204 
205 double
207  double t) const
208 {
209  double t2 = std::pow(t,2);
210  double T = T_;
211  double T2 = std::pow(T_,2);
212  double T3 = std::pow(T_,3);
213 
214  switch (node_value) {
215  case kPos: return (6*t)/T2 - (6*t2)/T3;
216  case kVel: return (3*t2)/T2 - (2*t)/T;
217  default: assert(false); // only derivative wrt nodes values calculated
218  }
219 }
220 
221 double
223  double t) const
224 {
225  double T = T_;
226  double T2 = std::pow(T_,2);
227  double T3 = std::pow(T_,3);
228 
229  switch (node_value) {
230  case kPos: return 6/T2 - (12*t)/T3;
231  case kVel: return (6*t)/T2 - 2/T;
232  default: assert(false); // only derivative wrt nodes values calculated
233  }
234 }
235 
238 {
239  VectorXd x0 = n0_.p();
240  VectorXd x1 = n1_.p();
241  VectorXd v0 = n0_.v();
242  VectorXd v1 = n1_.v();
243 
244  double t2 = std::pow(t,2);
245  double t3 = std::pow(t,3);
246  double T = T_;
247  double T2 = std::pow(T_,2);
248  double T3 = std::pow(T_,3);
249  double T4 = std::pow(T_,4);
250 
251  VectorXd deriv = (t3*(v0 + v1))/T3
252  - (t2*(2*v0 + v1))/T2
253  - (3*t3*(2*x0 - 2*x1 + T*v0 + T*v1))/T4
254  + (2*t2*(3*x0 - 3*x1 + 2*T*v0 + T*v1))/T3;
255 
256  return deriv;
257 }
258 
259 } // namespace towr
Stores at state comprised of values and higher-order derivatives.
Definition: state.h:49
const VectorXd at(Dx deriv) const
Read the state value or it&#39;s derivatives by index.
Definition: state.cc:41
A polynomial of arbitrary order and dimension.
Definition: polynomial.h:53
double T_
the total duration of the polynomial.
Definition: polynomial.h:157
double GetDerivativeOfAccWrtEndNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:222
void SetNodes(const Node &n0, const Node &n1)
Fully defines the polynomial by the node values using current duration.
Definition: polynomial.cc:85
Eigen::VectorXd VectorXd
Node n1_
the start and final node comprising the polynomial.
Definition: polynomial.h:161
double GetDerivativeOfVelWrtEndNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:206
double GetDerivativeOfAccWrtStartNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:174
CoeffIDVec coeff_ids_
Definition: polynomial.h:86
const VectorXd p() const
read access to the zero-derivative of the state, e.g. position.
Definition: state.cc:53
Polynomial(int poly_order, int poly_dim)
Constructs a polynomial with zero coefficient values.
Definition: polynomial.cc:38
Eigen::VectorXd VectorXd
Definition: polynomial.h:57
void SetDuration(double duration)
sets the total duration of the polynomial.
Definition: polynomial.cc:92
void UpdateCoeff()
updates the coefficients using current nodes and durations.
Definition: polynomial.cc:98
double GetDerivativeWrtStartNode(Dx dfdt, Dx node_deriv, double t) const
The derivative of the polynomial when changing the start node.
Definition: polynomial.cc:107
double GetDerivativeWrtCoeff(double t, Dx poly_deriv, Coefficients coeff) const
The derivative of the polynomial with respect to the coefficients.
Definition: polynomial.cc:64
double GetDerivativeWrtEndNode(Dx dfdt, Dx node_deriv, double t) const
The derivative of the polynomial when changing the end node.
Definition: polynomial.cc:124
std::vector< VectorXd > coeff_
Definition: polynomial.h:83
const VectorXd v() const
read access to the first-derivative of the state, e.g. velocity.
Definition: state.cc:59
double GetDerivativeOfVelWrtStartNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:158
double GetDerivativeOfPosWrtEndNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:189
double GetDerivativeOfPosWrtStartNode(Dx node_deriv, double t_local) const
Definition: polynomial.cc:141
State GetPoint(double t) const
Definition: polynomial.cc:47
VectorXd GetDerivativeOfPosWrtDuration(double t) const
How the total duration affect the value ("pos") of the polynomial.
Definition: polynomial.cc:237
A node represents the state of a trajectory at a specific time.
Definition: state.h:107
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41


towr
Author(s): Alexander W. Winkler
autogenerated on Fri Apr 2 2021 02:14:16