euler_converter.h
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 
30 #ifndef TOWR_VARIABLES_ANGULAR_STATE_CONVERTER_H_
31 #define TOWR_VARIABLES_ANGULAR_STATE_CONVERTER_H_
32 
33 #include <array>
34 
35 #include <Eigen/Dense>
36 #include <Eigen/Sparse>
37 
38 #include "cartesian_dimensions.h"
39 #include "node_spline.h"
40 
41 namespace towr {
42 
63 public:
64  using Vector3d = Eigen::Vector3d;
66  using EulerRates = Vector3d;
67 
68  using JacobianRow = Eigen::SparseVector<double, Eigen::RowMajor>;
69  using MatrixSXd = Eigen::SparseMatrix<double, Eigen::RowMajor>;
71  using JacRowMatrix = std::array<std::array<JacobianRow, k3D>, k3D>;
72 
73  EulerConverter () = default;
74 
90  EulerConverter (const NodeSpline::Ptr& euler_angles);
91  virtual ~EulerConverter () = default;
92 
98  Eigen::Quaterniond GetQuaternionBaseToWorld (double t) const;
99 
105  MatrixSXd GetRotationMatrixBaseToWorld(double t) const;
106 
109 
115  Vector3d GetAngularVelocityInWorld(double t) const;
116 
122  Vector3d GetAngularAccelerationInWorld(double t) const;
123 
131  Jacobian GetDerivOfAngVelWrtEulerNodes(double t) const;
132 
140  Jacobian GetDerivOfAngAccWrtEulerNodes(double t) const;
141 
153  Jacobian DerivOfRotVecMult(double t, const Vector3d& v, bool inverse) const;
154 
156  static Eigen::Quaterniond GetQuaternionBaseToWorld(const EulerAngles& pos);
157 
158 private:
160 
161  // Internal calculations for the conversion from euler rates to angular
162  // velocities and accelerations. These are done using the matrix M defined
163  // here: http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf
170  static MatrixSXd GetM(const EulerAngles& xyz);
171 
175  static MatrixSXd GetMdot(const EulerAngles& xyz, const EulerRates& xyz_d);
176 
185  Jacobian GetDerivMwrtNodes(double t, Dim3D dim) const;
186 
192  Jacobian GetDerivMdotwrtNodes(double t, Dim3D dim) const;
193 
200 
203 
206  const EulerRates& vel);
207 
208  JacobianRow GetJac(double t, Dx deriv, Dim3D dim) const;
210 };
211 
212 } /* namespace towr */
213 
214 #endif /* TOWR_VARIABLES_ANGULAR_STATE_CONVERTER_H_ */
virtual ~EulerConverter()=default
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
Converts Euler angles and derivatives to angular quantities.
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.
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.
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.
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 Fri Apr 2 2021 02:14:16