Public Member Functions | Private Attributes | List of all members
towr::SingleRigidBodyDynamics Class Reference

Dynamics model relating forces to base accelerations. More...

#include <single_rigid_body_dynamics.h>

Inheritance diagram for towr::SingleRigidBodyDynamics:
Inheritance graph
[legend]

Public Member Functions

BaseAcc GetDynamicViolation () const override
 The violation of the system dynamics incurred by the current values. More...
 
Jac GetJacobianWrtBaseAng (const EulerConverter &base_angular, double t) const override
 How the base orientation affects the dynamic violation. More...
 
Jac GetJacobianWrtBaseLin (const Jac &jac_base_lin_pos, const Jac &jac_acc_base_lin) const override
 How the base position affects the dynamic violation. More...
 
Jac GetJacobianWrtEEPos (const Jac &jac_ee_pos, EE) const override
 How the endeffector positions affect the dynamic violation. More...
 
Jac GetJacobianWrtForce (const Jac &jac_force, EE) const override
 How the endeffector forces affect the dynamic violation. More...
 
 SingleRigidBodyDynamics (double mass, const Eigen::Matrix3d &inertia_b, int ee_count)
 Constructs a specific model. More...
 
 SingleRigidBodyDynamics (double mass, double Ixx, double Iyy, double Izz, double Ixy, double Ixz, double Iyz, int ee_count)
 Constructs a specific model. More...
 
virtual ~SingleRigidBodyDynamics ()=default
 
- Public Member Functions inherited from towr::DynamicModel
double g () const
 
int GetEECount () const
 the number of endeffectors that this robot has. More...
 
double m () const
 
void SetCurrent (const ComPos &com_W, const Vector3d com_acc_W, const Matrix3d &w_R_b, const AngVel &omega_W, const Vector3d &omega_dot_W, const EELoad &force_W, const EEPos &pos_W)
 Sets the current state and input of the system. More...
 

Private Attributes

Eigen::SparseMatrix< double, Eigen::RowMajor > I_b
 

Additional Inherited Members

- Public Types inherited from towr::DynamicModel
using AngVel = Eigen::Vector3d
 
using BaseAcc = Eigen::Matrix< double, 6, 1 >
 
using ComPos = Eigen::Vector3d
 
using EE = uint
 
using EELoad = EEPos
 
using EEPos = std::vector< Eigen::Vector3d >
 
using Jac = Eigen::SparseMatrix< double, Eigen::RowMajor >
 
using Matrix3d = Eigen::Matrix3d
 
using Ptr = std::shared_ptr< DynamicModel >
 
using Vector3d = Eigen::Vector3d
 
- Protected Member Functions inherited from towr::DynamicModel
 DynamicModel (double mass, int ee_count)
 Construct a dynamic object. Protected as this is abstract base class. More...
 
virtual ~DynamicModel ()=default
 
- Protected Attributes inherited from towr::DynamicModel
Vector3d com_acc_
 x-y-z acceleration of the Center-of-Mass. More...
 
ComPos com_pos_
 x-y-z position of the Center-of-Mass. More...
 
EELoad ee_force_
 The endeffector force expressed in world frame. More...
 
EEPos ee_pos_
 The x-y-z position of each endeffector. More...
 
AngVel omega_
 angular velocity expressed in world frame. More...
 
Vector3d omega_dot_
 angular acceleration expressed in world frame. More...
 
Matrix3d w_R_b_
 rotation matrix from base (b) to world (w) frame. More...
 

Detailed Description

Dynamics model relating forces to base accelerations.

This class implements a Single Rigid Body dynamics model, a reduced dimensional model, lying in terms of accuracy between a Linear Inverted Pendulum model and a full Centroidal or Rigid-body-dynamics model.

This model makes the assumption that the motion of the limbs does not incur significant momentum and can therefore be neglected. This eliminates the nonlinear dependency on joint angles and allows to express all quantities in Cartesian space.

For the derivation and all assumptions of this model, see: https://doi.org/10.3929/ethz-b-000272432

See also
https://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations

Definition at line 56 of file single_rigid_body_dynamics.h.

Constructor & Destructor Documentation

towr::SingleRigidBodyDynamics::SingleRigidBodyDynamics ( double  mass,
const Eigen::Matrix3d &  inertia_b,
int  ee_count 
)

Constructs a specific model.

Parameters
massThe mass of the robot.
ee_countThe number of endeffectors/forces.
inertia_bThe elements of the 3x3 Inertia matrix around the CoM. This matrix maps angular accelerations expressed in base frame to moments in base frame.

Definition at line 69 of file single_rigid_body_dynamics.cc.

towr::SingleRigidBodyDynamics::SingleRigidBodyDynamics ( double  mass,
double  Ixx,
double  Iyy,
double  Izz,
double  Ixy,
double  Ixz,
double  Iyz,
int  ee_count 
)

Constructs a specific model.

Parameters
massMass of the robot.
I..Elements of the 3x3 Inertia matrix
ee_countNumber of endeffectors/forces.

Definition at line 59 of file single_rigid_body_dynamics.cc.

virtual towr::SingleRigidBodyDynamics::~SingleRigidBodyDynamics ( )
virtualdefault

Member Function Documentation

SingleRigidBodyDynamics::BaseAcc towr::SingleRigidBodyDynamics::GetDynamicViolation ( ) const
overridevirtual

The violation of the system dynamics incurred by the current values.

Returns
The 6-dimension generalized force violation (angular + linear).

Implements towr::DynamicModel.

Definition at line 77 of file single_rigid_body_dynamics.cc.

SingleRigidBodyDynamics::Jac towr::SingleRigidBodyDynamics::GetJacobianWrtBaseAng ( const EulerConverter base_angular,
double  t 
) const
overridevirtual

How the base orientation affects the dynamic violation.

Parameters
base_angularprovides Euler angles Jacobians.
tTime at which euler angles values are queried.
Returns
The 6xn Jacobian of dynamic violations with respect to variables defining the base angular spline (e.g. node values).

Implements towr::DynamicModel.

Definition at line 124 of file single_rigid_body_dynamics.cc.

SingleRigidBodyDynamics::Jac towr::SingleRigidBodyDynamics::GetJacobianWrtBaseLin ( const Jac jac_base_lin_pos,
const Jac jac_base_lin_acc 
) const
overridevirtual

How the base position affects the dynamic violation.

Parameters
jac_base_lin_posThe 3xn Jacobian of the base linear position.
jac_base_lin_accThe 3xn Jacobian of the base linear acceleration.
Returns
The 6xn Jacobian of dynamic violations with respect to variables defining the base linear spline (e.g. node values).

Implements towr::DynamicModel.

Definition at line 104 of file single_rigid_body_dynamics.cc.

SingleRigidBodyDynamics::Jac towr::SingleRigidBodyDynamics::GetJacobianWrtEEPos ( const Jac ee_pos,
EE  ee 
) const
overridevirtual

How the endeffector positions affect the dynamic violation.

Parameters
ee_forceThe 3xn Jacobian of the foot position x,y,z.
eeThe endeffector for which the senstivity is required.
Returns
The 6xn Jacobian of dynamic violations with respect to variables defining the foot positions (e.g. node values).

Implements towr::DynamicModel.

Definition at line 182 of file single_rigid_body_dynamics.cc.

SingleRigidBodyDynamics::Jac towr::SingleRigidBodyDynamics::GetJacobianWrtForce ( const Jac ee_force,
EE  ee 
) const
overridevirtual

How the endeffector forces affect the dynamic violation.

Parameters
ee_forceThe 3xn Jacobian of the foot force x,y,z.
eeThe endeffector for which the senstivity is required.
Returns
The 6xn Jacobian of dynamic violations with respect to variables defining the endeffector forces (e.g. node values).

Implements towr::DynamicModel.

Definition at line 168 of file single_rigid_body_dynamics.cc.

Member Data Documentation

Eigen::SparseMatrix<double, Eigen::RowMajor> towr::SingleRigidBodyDynamics::I_b
private

Inertia of entire robot around the CoM expressed in a frame anchored in the base.

Definition at line 95 of file single_rigid_body_dynamics.h.


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


towr
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 13 2019 02:28:00