The purpose of the FreeBody class is to provide the data (initial values and gravity) and methods for calculating the exact analytical solution for the translational and rotational motion of a torque-free rigid body B with axially symmetric inertia, in a Newtonian frame (World) N.
Examples of bodies with axially symmetric inertia include cylinders, rods or bars with a circular or square cross section and spinning tops. Since the only external forces on B are uniform gravitational forces, there exists an exact closed-form analytical solution for B's motion. The closed- form rotational solution is available since B is "torque-free", i.e., the moment of all forces about B's mass center is zero. This class calculates the body B's quaternion, angular velocity and angular acceleration expressed in B (body-frame) as well as the position, velocity, acceleration of Bcm (B's center of mass) in N (World). Algorithm from [Kane, 1983] Sections 1.13 and 3.1, Pages 60-62 and 159-169.
|
| FreeBody (const Eigen::Quaterniond &initial_quat_NB, const Eigen::Vector3d &initial_w_NB_B, const Eigen::Vector3d &initial_p_NoBcm_N, const Eigen::Vector3d &initial_v_NBcm_B, const Eigen::Vector3d &gravity_N) |
| Constructs a class that can be queried for exact values of orientation, position, and motion of a torque-free rigid body at time t. More...
|
|
| ~FreeBody ()=default |
|
double | get_I () const |
| Returns body B's moment of inertia about any axis that passes through Bcm (B's center of mass) and is perpendicular to B's inertia symmetry axis. More...
|
|
double | get_J () const |
| Returns body's moment of inertia about the axis that passes through Bcm (B's center of mass) and is parallel to B's inertia symmetry axis. More...
|
|
const Eigen::Quaterniond & | get_initial_quat_NB () const |
|
const Eigen::Vector3d & | get_initial_w_NB_B () const |
|
Eigen::Vector3d | CalcInitial_w_NB_N () const |
|
const Eigen::Vector3d & | get_initial_p_NoBcm_N () const |
|
const Eigen::Vector3d & | get_uniform_gravity_expressed_in_world () const |
|
Eigen::Vector3d | CalcInitial_v_NBcm_N () const |
|
void | set_initial_quat_NB (const Eigen::Quaterniond &quat_NB) |
|
void | set_initial_w_NB_B (const Eigen::Vector3d &w_NB_B) |
|
void | set_initial_p_NoBcm_N (const Eigen::Vector3d &p_NoBcm_N) |
|
void | set_initial_v_NBcm_B (const Eigen::Vector3d &v_NBcm_B) |
|
void | SetUniformGravityExpressedInWorld (const Eigen::Vector3d &gravity) |
|
std::tuple< Eigen::Quaterniond, Eigen::Vector4d, Eigen::Vector3d, Eigen::Vector3d > | CalculateExactRotationalSolutionNB (const double t) const |
| Calculates exact solutions for quaternion and angular velocity expressed in body-frame, and their time derivatives for torque-free rotational motion of axis-symmetric rigid body B in Newtonian frame (World) N, where torque-free means the moment of forces about B's mass center is zero. More...
|
|
std::tuple< Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d > | CalculateExactTranslationalSolution (const double t) const |
| Calculates exact solutions for translational motion of an arbitrary rigid body B in a Newtonian frame (world) N. More...
|
|
std::pair< double, double > | CalcAngularRates_s_p () const |
| Returns angular rates associated with spin s and precession p from the analytical solution [Kane, 1983] for rotational motion (angular velocity and quaternion) for torque-free motion of an axis-symmetric rigid body B in a Newtonian frame (World). More...
|
|
|
| FreeBody (const FreeBody &)=default |
|
FreeBody & | operator= (const FreeBody &)=default |
|
| FreeBody (FreeBody &&)=default |
|
FreeBody & | operator= (FreeBody &&)=default |
|
Returns angular rates associated with spin s
and precession p
from the analytical solution [Kane, 1983] for rotational motion (angular velocity and quaternion) for torque-free motion of an axis-symmetric rigid body B in a Newtonian frame (World).
Kane's solution for B's angular velocity wx*Bx + wy*By + wz*Bz
is in terms of initial values wx0, wy0, wz0 as wx = wx0 * cos(s * t) + wy0 * sin(s * t) wy = -wx0 * sin(s * t) + wy0 * cos(s * t) wz = wz0 For more information, see [Kane, 1983] Pages 60-62 and 159-169.
- Note
- the return value of
s
may be negative, zero, or positive, whereas the return value of p
is nonnegative. The values of s
and p
are returned in units of radian/second.
std::tuple<Eigen::Quaterniond, Eigen::Vector4d, Eigen::Vector3d, Eigen::Vector3d> CalculateExactRotationalSolutionNB |
( |
const double |
t | ) |
const |
Calculates exact solutions for quaternion and angular velocity expressed in body-frame, and their time derivatives for torque-free rotational motion of axis-symmetric rigid body B in Newtonian frame (World) N, where torque-free means the moment of forces about B's mass center is zero.
The quaternion characterizes the orientation between right-handed orthogonal unit vectors Nx, Ny, Nz fixed in N and right-handed orthogonal unit vectors Bx, By, Bz fixed in B, where Bz is parallel to B's symmetry axis.
- Note
- CalculateExactRotationalSolutionABInitiallyAligned() implements the algorithm from [Kane, 1983] Sections 1.13 and 3.1, Pages 60-62 and 159-169.
- Parameters
-
- Returns
- Machine-precision values at time t are returned as defined below.
- Note
- This function allows for initial misalignment of Nx, Ny, Nz and Bx, By, Bz.
std::tuple | Description |
quat_NB | Quaternion relating frame N to frame B: [e0, e1, e2, e3] |
| Note: quat_NB is analogous to the rotation matrix R_NB. quatDt | Time-derivative of ‘quat_NB’, i.e., [ė0, ė1, ė2, ė3]. w_NB_B | B's angular velocity in N, expressed in B. alpha_NB_B | B's angular acceleration in N, expressed in B.