forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
2.4 KiB
53 lines
2.4 KiB
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "drake/common/eigen_types.h"
|
|
|
|
namespace drake {
|
|
namespace math {
|
|
|
|
/// Given ᴮd/dt(v) (the time derivative in frame B of an arbitrary 3D vector v)
|
|
/// and given ᴬωᴮ (frame B's angular velocity in another frame A), this method
|
|
/// computes ᴬd/dt(v) (the time derivative in frame A of v) by:
|
|
/// ᴬd/dt(v) = ᴮd/dt(v) + ᴬωᴮ x v
|
|
///
|
|
/// This mathematical operation is known as the "Transport Theorem" or the
|
|
/// "Golden Rule for Vector Differentiation" [Mitiguy 2016, §7.3]. It was
|
|
/// discovered by Euler in 1758. Its explicit notation with superscript
|
|
/// frames was invented by Thomas Kane in 1950. Its use as the defining
|
|
/// property of angular velocity was by Mitiguy in 1993.
|
|
///
|
|
/// In source code and comments, we use the following monogram notations:
|
|
/// DtA_v = ᴬd/dt(v) denotes the time derivative in frame A of the vector v.
|
|
/// DtA_v_E = [ᴬd/dt(v)]_E denotes the time derivative in frame A of vector v,
|
|
/// with the resulting new vector quantity expressed in a frame E.
|
|
///
|
|
/// In source code, this mathematical operation is performed with all vectors
|
|
/// expressed in the same frame E as [ᴬd/dt(v)]ₑ = [ᴮd/dt(v)]ₑ + [ᴬωᴮ]ₑ x [v]ₑ
|
|
/// which in monogram notation is: <pre>
|
|
/// DtA_v_E = DtB_v_E + w_AB_E x v_E
|
|
/// </pre>
|
|
///
|
|
/// [Mitiguy 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion Simulation.
|
|
template <typename v_Type, typename DtB_v_Type, typename w_AB_Type>
|
|
Vector3<typename v_Type::Scalar> ConvertTimeDerivativeToOtherFrame(
|
|
const Eigen::MatrixBase<v_Type>& v_E,
|
|
const Eigen::MatrixBase<DtB_v_Type>& DtB_v_E,
|
|
const Eigen::MatrixBase<w_AB_Type>& w_AB_E) {
|
|
// All input vectors must be three dimensional vectors.
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<v_Type>, 3);
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<DtB_v_Type>, 3);
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<w_AB_Type>, 3);
|
|
typedef typename v_Type::Scalar T;
|
|
// All input vectors must be templated on the same scalar type.
|
|
static_assert(std::is_same_v<typename DtB_v_Type::Scalar, T>,
|
|
"DtB_v_E must be templated on the same scalar type as v_E");
|
|
static_assert(std::is_same_v<typename w_AB_Type::Scalar, T>,
|
|
"w_AB_E must be templated on the same scalar type as v_E");
|
|
return DtB_v_E + w_AB_E.cross(v_E);
|
|
}
|
|
|
|
} // namespace math
|
|
} // namespace drake
|