RotationKinematics.hpp

Go to the documentation of this file.

// Vulcan Rotation Kinematics
// Angular velocity and rotation rate relationships
#pragma once
 
#include <vulcan/core/VulcanTypes.hpp>
#include <vulcan/rotations/AxisAngle.hpp>
#include <vulcan/rotations/DCMUtils.hpp>
 
#include <janus/math/Quaternion.hpp>
 
namespace vulcan {
 
// =============================================================================
// Quaternion Rate <-> Angular Velocity
// =============================================================================

template <typename Scalar>
Vec3<Scalar>
omega_from_quaternion_rate(const janus::Quaternion<Scalar> &q,
                           const janus::Quaternion<Scalar> &q_dot) {
    // omega_body = 2 * q* * q_dot
    auto omega_quat = q.conjugate() * q_dot;
    Scalar two = Scalar(2);
 
    Vec3<Scalar> omega;
    omega(0) = two * omega_quat.x;
    omega(1) = two * omega_quat.y;
    omega(2) = two * omega_quat.z;
    return omega;
}

template <typename Scalar>
janus::Quaternion<Scalar>
quaternion_rate_from_omega(const janus::Quaternion<Scalar> &q,
                           const Vec3<Scalar> &omega_body) {
    // q_dot = 0.5 * q * (0, omega)
    janus::Quaternion<Scalar> omega_quat(Scalar(0), omega_body(0),
                                         omega_body(1), omega_body(2));
    Scalar half = Scalar(0.5);
    auto q_dot = q * omega_quat;
    return janus::Quaternion<Scalar>(half * q_dot.w, half * q_dot.x,
                                     half * q_dot.y, half * q_dot.z);
}
 
// =============================================================================
// DCM Rate <-> Angular Velocity
// =============================================================================

template <typename Scalar>
Vec3<Scalar> omega_from_dcm_rate(const Mat3<Scalar> &R,
                                 const Mat3<Scalar> &R_dot) {
    Mat3<Scalar> omega_skew = R.transpose() * R_dot;
    return unskew(omega_skew);
}

template <typename Scalar>
Mat3<Scalar> dcm_rate_from_omega(const Mat3<Scalar> &R,
                                 const Vec3<Scalar> &omega_body) {
    return R * skew(omega_body);
}
 
// =============================================================================
// Rotation Composition
// =============================================================================

template <typename Scalar>
janus::Quaternion<Scalar>
compose_rotations(const janus::Quaternion<Scalar> &q1,
                  const janus::Quaternion<Scalar> &q2) {
    return q2 * q1;
}

template <typename Scalar>
janus::Quaternion<Scalar>
relative_rotation(const janus::Quaternion<Scalar> &q_from,
                  const janus::Quaternion<Scalar> &q_to) {
    return q_to * q_from.conjugate();
}

template <typename Scalar>
Vec3<Scalar> rotation_error(const janus::Quaternion<Scalar> &q_actual,
                            const janus::Quaternion<Scalar> &q_desired) {
    // q_error = q_desired * q_actual^(-1)
    auto q_error = q_desired * q_actual.conjugate();
    return rotation_vector_from_quaternion(q_error);
}
 
} // namespace vulcan