Public Member Functions | Protected Member Functions | Protected Attributes | Private Attributes
towr::DynamicModel Class Reference

A interface for the the system dynamics of a legged robot. More...

#include <dynamic_model.h>

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

List of all members.

Public Member Functions

double g () const
virtual BaseAcc GetDynamicViolation () const =0
 The violation of the system dynamics incurred by the current values.
int GetEECount () const
 the number of endeffectors that this robot has.
virtual Jac GetJacobianWrtBaseAng (const EulerConverter &base_angular, double t) const =0
 How the base orientation affects the dynamic violation.
virtual Jac GetJacobianWrtBaseLin (const Jac &jac_base_lin_pos, const Jac &jac_base_lin_acc) const =0
 How the base position affects the dynamic violation.
virtual Jac GetJacobianWrtEEPos (const Jac &ee_pos, EE ee) const =0
 How the endeffector positions affect the dynamic violation.
virtual Jac GetJacobianWrtForce (const Jac &ee_force, EE ee) const =0
 How the endeffector forces affect the dynamic violation.
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.

Protected Member Functions

 DynamicModel (double mass, int ee_count)
 Construct a dynamic object. Protected as this is abstract base class.
virtual ~DynamicModel ()

Protected Attributes

Vector3d com_acc_
 x-y-z acceleration of the Center-of-Mass.
ComPos com_pos_
 x-y-z position of the Center-of-Mass.
EELoad ee_force_
 The endeffector force expressed in world frame.
EEPos ee_pos_
 The x-y-z position of each endeffector.
AngVel omega_
 angular velocity expressed in world frame.
Vector3d omega_dot_
 angular acceleration expressed in world frame.
Matrix3d w_R_b_
 rotation matrix from base (b) to world (w) frame.

Private Attributes

double g_
 gravity acceleration [m/s^2]
double m_
 mass of the robot

Detailed Description

A interface for the the system dynamics of a legged robot.

This class is responsible for verifying that the current acceleration of a system given a specific robot state and input forces ensure the system dynamics, so g = xdd(t) - f(x(t), f(t)).

Dynamic models can for example be: Linear Inverted Pendulum (LIP), SingleRigidBodyDynamics (SRBD), Centroidal Dynamics, Full Rigid Body Dynamics (RBD).

An overview of all these models can be found here: https://doi.org/10.3929/ethz-b-000272432

This model is used in DynamicConstraint to ensure that the optimized motion trajectory complies to this. Currently, only SingleRigidBodyDynamics is implemented.

Definition at line 66 of file dynamic_model.h.


Constructor & Destructor Documentation

towr::DynamicModel::DynamicModel ( double  mass,
int  ee_count 
) [protected]

Construct a dynamic object. Protected as this is abstract base class.

Parameters:
massThe mass of the system.

Definition at line 34 of file dynamic_model.cc.

virtual towr::DynamicModel::~DynamicModel ( ) [protected, virtual]

Member Function Documentation

double towr::DynamicModel::g ( ) const [inline]
Returns:
The gravity acceleration [m/s^2] (positive)

Definition at line 144 of file dynamic_model.h.

virtual BaseAcc towr::DynamicModel::GetDynamicViolation ( ) const [pure virtual]

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

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

Implemented in towr::SingleRigidBodyDynamics.

int towr::DynamicModel::GetEECount ( ) const [inline]

the number of endeffectors that this robot has.

Definition at line 154 of file dynamic_model.h.

virtual Jac towr::DynamicModel::GetJacobianWrtBaseAng ( const EulerConverter base_angular,
double  t 
) const [pure virtual]

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

Implemented in towr::SingleRigidBodyDynamics.

virtual Jac towr::DynamicModel::GetJacobianWrtBaseLin ( const Jac &  jac_base_lin_pos,
const Jac &  jac_base_lin_acc 
) const [pure virtual]

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

Implemented in towr::SingleRigidBodyDynamics.

virtual Jac towr::DynamicModel::GetJacobianWrtEEPos ( const Jac &  ee_pos,
EE  ee 
) const [pure virtual]

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

Implemented in towr::SingleRigidBodyDynamics.

virtual Jac towr::DynamicModel::GetJacobianWrtForce ( const Jac &  ee_force,
EE  ee 
) const [pure virtual]

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

Implemented in towr::SingleRigidBodyDynamics.

double towr::DynamicModel::m ( ) const [inline]
Returns:
The mass of the robot [kg].

Definition at line 149 of file dynamic_model.h.

void towr::DynamicModel::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.

Parameters:
com_WCurrent Center-of-Mass (x,y,z) position in world frame.
com_acc_WCurrent Center-of-Mass (x,y,z) acceleration in world.
w_R_bCurrent rotation from base to world frame.
omega_WCurrent angular velocity in world frame.
omega_dot_WCurrent angular acceleration in world frame.
force_WForce at each foot expressed in world frame.
pos_WPosition of each foot expressed in world frame

Definition at line 51 of file dynamic_model.cc.


Member Data Documentation

Vector3d towr::DynamicModel::com_acc_ [protected]

x-y-z acceleration of the Center-of-Mass.

Definition at line 158 of file dynamic_model.h.

ComPos towr::DynamicModel::com_pos_ [protected]

x-y-z position of the Center-of-Mass.

Definition at line 154 of file dynamic_model.h.

EELoad towr::DynamicModel::ee_force_ [protected]

The endeffector force expressed in world frame.

Definition at line 165 of file dynamic_model.h.

EEPos towr::DynamicModel::ee_pos_ [protected]

The x-y-z position of each endeffector.

Definition at line 164 of file dynamic_model.h.

double towr::DynamicModel::g_ [private]

gravity acceleration [m/s^2]

Definition at line 175 of file dynamic_model.h.

double towr::DynamicModel::m_ [private]

mass of the robot

Definition at line 176 of file dynamic_model.h.

AngVel towr::DynamicModel::omega_ [protected]

angular velocity expressed in world frame.

Definition at line 161 of file dynamic_model.h.

Vector3d towr::DynamicModel::omega_dot_ [protected]

angular acceleration expressed in world frame.

Definition at line 162 of file dynamic_model.h.

Matrix3d towr::DynamicModel::w_R_b_ [protected]

rotation matrix from base (b) to world (w) frame.

Definition at line 160 of file dynamic_model.h.


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


towr
Author(s): Alexander W. Winkler
autogenerated on Mon Apr 15 2019 02:42:32