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 
122  Jacobian dMdot_du = GetDerivMdotwrtNodes(t,dim);
123  Jacobian dM_du = GetDerivMwrtNodes(t,dim);
124 
125  jac.row(dim) = vel * dMdot_du
126  + GetMdot(ori.p(), ori.v()).row(dim)* dVel_du
127  + acc * dM_du
128  + GetM(ori.p()).row(dim) * dAcc_du;
129  }
130 
131  return jac;
132 }
133 
136 {
137  double z = xyz(Z);
138  double y = xyz(Y);
139 
140  // Euler ZYX rates to angular velocity
141  // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf
142  Jacobian M(k3D, k3D);
143 
144  /* - */ M.coeffRef(0,Y) = -sin(z); M.coeffRef(0,X) = cos(y)*cos(z);
145  /* - */ M.coeffRef(1,Y) = cos(z); M.coeffRef(1,X) = cos(y)*sin(z);
146  M.coeffRef(2,Z) = 1.0; /* - */ M.coeffRef(2,X) = -sin(y);
147 
148  return M;
149 }
150 
153  const EulerRates& xyz_d)
154 {
155  double z = xyz(Z);
156  double zd = xyz_d(Z);
157  double y = xyz(Y);
158  double yd = xyz_d(Y);
159 
160  Jacobian Mdot(k3D, k3D);
161 
162  Mdot.coeffRef(0,Y) = -cos(z)*zd; Mdot.coeffRef(0,X) = -cos(z)*sin(y)*yd - cos(y)*sin(z)*zd;
163  Mdot.coeffRef(1,Y) = -sin(z)*zd; Mdot.coeffRef(1,X) = cos(y)*cos(z)*zd - sin(y)*sin(z)*yd;
164  /* - */ Mdot.coeffRef(2,X) = -cos(y)*yd;
165 
166  return Mdot;
167 }
168 
170 EulerConverter::GetDerivMwrtNodes (double t, Dim3D ang_acc_dim) const
171 {
172  State ori = euler_->GetPoint(t);
173 
174  double z = ori.p()(Z);
175  double y = ori.p()(Y);
176  JacobianRow jac_z = GetJac(t, kPos, Z);
177  JacobianRow jac_y = GetJac(t, kPos, Y);
178 
180 
181  switch (ang_acc_dim) {
182  case X: // basically derivative of top row (3 elements) of matrix M
183  jac.row(Y) = -cos(z)*jac_z;
184  jac.row(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
185  break;
186  case Y: // middle row of M
187  jac.row(Y) = -sin(z)*jac_z;
188  jac.row(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
189  break;
190  case Z: // bottom row of M
191  jac.row(X) = -cos(y)*jac_y;
192  break;
193  default:
194  assert(false);
195  break;
196  }
197 
198  return jac;
199 }
200 
203 {
204  State ori = euler_->GetPoint(t);
205  return GetRotationMatrixBaseToWorld(ori.p());
206 }
207 
210 {
211  double x = xyz(X);
212  double y = xyz(Y);
213  double z = xyz(Z);
214 
215  Eigen::Matrix3d M;
216  // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf (Euler ZYX)
217  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),
218  cos(y)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(x)*sin(y)*sin(z) - cos(z)*sin(x),
219  -sin(y), cos(y)*sin(x), cos(x)*cos(y);
220 
221  return M.sparseView(1.0, -1.0);
222 }
223 
225 EulerConverter::DerivOfRotVecMult (double t, const Vector3d& v, bool inverse) const
226 {
229 
230  for (int row : {X,Y,Z}) {
231  for (int col : {X, Y, Z}) {
232 
233  // since for every rotation matrix R^(-1) = R^T, just swap rows and
234  // columns for calculation of derivative of inverse rotation matrix
235  JacobianRow jac_row = inverse? Rd.at(col).at(row) : Rd.at(row).at(col);
236  jac.row(row) += v(col)*jac_row;
237  }
238  }
239 
240  return jac;
241 }
242 
245 {
246  JacRowMatrix jac;
247 
248  State ori = euler_->GetPoint(t);
249  double x = ori.p()(X);
250  double y = ori.p()(Y);
251  double z = ori.p()(Z);
252 
253  JacobianRow jac_x = GetJac(t, kPos, X);
254  JacobianRow jac_y = GetJac(t, kPos, Y);
255  JacobianRow jac_z = GetJac(t, kPos, Z);
256 
257  jac.at(X).at(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
258  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;
259  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;
260 
261  jac.at(Y).at(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
262  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;
263  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;
264 
265  jac.at(Z).at(X) = -cos(y)*jac_y;
266  jac.at(Z).at(Y) = cos(x)*cos(y)*jac_x - sin(x)*sin(y)*jac_y;
267  jac.at(Z).at(Z) = -cos(y)*sin(x)*jac_x - cos(x)*sin(y)*jac_y;
268 
269  return jac;
270 }
271 
273 EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const
274 {
275  State ori = euler_->GetPoint(t);
276 
277  double z = ori.p()(Z);
278  double zd = ori.v()(Z);
279  double y = ori.p()(Y);
280  double yd = ori.v()(Y);
281 
282  JacobianRow jac_z = GetJac(t, kPos, Z);
283  JacobianRow jac_y = GetJac(t, kPos, Y);
284  JacobianRow jac_zd = GetJac(t, kVel, Z);
285  JacobianRow jac_yd = GetJac(t, kVel, Y);
286 
288  switch (ang_acc_dim) {
289  case X: // derivative of top row (3 elements) of matrix M-dot
290  jac.row(Y) = sin(z)*zd*jac_z - cos(z)*jac_zd;
291  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;
292  break;
293  case Y: // middle row of M
294  jac.row(Y) = - sin(z)*jac_zd - cos(z)*zd*jac_z;
295  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;
296  break;
297  case Z: // bottom Row of M
298  jac.row(X) = sin(y)*yd*jac_y - cos(y)*jac_yd;
299  break;
300  default:
301  assert(false);
302  break;
303  }
304 
305  return jac;
306 }
307 
309 EulerConverter::GetJac (double t, Dx deriv, Dim3D dim) const
310 {
311  return euler_->GetJacobianWrtNodes(t, deriv).row(dim);
312 }
313 
314 } /* 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:52
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
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
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
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_core
Author(s): Alexander W. Winkler
autogenerated on Sun Apr 8 2018 02:18:53