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 
112  Vector3d GetAngularVelocityInWorld(double t) const;
113 
119  Vector3d GetAngularAccelerationInWorld(double t) const;
120 
128  Jacobian GetDerivOfAngVelWrtEulerNodes(double t) const;
129 
137  Jacobian GetDerivOfAngAccWrtEulerNodes(double t) const;
138 
150  Jacobian DerivOfRotVecMult(double t, const Vector3d& v, bool inverse) const;
151 private:
153 
154  // Internal calculations for the conversion from euler rates to angular
155  // velocities and accelerations. These are done using the matrix M defined
156  // here: http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf
163  static MatrixSXd GetM(const EulerAngles& xyz);
164 
168  static MatrixSXd GetMdot(const EulerAngles& xyz, const EulerRates& xyz_d);
169 
178  Jacobian GetDerivMwrtNodes(double t, Dim3D dim) const;
179 
185  Jacobian GetDerivMdotwrtNodes(double t, Dim3D dim) const;
186 
193 
196 
198  static Eigen::Quaterniond GetQuaternionBaseToWorld(const EulerAngles& pos);
199 
202 
205  const EulerRates& vel);
206 
207  JacobianRow GetJac(double t, Dx deriv, Dim3D dim) const;
209 };
210 
211 } /* namespace towr */
212 
213 #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: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
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_core
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 7 2018 02:15:57