Drake
FreeBody Class Reference

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 toque-free rigid body B with axially symmetric inertia, in a Newtonian frame (World) N. More...

#include <drake/multibody/benchmarks/free_body/free_body.h>

Public Member Functions

 FreeBody (const Eigen::Quaterniond &quat_NB_initial, const Eigen::Vector3d &w_NB_B_initial, const Eigen::Vector3d &p_NoBcm_N_initial, const Eigen::Vector3d &v_NBcm_B_initial, const Eigen::Vector3d &gravity_N)
 
 ~FreeBody ()=default
 
double get_I () const
 Returns the body's moment of inertia about an axis perpendicular to its axis of rotation and passing through its center of mass. More...
 
double get_J () const
 Returns body's moment of inertia about its axis of rotation. More...
 
const Eigen::Quaterniond & get_quat_NB_initial () const
 
const Eigen::Vector3d & get_w_NB_B_initial () const
 
const Eigen::Vector3d & get_p_NoBcm_N_initial () const
 
const Eigen::Vector3d & get_v_NBcm_B_initial () const
 
const Eigen::Vector3d & get_uniform_gravity_expressed_in_world () const
 
Eigen::Vector3d GetInitialVelocityOfBcmInWorldExpressedInWorld () const
 
void set_quat_NB_initial (const Eigen::Quaterniond &quat_NB_initial)
 
void set_w_NB_B_initial (const Eigen::Vector3d &w_NB_B_initial)
 
void set_p_NoBcm_N_initial (const Eigen::Vector3d &p_NoBcm_N_initial)
 
void set_v_NBcm_B_initial (const Eigen::Vector3d &v_NBcm_B_initial)
 
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...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 FreeBody (const FreeBody &)=default
 
FreeBodyoperator= (const FreeBody &)=default
 
 FreeBody (FreeBody &&)=default
 
FreeBodyoperator= (FreeBody &&)=default
 

Detailed Description

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 toque-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.

Constructor & Destructor Documentation

FreeBody ( const FreeBody )
default
FreeBody ( FreeBody &&  )
default
FreeBody ( const Eigen::Quaterniond &  quat_NB_initial,
const Eigen::Vector3d &  w_NB_B_initial,
const Eigen::Vector3d &  p_NoBcm_N_initial,
const Eigen::Vector3d &  v_NBcm_B_initial,
const Eigen::Vector3d &  gravity_N 
)
inline

Here is the call graph for this function:

~FreeBody ( )
default

Here is the caller graph for this function:

Member Function Documentation

std::tuple< Quaterniond, Vector4d, Vector3d, 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
tCurrent value of time.
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.

Here is the call graph for this function:

Here is the caller graph for this function:

std::tuple< Vector3d, Vector3d, Vector3d > CalculateExactTranslationalSolution ( const double  t) const

Calculates exact solutions for translational motion of an arbitrary rigid body B in a Newtonian frame (world) N.

Algorithm from high-school physics.

Parameters
tCurrent value of time.
Returns
Machine-precision values at time t are returned as defined below.
std::tuple Description
xyz Vector3d [x, y, z], Bcm's position from No, expressed in N.
xyzDt Vector3d [ẋ, ẏ, ż] Bcm's velocity in N, expressed in N.
xyzDDt Vector3d [ẍ ÿ z̈], Bcm's acceleration in N, expressed in N.

Here is the call graph for this function:

Here is the caller graph for this function:

double get_I ( ) const
inline

Returns the body's moment of inertia about an axis perpendicular to its axis of rotation and passing through its center of mass.

For example, for a cylinder of radius r, length h and uniformly distributed mass m with its rotational axis aligined along its body frame z-axis this would be:

  I = Ixx = Iyy = m / 12 (3 r² + h²)
double get_J ( ) const
inline

Returns body's moment of inertia about its axis of rotation.

For example, for a cylinder of radius r, length h and uniformly distributed mass m with its rotational axis aligined along its body frame z-axis this would be:

  J = Izz = m r² / 2
const Eigen::Vector3d& get_p_NoBcm_N_initial ( ) const
inline

Here is the caller graph for this function:

const Eigen::Quaterniond& get_quat_NB_initial ( ) const
inline

Here is the caller graph for this function:

const Eigen::Vector3d& get_uniform_gravity_expressed_in_world ( ) const
inline

Here is the caller graph for this function:

const Eigen::Vector3d& get_v_NBcm_B_initial ( ) const
inline

Here is the caller graph for this function:

const Eigen::Vector3d& get_w_NB_B_initial ( ) const
inline

Here is the caller graph for this function:

Eigen::Vector3d GetInitialVelocityOfBcmInWorldExpressedInWorld ( ) const
inline

Here is the caller graph for this function:

FreeBody& operator= ( FreeBody &&  )
default
FreeBody& operator= ( const FreeBody )
default
void set_p_NoBcm_N_initial ( const Eigen::Vector3d &  p_NoBcm_N_initial)
inline
void set_quat_NB_initial ( const Eigen::Quaterniond &  quat_NB_initial)
inline

Here is the caller graph for this function:

void set_v_NBcm_B_initial ( const Eigen::Vector3d &  v_NBcm_B_initial)
inline
void set_w_NB_B_initial ( const Eigen::Vector3d &  w_NB_B_initial)
inline
void SetUniformGravityExpressedInWorld ( const Eigen::Vector3d &  gravity)
inline

Here is the call graph for this function:


The documentation for this class was generated from the following files: