euler_converter.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 namespace towr {
36 
37 
39 {
40  euler_ = euler;
41  jac_wrt_nodes_structure_ = Jacobian(k3D, euler->GetNodeVariablesCount());
42 }
43 
44 Eigen::Quaterniond
46 {
47  State ori = euler_->GetPoint(t);
48  return GetQuaternionBaseToWorld(ori.p());
49 }
50 
51 Eigen::Quaterniond
53 {
54  Eigen::Matrix3d R_WB = GetRotationMatrixBaseToWorld(pos);
55  return Eigen::Quaterniond(R_WB);
56 }
57 
58 Eigen::Vector3d
60 {
61  State ori = euler_->GetPoint(t);
62  return GetAngularVelocityInWorld(ori.p(), ori.v());
63 }
64 
65 Eigen::Vector3d
67  const EulerRates& vel)
68 {
69  return GetM(pos)*vel;
70 }
71 
72 Eigen::Vector3d
74 {
75  State ori = euler_->GetPoint(t);
77 }
78 
79 Eigen::Vector3d
81 {
82  return GetMdot(ori.p(), ori.v())*ori.v() + GetM(ori.p())*ori.a();
83 }
84 
87 {
89 
90  State ori = euler_->GetPoint(t);
91  // convert to sparse, but also regard 0.0 as non-zero element, because
92  // could turn nonzero during the course of the program
93  JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0);
94  Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel);
95 
96  for (auto dim : {X,Y,Z}) {
97  Jacobian dM_du = GetDerivMwrtNodes(t,dim);
98  jac.row(dim) = vel*dM_du + GetM(ori.p()).row(dim)*dVel_du;
99  }
100 
101  return jac;
102 }
103 
106 {
108 
109 
110  State ori = euler_->GetPoint(t);
111  // convert to sparse, but also regard 0.0 as non-zero element, because
112  // could turn nonzero during the course of the program
113  JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0);
114  JacobianRow acc = ori.a().transpose().sparseView(1.0, -1.0);
115 
116  Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel);
117  Jacobian dAcc_du = euler_->GetJacobianWrtNodes(t, kAcc);
118 
119 
120  for (auto dim : {X,Y,Z}) {
121  Jacobian dMdot_du = GetDerivMdotwrtNodes(t,dim);
122  Jacobian dM_du = GetDerivMwrtNodes(t,dim);
123 
124  jac.row(dim) = vel * dMdot_du
125  + GetMdot(ori.p(), ori.v()).row(dim)* dVel_du
126  + acc * dM_du
127  + GetM(ori.p()).row(dim) * dAcc_du;
128  }
129 
130  return jac;
131 }
132 
135 {
136  double z = xyz(Z);
137  double y = xyz(Y);
138 
139  // Euler ZYX rates to angular velocity
140  // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf
141  Jacobian M(k3D, k3D);
142 
143  /* - */ M.coeffRef(0,Y) = -sin(z); M.coeffRef(0,X) = cos(y)*cos(z);
144  /* - */ M.coeffRef(1,Y) = cos(z); M.coeffRef(1,X) = cos(y)*sin(z);
145  M.coeffRef(2,Z) = 1.0; /* - */ M.coeffRef(2,X) = -sin(y);
146 
147  return M;
148 }
149 
152  const EulerRates& xyz_d)
153 {
154  double z = xyz(Z);
155  double zd = xyz_d(Z);
156  double y = xyz(Y);
157  double yd = xyz_d(Y);
158 
159  Jacobian Mdot(k3D, k3D);
160 
161  Mdot.coeffRef(0,Y) = -cos(z)*zd; Mdot.coeffRef(0,X) = -cos(z)*sin(y)*yd - cos(y)*sin(z)*zd;
162  Mdot.coeffRef(1,Y) = -sin(z)*zd; Mdot.coeffRef(1,X) = cos(y)*cos(z)*zd - sin(y)*sin(z)*yd;
163  /* - */ Mdot.coeffRef(2,X) = -cos(y)*yd;
164 
165  return Mdot;
166 }
167 
169 EulerConverter::GetDerivMwrtNodes (double t, Dim3D ang_acc_dim) const
170 {
171  State ori = euler_->GetPoint(t);
172 
173  double z = ori.p()(Z);
174  double y = ori.p()(Y);
175  JacobianRow jac_z = GetJac(t, kPos, Z);
176  JacobianRow jac_y = GetJac(t, kPos, Y);
177 
179 
180  switch (ang_acc_dim) {
181  case X: // basically derivative of top row (3 elements) of matrix M
182  jac.row(Y) = -cos(z)*jac_z;
183  jac.row(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
184  break;
185  case Y: // middle row of M
186  jac.row(Y) = -sin(z)*jac_z;
187  jac.row(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
188  break;
189  case Z: // bottom row of M
190  jac.row(X) = -cos(y)*jac_y;
191  break;
192  default:
193  assert(false);
194  break;
195  }
196 
197  return jac;
198 }
199 
202 {
203  State ori = euler_->GetPoint(t);
204  return GetRotationMatrixBaseToWorld(ori.p());
205 }
206 
209 {
210  double x = xyz(X);
211  double y = xyz(Y);
212  double z = xyz(Z);
213 
214  Eigen::Matrix3d M;
215  // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf (Euler ZYX)
216  M << cos(y)*cos(z), cos(z)*sin(x)*sin(y) - cos(x)*sin(z), sin(x)*sin(z) + cos(x)*cos(z)*sin(y),
217  cos(y)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(x)*sin(y)*sin(z) - cos(z)*sin(x),
218  -sin(y), cos(y)*sin(x), cos(x)*cos(y);
219 
220  return M.sparseView(1.0, -1.0);
221 }
222 
224 EulerConverter::DerivOfRotVecMult (double t, const Vector3d& v, bool inverse) const
225 {
228 
229  for (int row : {X,Y,Z}) {
230  for (int col : {X, Y, Z}) {
231  // since for every rotation matrix R^(-1) = R^T, just swap rows and
232  // columns for calculation of derivative of inverse rotation matrix
233  JacobianRow jac_row = inverse? Rd.at(col).at(row) : Rd.at(row).at(col);
234  jac.row(row) += v(col)*jac_row;
235  }
236  }
237 
238  return jac;
239 }
240 
243 {
244  JacRowMatrix jac;
245 
246  State ori = euler_->GetPoint(t);
247  double x = ori.p()(X);
248  double y = ori.p()(Y);
249  double z = ori.p()(Z);
250 
251  JacobianRow jac_x = GetJac(t, kPos, X);
252  JacobianRow jac_y = GetJac(t, kPos, Y);
253  JacobianRow jac_z = GetJac(t, kPos, Z);
254 
255  jac.at(X).at(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
256  jac.at(X).at(Y) = sin(x)*sin(z)*jac_x - cos(x)*cos(z)*jac_z - sin(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(z)*sin(y)*jac_x + cos(y)*cos(z)*sin(x)*jac_y;
257  jac.at(X).at(Z) = cos(x)*sin(z)*jac_x + cos(z)*sin(x)*jac_z - cos(z)*sin(x)*sin(y)*jac_x - cos(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(y)*cos(z)*jac_y;
258 
259  jac.at(Y).at(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
260  jac.at(Y).at(Y) = cos(x)*sin(y)*sin(z)*jac_x - cos(x)*sin(z)*jac_z - cos(z)*sin(x)*jac_x + cos(y)*sin(x)*sin(z)*jac_y + cos(z)*sin(x)*sin(y)*jac_z;
261  jac.at(Y).at(Z) = sin(x)*sin(z)*jac_z - cos(x)*cos(z)*jac_x - sin(x)*sin(y)*sin(z)*jac_x + cos(x)*cos(y)*sin(z)*jac_y + cos(x)*cos(z)*sin(y)*jac_z;
262 
263  jac.at(Z).at(X) = -cos(y)*jac_y;
264  jac.at(Z).at(Y) = cos(x)*cos(y)*jac_x - sin(x)*sin(y)*jac_y;
265  jac.at(Z).at(Z) = -cos(y)*sin(x)*jac_x - cos(x)*sin(y)*jac_y;
266 
267  return jac;
268 }
269 
271 EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const
272 {
273  State ori = euler_->GetPoint(t);
274 
275  double z = ori.p()(Z);
276  double zd = ori.v()(Z);
277  double y = ori.p()(Y);
278  double yd = ori.v()(Y);
279 
280  JacobianRow jac_z = GetJac(t, kPos, Z);
281  JacobianRow jac_y = GetJac(t, kPos, Y);
282  JacobianRow jac_zd = GetJac(t, kVel, Z);
283  JacobianRow jac_yd = GetJac(t, kVel, Y);
284 
286  switch (ang_acc_dim) {
287  case X: // derivative of top row (3 elements) of matrix M-dot
288  jac.row(Y) = sin(z)*zd*jac_z - cos(z)*jac_zd;
289  jac.row(X) = sin(y)*sin(z)*yd*jac_z - cos(y)*sin(z)*jac_zd - cos(y)*cos(z)*yd*jac_y - cos(y)*cos(z)*zd*jac_z - cos(z)*sin(y)*jac_yd + sin(y)*sin(z)*jac_y*zd;
290  break;
291  case Y: // middle row of M
292  jac.row(Y) = - sin(z)*jac_zd - cos(z)*zd*jac_z;
293  jac.row(X) = cos(y)*cos(z)*jac_zd - sin(y)*sin(z)*jac_yd - cos(y)*sin(z)*yd*jac_y - cos(z)*sin(y)*yd*jac_z - cos(z)*sin(y)*jac_y*zd - cos(y)*sin(z)*zd*jac_z;
294  break;
295  case Z: // bottom Row of M
296  jac.row(X) = sin(y)*yd*jac_y - cos(y)*jac_yd;
297  break;
298  default:
299  assert(false);
300  break;
301  }
302 
303  return jac;
304 }
305 
307 EulerConverter::GetJac (double t, Dx deriv, Dim3D dim) const
308 {
309  return euler_->GetJacobianWrtNodes(t, deriv).row(dim);
310 }
311 
312 } /* namespace towr */
Stores at state comprised of values and higher-order derivatives.
Definition: state.h:49
std::shared_ptr< NodeSpline > Ptr
Definition: node_spline.h:51
JacRowMatrix GetDerivativeOfRotationMatrixWrtNodes(double t) const
matrix of derivatives of each cell w.r.t node values.
Jacobian jac_wrt_nodes_structure_
std::array< std::array< JacobianRow, k3D >, k3D > JacRowMatrix
Eigen::SparseMatrix< double, Eigen::RowMajor > MatrixSXd
static MatrixSXd GetMdot(const EulerAngles &xyz, const EulerRates &xyz_d)
time derivative of GetM()
EulerConverter()=default
Vector3d GetAngularAccelerationInWorld(double t) const
Converts Euler angles, rates and rate derivatives o angular accelerations.
Eigen::SparseVector< double, Eigen::RowMajor > JacobianRow
Jacobian GetDerivOfAngAccWrtEulerNodes(double t) const
Jacobian of the angular acceleration with respect to the Euler nodes.
static MatrixSXd GetM(const EulerAngles &xyz)
Matrix that maps euler rates to angular velocities in world.
const VectorXd p() const
read access to the zero-derivative of the state, e.g. position.
Definition: state.cc:53
Eigen::Vector3d Vector3d
Eigen::Quaterniond GetQuaternionBaseToWorld(double t) const
Converts the Euler angles at time t to a Quaternion.
Vector3d GetAngularVelocityInWorld(double t) const
Converts Euler angles and Euler rates to angular velocities.
Jacobian GetDerivMdotwrtNodes(double t, Dim3D dim) const
Derivative of the dim row of the time derivative of M with respect to the node values.
Vector3d EulerRates
derivative of the above
Vector3d EulerAngles
roll, pitch, yaw.
static constexpr int k3D
MatrixSXd GetRotationMatrixBaseToWorld(double t) const
Converts the Euler angles at time t to a rotation matrix.
Jacobian GetDerivOfAngVelWrtEulerNodes(double t) const
Jacobian of the angular velocity with respect to the Euler nodes.
const VectorXd v() const
read access to the first-derivative of the state, e.g. velocity.
Definition: state.cc:59
Jacobian GetDerivMwrtNodes(double t, Dim3D dim) const
Derivative of the dim row of matrix M with respect to the node values.
Jacobian DerivOfRotVecMult(double t, const Vector3d &v, bool inverse) const
Returns the derivative of result of the linear equation M*v.
const VectorXd a() const
read access to the second-derivative of the state, e.g. acceleration.
Definition: state.cc:65
NodeSpline::Ptr euler_
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41
JacobianRow GetJac(double t, Dx deriv, Dim3D dim) const


towr
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 13 2019 02:28:00