#include <FloatingBaseModel.h>

Inheritance diagram for FloatingBaseModel< T >:

Collaboration diagram for FloatingBaseModel< T >:

Public Member Functions
	FloatingBaseModel ()

	~FloatingBaseModel ()

void	addBase (const SpatialInertia< T > &inertia)

void	addBase (T mass, const Vec3< T > &com, const Mat3< T > &I)

int	addGroundContactPoint (int bodyID, const Vec3< T > &location, bool isFoot=false)

void	addGroundContactBoxPoints (int bodyId, const Vec3< T > &dims)

int	addBody (const SpatialInertia< T > &inertia, const SpatialInertia< T > &rotorInertia, T gearRatio, int parent, JointType jointType, CoordinateAxis jointAxis, const Mat6< T > &Xtree, const Mat6< T > &Xrot)

int	addBody (const MassProperties< T > &inertia, const MassProperties< T > &rotorInertia, T gearRatio, int parent, JointType jointType, CoordinateAxis jointAxis, const Mat6< T > &Xtree, const Mat6< T > &Xrot)

void	check ()

T	totalRotorMass ()

T	totalNonRotorMass ()

const std::vector< int > &	getParentVector ()

const std::vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > > &	getBodyInertiaVector ()

const std::vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > > &	getRotorInertiaVector ()

void	setGravity (Vec3< T > &g)

void	setContactComputeFlag (size_t gc_index, bool flag)

DMat< T >	invContactInertia (const int gc_index, const D6Mat< T > &force_directions)

T	invContactInertia (const int gc_index, const Vec3< T > &force_ics_at_contact)

T	applyTestForce (const int gc_index, const Vec3< T > &force_ics_at_contact, FBModelStateDerivative< T > &dstate_out)

T	applyTestForce (const int gc_index, const Vec3< T > &force_ics_at_contact, DVec< T > &dstate_out)

void	addDynamicsVars (int count)

void	resizeSystemMatricies ()

void	setState (const FBModelState< T > &state)

void	resetCalculationFlags ()

void	setDState (const FBModelStateDerivative< T > &dState)

Vec3< T >	getPosition (const int link_idx, const Vec3< T > &local_pos)

Vec3< T >	getPosition (const int link_idx)

Mat3< T >	getOrientation (const int link_idx)

Vec3< T >	getLinearVelocity (const int link_idx, const Vec3< T > &point)

Vec3< T >	getLinearVelocity (const int link_idx)

Vec3< T >	getLinearAcceleration (const int link_idx, const Vec3< T > &point)

Vec3< T >	getLinearAcceleration (const int link_idx)

Vec3< T >	getAngularVelocity (const int link_idx)

Vec3< T >	getAngularAcceleration (const int link_idx)

void	forwardKinematics ()

void	biasAccelerations ()

void	compositeInertias ()

void	forwardAccelerationKinematics ()

void	contactJacobians ()

DVec< T >	generalizedGravityForce ()

DVec< T >	generalizedCoriolisForce ()

DMat< T >	massMatrix ()

DVec< T >	inverseDynamics (const FBModelStateDerivative< T > &dState)

void	runABA (const DVec< T > &tau, FBModelStateDerivative< T > &dstate)

const DMat< T > &	getMassMatrix () const

const DVec< T > &	getGravityForce () const

const DVec< T > &	getCoriolisForce () const

void	updateArticulatedBodies ()

void	updateForcePropagators ()

void	udpateQddEffects ()

void	resetExternalForces ()

Public Attributes
size_t	_nDof = 0

Vec3< T >	_gravity

vector< int >	_parents

vector< T >	_gearRatios

vector< T >	_d

vector< T >	_u

vector< JointType >	_jointTypes

vector< CoordinateAxis >	_jointAxes

vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > >	_Xtree

vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > >	_Xrot

vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > >	_Ibody

vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > >	_Irot

vector< std::string >	_bodyNames

size_t	_nGroundContact = 0

vector< size_t >	_gcParent

vector< Vec3< T > >	_gcLocation

vector< uint64_t >	_footIndicesGC

vector< Vec3< T > >	_pGC

vector< Vec3< T > >	_vGC

vector< bool >	_compute_contact_info

FBModelState< T >	_state
	BEGIN ALGORITHM SUPPORT VARIABLES. More...

FBModelStateDerivative< T >	_dState

vectorAligned< SVec< T > >	_v

vectorAligned< SVec< T > >	_vrot

vectorAligned< SVec< T > >	_a

vectorAligned< SVec< T > >	_arot

vectorAligned< SVec< T > >	_avp

vectorAligned< SVec< T > >	_avprot

vectorAligned< SVec< T > >	_c

vectorAligned< SVec< T > >	_crot

vectorAligned< SVec< T > >	_S

vectorAligned< SVec< T > >	_Srot

vectorAligned< SVec< T > >	_fvp

vectorAligned< SVec< T > >	_fvprot

vectorAligned< SVec< T > >	_ag

vectorAligned< SVec< T > >	_agrot

vectorAligned< SVec< T > >	_f

vectorAligned< SVec< T > >	_frot

vectorAligned< SVec< T > >	_U

vectorAligned< SVec< T > >	_Urot

vectorAligned< SVec< T > >	_Utot

vectorAligned< SVec< T > >	_pA

vectorAligned< SVec< T > >	_pArot

vectorAligned< SVec< T > >	_externalForces

vectorAligned< SpatialInertia< T > >	_IC

vectorAligned< Mat6< T > >	_Xup

vectorAligned< Mat6< T > >	_Xa

vectorAligned< Mat6< T > >	_Xuprot

vectorAligned< Mat6< T > >	_IA

vectorAligned< Mat6< T > >	_ChiUp

DMat< T >	_H

DMat< T >	_C

DVec< T >	_Cqd

DVec< T >	_G

vectorAligned< D6Mat< T > >	_J

vectorAligned< SVec< T > >	_Jdqd

vectorAligned< D3Mat< T > >	_Jc

vectorAligned< Vec3< T > >	_Jcdqd

bool	_kinematicsUpToDate = false

bool	_biasAccelerationsUpToDate = false

bool	_accelerationsUpToDate = false

bool	_compositeInertiasUpToDate = false

bool	_articulatedBodiesUpToDate = false

bool	_forcePropagatorsUpToDate = false

bool	_qddEffectsUpToDate = false

DMat< T >	_qdd_from_base_accel

DMat< T >	_qdd_from_subqdd

Eigen::ColPivHouseholderQR< Mat6< T > >	_invIA5

Detailed Description

template<typename T>
class FloatingBaseModel< T >

Class to represent a floating base rigid body model with rotors and ground contacts. No concept of state.

Definition at line 69 of file FloatingBaseModel.h.

Constructor & Destructor Documentation

template<typename T>

FloatingBaseModel< T >::FloatingBaseModel ( )

inline

Initialize a floating base model with default gravity

Definition at line 74 of file FloatingBaseModel.h.

74 : _gravity(0, 0, -9.81) {}

FloatingBaseModel::_gravity

Vec3< T > _gravity

Definition: FloatingBaseModel.h:239

template<typename T>

FloatingBaseModel< T >::~FloatingBaseModel ( )

inline

Definition at line 75 of file FloatingBaseModel.h.

75 {}

Member Function Documentation

template<typename T>

void FloatingBaseModel< T >::addBase ( const SpatialInertia< T > & inertia )

Add floating base. Must be the first body added, and there can only be one

Create the floating body

Parameters

inertia Spatial inertia of the floating body

Definition at line 267 of file FloatingBaseModel.cpp.

References ori::X.

                                                                    {
   if (_nDof) {
     throw std::runtime_error("Cannot add base multiple times!\n");
   }
 
   Mat6<T> eye6 = Mat6<T>::Identity();
   Mat6<T> zero6 = Mat6<T>::Zero();
   SpatialInertia<T> zeroInertia(zero6);
   // the floating base has 6 DOFs
 
   _nDof = 6;
   for (size_t i = 0; i < 6; i++) {
     _parents.push_back(0);
     _gearRatios.push_back(0);
     _jointTypes.push_back(JointType::Nothing);  // doesn't actually matter
     _jointAxes.push_back(CoordinateAxis::X);    // doesn't actually matter
     _Xtree.push_back(eye6);
     _Ibody.push_back(zeroInertia);
     _Xrot.push_back(eye6);
     _Irot.push_back(zeroInertia);
     _bodyNames.push_back("N/A");
   }
 
   _jointTypes[5] = JointType::FloatingBase;
   _Ibody[5] = inertia;
   _gearRatios[5] = 1;
   _bodyNames[5] = "Floating Base";
 
   addDynamicsVars(6);
 }

Here is the caller graph for this function:

template<typename T>

void FloatingBaseModel< T >::addBase	(	T	mass,
		const Vec3< T > &	com,
		const Mat3< T > &	I
	)

Add floating base. Must be the first body added, and there can only be one

Create the floating body

Parameters

mass	Mass of the floating body
com	Center of mass of the floating body
I	Rotational inertia of the floating body

Definition at line 305 of file FloatingBaseModel.cpp.

                                                      {
   SpatialInertia<T> IS(mass, com, I);
   addBase(IS);
 }

template<typename T>

int FloatingBaseModel< T >::addBody	(	const SpatialInertia< T > &	inertia,
		const SpatialInertia< T > &	rotorInertia,
		T	gearRatio,
		int	parent,
		JointType	jointType,
		CoordinateAxis	jointAxis,
		const Mat6< T > &	Xtree,
		const Mat6< T > &	Xrot
	)

Add a body to the tree

Parameters

inertia	: Inertia of body (body coords)
rotorInertia	: Inertia of rotor (rotor coords)
gearRatio	: Gear ratio. >1 for a gear reduction
parent	: Body ID of the link that the body is connected to
jointType	: Type of joint
jointAxis	: Axis of joint
Xtree	: Location of joint
Xrot	: Location of rotor

Returns: : bodyID

Add a body

Parameters

inertia	The inertia of the body
rotorInertia	The inertia of the rotor the body is connected to
gearRatio	The gear ratio between the body and the rotor
parent	The parent body, which is also assumed to be the body the rotor is connected to
jointType	The type of joint (prismatic or revolute)
jointAxis	The joint axis (X,Y,Z), in the parent's frame
Xtree	The coordinate transformation from parent to this body
Xrot	The coordinate transformation from parent to this body's rotor

Returns: The body's ID (can be used as the parent)

Definition at line 392 of file FloatingBaseModel.cpp.

                                                                              {
   if ((size_t)parent >= _nDof) {
     throw std::runtime_error(
         "addBody got invalid parent: " + std::to_string(parent) +
         " nDofs: " + std::to_string(_nDof) + "\n");
   }
 
   _parents.push_back(parent);
   _gearRatios.push_back(gearRatio);
   _jointTypes.push_back(jointType);
   _jointAxes.push_back(jointAxis);
   _Xtree.push_back(Xtree);
   _Xrot.push_back(Xrot);
   _Ibody.push_back(inertia);
   _Irot.push_back(rotorInertia);
   _nDof++;
 
   addDynamicsVars(1);
 
   return _nDof;
 }

Here is the caller graph for this function:

template<typename T>

int FloatingBaseModel< T >::addBody	(	const MassProperties< T > &	inertia,
		const MassProperties< T > &	rotorInertia,
		T	gearRatio,
		int	parent,
		JointType	jointType,
		CoordinateAxis	jointAxis,
		const Mat6< T > &	Xtree,
		const Mat6< T > &	Xrot
	)

Add a body to the tree

Parameters

inertia	: Inertia of body (body coords)
rotorInertia	: Inertia of rotor (rotor coords)
gearRatio	: Gear ratio. >1 for a gear reduction
parent	: Body ID of the link that the body is connected to
jointType	: Type of joint
jointAxis	: Axis of joint
Xtree	: Location of joint
Xrot	: Location of rotor

Returns: : bodyID

Add a body

Parameters

inertia	The inertia of the body
rotorInertia	The inertia of the rotor the body is connected to
gearRatio	The gear ratio between the body and the rotor
parent	The parent body, which is also assumed to be the body the rotor is connected to
jointType	The type of joint (prismatic or revolute)
jointAxis	The joint axis (X,Y,Z), in the parent's frame
Xtree	The coordinate transformation from parent to this body
Xrot	The coordinate transformation from parent to this body's rotor

Returns: The body's ID (can be used as the parent)

Definition at line 432 of file FloatingBaseModel.cpp.

                                                                              {
   return addBody(SpatialInertia<T>(inertia), SpatialInertia<T>(rotorInertia),
                  gearRatio, parent, jointType, jointAxis, Xtree, Xrot);
 }

template<typename T >

void FloatingBaseModel< T >::addDynamicsVars ( int count )

Populate member variables when bodies are added

Parameters

count (6 for fb, 1 for joint)

Definition at line 185 of file FloatingBaseModel.cpp.

                                                     {
   if (count != 1 && count != 6) {
     throw std::runtime_error(
         "addDynamicsVars must be called with count=1 (joint) or count=6 "
         "(base).\n");
   }
 
   Mat6<T> eye6 = Mat6<T>::Identity();
   SVec<T> zero6 = SVec<T>::Zero();
   Mat6<T> zero66 = Mat6<T>::Zero();
 
   SpatialInertia<T> zeroInertia(zero66);
   for (int i = 0; i < count; i++) {
     _v.push_back(zero6);
     _vrot.push_back(zero6);
     _a.push_back(zero6);
     _arot.push_back(zero6);
     _avp.push_back(zero6);
     _avprot.push_back(zero6);
     _c.push_back(zero6);
     _crot.push_back(zero6);
     _S.push_back(zero6);
     _Srot.push_back(zero6);
     _f.push_back(zero6);
     _frot.push_back(zero6);
     _fvp.push_back(zero6);
     _fvprot.push_back(zero6);
     _ag.push_back(zero6);
     _agrot.push_back(zero6);
     _IC.push_back(zeroInertia);
     _Xup.push_back(eye6);
     _Xuprot.push_back(eye6);
     _Xa.push_back(eye6);
 
     _ChiUp.push_back(eye6);
     _d.push_back(0.);
     _u.push_back(0.);
     _IA.push_back(eye6);
 
     _U.push_back(zero6);
     _Urot.push_back(zero6);
     _Utot.push_back(zero6);
     _pA.push_back(zero6);
     _pArot.push_back(zero6);
     _externalForces.push_back(zero6);
   }
 
   _J.push_back(D6Mat<T>::Zero(6, _nDof));
   _Jdqd.push_back(SVec<T>::Zero());
 
   resizeSystemMatricies();
 }

template<typename T>

void FloatingBaseModel< T >::addGroundContactBoxPoints	(	int	bodyId,
		const Vec3< T > &	dims
	)

Add bounding box collision points around a body.

Parameters

bodyId	: Body to add
dims	: Dimension of points

Add the bounding points of a box to the contact model. Assumes the box is centered around the origin of the body coordinate system and is axis aligned.

Definition at line 360 of file FloatingBaseModel.cpp.

                                                                           {
   // addGroundContactPoint(bodyId, Vec3<T>( dims(0),  dims(1),  dims(2))/2);
   // addGroundContactPoint(bodyId, Vec3<T>(-dims(0),  dims(1),  dims(2))/2);
   // addGroundContactPoint(bodyId, Vec3<T>( dims(0), -dims(1),  dims(2))/2);
   // addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1),  dims(2))/2);
 
   addGroundContactPoint(bodyId, Vec3<T>(dims(0), dims(1), 0.) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), 0.) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(dims(0), -dims(1), 0.) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), 0.) / 2);
 
   addGroundContactPoint(bodyId, Vec3<T>(dims(0), dims(1), -dims(2)) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), -dims(2)) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(dims(0), -dims(1), -dims(2)) / 2);
   addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), -dims(2)) / 2);
 }

Here is the caller graph for this function:

template<typename T>

int FloatingBaseModel< T >::addGroundContactPoint	(	int	bodyID,
		const Vec3< T > &	location,
		bool	isFoot = `false`
	)

Add a point for collisions

Parameters

bodyID	: body that the point belongs to (body 5 for floating base)
location	: location of point in body coordinates
isFoot	: if the point is a foot or not. Only feet have their Jacobian calculated on the robot

Returns: collisionPointID of the new point

Add a ground contact point to a model

Parameters

bodyID	The ID of the body containing the contact point
location	The location (in body coordinate) of the contact point
isFoot	True if foot or not.

Returns: The ID of the ground contact point

Definition at line 319 of file FloatingBaseModel.cpp.

                                                              {
   if ((size_t)bodyID >= _nDof) {
     throw std::runtime_error(
         "addGroundContactPoint got invalid bodyID: " + std::to_string(bodyID) +
         " nDofs: " + std::to_string(_nDof) + "\n");
   }
 
   // std::cout << "pt-add: " << location.transpose() << "\n";
   _gcParent.push_back(bodyID);
   _gcLocation.push_back(location);
 
   Vec3<T> zero3 = Vec3<T>::Zero();
 
   _pGC.push_back(zero3);
   _vGC.push_back(zero3);
 
   D3Mat<T> J(3, _nDof);
   J.setZero();
 
   _Jc.push_back(J);
   _Jcdqd.push_back(zero3);
   //_compute_contact_info.push_back(false);
   _compute_contact_info.push_back(true);
 
   // add foot to foot list
   if (isFoot) {
     _footIndicesGC.push_back(_nGroundContact);
     _compute_contact_info[_nGroundContact] = true;
   }
 
   resizeSystemMatricies();
   return _nGroundContact++;
 }

Here is the caller graph for this function:

template<typename T>

T FloatingBaseModel< T >::applyTestForce	(	const int	gc_index,
		const Vec3< T > &	force_ics_at_contact,
		FBModelStateDerivative< T > &	dstate_out
	)

Apply a unit test force at a contact. Returns the inv contact inertia in that direction and computes the resultant qdd

Parameters

gc_index	index of the contact
force_ics_at_contact	unit test forcoe dstate - Output paramter of resulting accelerations

Returns: the 1x1 inverse contact inertia J H^{-1} J^T

Definition at line 962 of file FloatingBaseModel.cpp.

References spatial::createSXform(), FBModelStateDerivative< T >::dBodyVelocity, and FBModelStateDerivative< T >::qdd.

                                                                               {
   forwardKinematics();
   updateArticulatedBodies();
   updateForcePropagators();
   udpateQddEffects();
 
   size_t i_opsp = _gcParent.at(gc_index);
   size_t i = i_opsp;
 
   dstate_out.qdd.setZero();
 
   // Rotation to absolute coords
   Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
   Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
 
   // D is one column of an extended force propagator matrix (See Wensing, 2012
   // ICRA)
   SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
 
   double LambdaInv = 0;
   double tmp = 0;
 
   // from tips to base
   while (i > 5) {
     tmp = F.dot(_S[i]);
     LambdaInv += tmp * tmp / _d[i];
     dstate_out.qdd += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];
 
     // Apply force propagator (see Pat's ICRA 2012 paper)
     // essentially, since the joint is articulated, only a portion of the force
     // is felt on the predecessor. So, while Xup^T sends a force backwards as if
     // the joint was locked, ChiUp^T sends the force backward as if the joint
     // were free
     F = _ChiUp[i].transpose() * F;
     i = _parents[i];
   }
 
   // TODO: Only carry out the QR once within update Aritculated Bodies
   dstate_out.dBodyVelocity = _invIA5.solve(F);
   LambdaInv += F.dot(dstate_out.dBodyVelocity);
   dstate_out.qdd += _qdd_from_base_accel * dstate_out.dBodyVelocity;
 
   return LambdaInv;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

T FloatingBaseModel< T >::applyTestForce	(	const int	gc_index,
		const Vec3< T > &	force_ics_at_contact,
		DVec< T > &	dstate_out
	)

Apply a unit test force at a contact. Returns the inv contact inertia in that direction and computes the resultant qdd

Parameters

gc_index	index of the contact
force_ics_at_contact	unit test force expressed in inertial coordinates dstate - Output paramter of resulting accelerations

Returns: the 1x1 inverse contact inertia J H^{-1} J^T

Definition at line 40 of file FloatingBaseModel.cpp.

References spatial::createSXform().

                                                             {
   forwardKinematics();
   updateArticulatedBodies();
   updateForcePropagators();
   udpateQddEffects();
 
   size_t i_opsp = _gcParent.at(gc_index);
   size_t i = i_opsp;
 
   dstate_out = DVec<T>::Zero(_nDof);
 
   // Rotation to absolute coords
   Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
   Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
 
   // D is one column of an extended force propagator matrix (See Wensing, 2012
   // ICRA)
   SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
 
   T LambdaInv = 0;
   T tmp = 0;
 
   // from tips to base
   while (i > 5) {
     tmp = F.dot(_S[i]);
     LambdaInv += tmp * tmp / _d[i];
     dstate_out.tail(_nDof - 6) += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];
 
     // Apply force propagator (see Pat's ICRA 2012 paper)
     // essentially, since the joint is articulated, only a portion of the force
     // is felt on the predecessor. So, while Xup^T sends a force backwards as if
     // the joint was locked, ChiUp^T sends the force backward as if the joint
     // were free
     F = _ChiUp[i].transpose() * F;
     i = _parents[i];
   }
 
   dstate_out.head(6) = _invIA5.solve(F);
   LambdaInv += F.dot(dstate_out.head(6));
   dstate_out.tail(_nDof - 6) += _qdd_from_base_accel * dstate_out.head(6);
 
   return LambdaInv;
 }

Here is the call graph for this function:

template<typename T >

void FloatingBaseModel< T >::biasAccelerations ( )

(Support Function) Computes velocity product accelerations of each link and rotor _avp, and _avprot

Definition at line 590 of file FloatingBaseModel.cpp.

                                              {
   if (_biasAccelerationsUpToDate) return;
   forwardKinematics();
   // velocity product acceelration of base
   _avp[5] << 0, 0, 0, 0, 0, 0;
 
   // from base to tips
   for (size_t i = 6; i < _nDof; i++) {
     // Outward kinamtic propagtion
     _avp[i] = _Xup[i] * _avp[_parents[i]] + _c[i];
     _avprot[i] = _Xuprot[i] * _avp[_parents[i]] + _crot[i];
   }
   _biasAccelerationsUpToDate = true;
 }

template<typename T >

void FloatingBaseModel< T >::check ( )

Very simple to check to make sure the dimensions are correct

Definition at line 442 of file FloatingBaseModel.cpp.

                                  {
   if (_nDof != _parents.size())
     throw std::runtime_error("Invalid dof and parents length");
 }

Here is the caller graph for this function:

template<typename T >

void FloatingBaseModel< T >::compositeInertias ( )

(Support Function) Computes the composite rigid body inertia of each subtree _IC[i] contains body i, and the body/rotor inertias of all successors of body i. (key note: _IC[i] does not contain rotor i)

Definition at line 753 of file FloatingBaseModel.cpp.

                                              {
   if (_compositeInertiasUpToDate) return;
 
   forwardKinematics();
   // initialize
   for (size_t i = 5; i < _nDof; i++) {
     _IC[i].setMatrix(_Ibody[i].getMatrix());
   }
 
   // backward loop
   for (size_t i = _nDof - 1; i > 5; i--) {
     // Propagate inertia down the tree
     _IC[_parents[i]].addMatrix(_Xup[i].transpose() * _IC[i].getMatrix() *
                                _Xup[i]);
     _IC[_parents[i]].addMatrix(_Xuprot[i].transpose() * _Irot[i].getMatrix() *
                                _Xuprot[i]);
   }
   _compositeInertiasUpToDate = true;
 }

template<typename T >

void FloatingBaseModel< T >::contactJacobians ( )

Compute the contact Jacobians (3xn matrices) for the velocity of each contact point expressed in absolute coordinates

Definition at line 548 of file FloatingBaseModel.cpp.

References spatial::createSXform(), and spatial::spatialToLinearAcceleration().

                                             {
   forwardKinematics();
   biasAccelerations();
 
   for (size_t k = 0; k < _nGroundContact; k++) {
     _Jc[k].setZero();
     _Jcdqd[k].setZero();
 
     // Skip it if we don't care about it
     if (!_compute_contact_info[k]) continue;
 
     size_t i = _gcParent.at(k);
 
     // Rotation to absolute coords
     Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
     Mat6<T> Xc = createSXform(Rai, _gcLocation.at(k));
 
     // Bias acceleration
     SVec<T> ac = Xc * _avp[i];
     SVec<T> vc = Xc * _v[i];
 
     // Correct to classical
     _Jcdqd[k] = spatialToLinearAcceleration(ac, vc);
 
     // rows for linear velcoity in the world
     D3Mat<T> Xout = Xc.template bottomRows<3>();
 
     // from tips to base
     while (i > 5) {
       _Jc[k].col(i) = Xout * _S[i];
       Xout = Xout * _Xup[i];
       i = _parents[i];
     }
     _Jc[k].template leftCols<6>() = Xout;
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

void FloatingBaseModel< T >::forwardAccelerationKinematics ( )

Definition at line 813 of file FloatingBaseModel.cpp.

                                                          {
   if (_accelerationsUpToDate) {
     return;
   }
 
   forwardKinematics();
   biasAccelerations();
 
   // Initialize gravity with model info
   SVec<T> aGravity = SVec<T>::Zero();
   aGravity.template tail<3>() = _gravity;
 
   // Spatial force for floating base
   _a[5] = -_Xup[5] * aGravity + _dState.dBodyVelocity;
 
   // loop through joints
   for (size_t i = 6; i < _nDof; i++) {
     // spatial acceleration
     _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * _dState.qdd[i - 6] + _c[i];
     _arot[i] =
         _Xuprot[i] * _a[_parents[i]] + _Srot[i] * _dState.qdd[i - 6] + _crot[i];
   }
   _accelerationsUpToDate = true;
 }

template<typename T >

void FloatingBaseModel< T >::forwardKinematics ( )

Forward kinematics of all bodies. Computes _Xup (from up the tree) and _Xa (from absolute) Also computes _S (motion subspace), _v (spatial velocity in link coordinates), and _c (coriolis acceleration in link coordinates)

Definition at line 479 of file FloatingBaseModel.cpp.

References spatial::createSXform(), spatial::invertSXform(), spatial::jointXform(), spatial::motionCrossProduct(), ori::quaternionToRotationMatrix(), spatial::spatialToLinearVelocity(), and spatial::sXFormPoint().

                                              {
   if (_kinematicsUpToDate) return;
 
   // calculate joint transformations
   _Xup[5] = createSXform(quaternionToRotationMatrix(_state.bodyOrientation),
                          _state.bodyPosition);
   _v[5] = _state.bodyVelocity;
   for (size_t i = 6; i < _nDof; i++) {
     // joint xform
     Mat6<T> XJ = jointXform(_jointTypes[i], _jointAxes[i], _state.q[i - 6]);
     _Xup[i] = XJ * _Xtree[i];
     _S[i] = jointMotionSubspace<T>(_jointTypes[i], _jointAxes[i]);
     SVec<T> vJ = _S[i] * _state.qd[i - 6];
     // total velocity of body i
     _v[i] = _Xup[i] * _v[_parents[i]] + vJ;
 
     // Same for rotors
     Mat6<T> XJrot = jointXform(_jointTypes[i], _jointAxes[i],
                                _state.q[i - 6] * _gearRatios[i]);
     _Srot[i] = _S[i] * _gearRatios[i];
     SVec<T> vJrot = _Srot[i] * _state.qd[i - 6];
     _Xuprot[i] = XJrot * _Xrot[i];
     _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;
 
     // Coriolis accelerations
     _c[i] = motionCrossProduct(_v[i], vJ);
     _crot[i] = motionCrossProduct(_vrot[i], vJrot);
   }
 
   // calculate from absolute transformations
   for (size_t i = 5; i < _nDof; i++) {
     if (_parents[i] == 0) {
       _Xa[i] = _Xup[i];  // float base
     } else {
       _Xa[i] = _Xup[i] * _Xa[_parents[i]];
     }
   }
 
   // ground contact points
   //  // TODO : we end up inverting the same Xa a few times (like for the 8
   //  points on the body). this isn't super efficient.
   for (size_t j = 0; j < _nGroundContact; j++) {
     if (!_compute_contact_info[j]) continue;
     size_t i = _gcParent.at(j);
     Mat6<T> Xai = invertSXform(_Xa[i]);  // from link to absolute
     SVec<T> vSpatial = Xai * _v[i];
 
     // foot position in world
     _pGC.at(j) = sXFormPoint(Xai, _gcLocation.at(j));
     _vGC.at(j) = spatialToLinearVelocity(vSpatial, _pGC.at(j));
   }
   _kinematicsUpToDate = true;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

DVec< T > FloatingBaseModel< T >::generalizedCoriolisForce ( )

Computes the generalized coriolis forces (Cqd) in the inverse dynamics

Returns: Cqd (_nDof x 1 vector)

Definition at line 636 of file FloatingBaseModel.cpp.

References spatial::forceCrossProduct().

                                                        {
   biasAccelerations();
 
   // Floating base force
   Mat6<T> Ifb = _Ibody[5].getMatrix();
   SVec<T> hfb = Ifb * _v[5];
   _fvp[5] = Ifb * _avp[5] + forceCrossProduct(_v[5], hfb);
 
   for (size_t i = 6; i < _nDof; i++) {
     // Force on body i
     Mat6<T> Ii = _Ibody[i].getMatrix();
     SVec<T> hi = Ii * _v[i];
     _fvp[i] = Ii * _avp[i] + forceCrossProduct(_v[i], hi);
 
     // Force on rotor i
     Mat6<T> Ir = _Irot[i].getMatrix();
     SVec<T> hr = Ir * _vrot[i];
     _fvprot[i] = Ir * _avprot[i] + forceCrossProduct(_vrot[i], hr);
   }
 
   for (size_t i = _nDof - 1; i > 5; i--) {
     // Extract force along the joints
     _Cqd[i] = _S[i].dot(_fvp[i]) + _Srot[i].dot(_fvprot[i]);
 
     // Propage force down the tree
     _fvp[_parents[i]] += _Xup[i].transpose() * _fvp[i];
     _fvp[_parents[i]] += _Xuprot[i].transpose() * _fvprot[i];
   }
 
   // Force on floating base
   _Cqd.template topRows<6>() = _fvp[5];
   return _Cqd;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

DVec< T > FloatingBaseModel< T >::generalizedGravityForce ( )

Computes the generalized gravitational force (G) in the inverse dynamics

Returns: G (_nDof x 1 vector)

Definition at line 610 of file FloatingBaseModel.cpp.

                                                       {
   compositeInertias();
 
   SVec<T> aGravity;
   aGravity << 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];
   _ag[5] = _Xup[5] * aGravity;
 
   // Gravity comp force is the same as force required to accelerate
   // oppostite gravity
   _G.template topRows<6>() = -_IC[5].getMatrix() * _ag[5];
   for (size_t i = 6; i < _nDof; i++) {
     _ag[i] = _Xup[i] * _ag[_parents[i]];
     _agrot[i] = _Xuprot[i] * _ag[_parents[i]];
 
     // body and rotor
     _G[i] = -_S[i].dot(_IC[i].getMatrix() * _ag[i]) -
             _Srot[i].dot(_Irot[i].getMatrix() * _agrot[i]);
   }
   return _G;
 }

Here is the caller graph for this function:

template<typename T >

Vec3< T > FloatingBaseModel< T >::getAngularAcceleration ( const int link_idx )

Definition at line 740 of file FloatingBaseModel.cpp.

                                                                        {
   forwardAccelerationKinematics();
   Mat3<T> Rai = getOrientation(link_idx);
   return Rai * _a[link_idx].template head<3>();
 }

template<typename T >

Vec3< T > FloatingBaseModel< T >::getAngularVelocity ( const int link_idx )

Definition at line 731 of file FloatingBaseModel.cpp.

                                                                    {
   forwardKinematics();
   Mat3<T> Rai = getOrientation(link_idx);
   // Vec3<T> v3 =
   return Rai * _v[link_idx].template head<3>();
   ;
 }

template<typename T>

const std::vector<SpatialInertia<T>, Eigen::aligned_allocator<SpatialInertia<T> > >& FloatingBaseModel< T >::getBodyInertiaVector ( )

inline

Definition at line 158 of file FloatingBaseModel.h.

                          {
     return _Ibody;
   }

Here is the caller graph for this function:

template<typename T>

const DVec<T>& FloatingBaseModel< T >::getCoriolisForce ( ) const

inline

Definition at line 263 of file FloatingBaseModel.h.

263 { return _Cqd; }

FloatingBaseModel::_Cqd

DVec< T > _Cqd

Definition: FloatingBaseModel.h:283

template<typename T>

const DVec<T>& FloatingBaseModel< T >::getGravityForce ( ) const

inline

Definition at line 262 of file FloatingBaseModel.h.

262 { return _G; }

FloatingBaseModel::_G

DVec< T > _G

Definition: FloatingBaseModel.h:283

template<typename T>

Vec3< T > FloatingBaseModel< T >::getLinearAcceleration	(	const int	link_idx,
		const Vec3< T > &	point
	)

Definition at line 698 of file FloatingBaseModel.cpp.

References spatial::spatialToLinearAcceleration().

                                                                           {
   forwardAccelerationKinematics();
   Mat3<T> R = getOrientation(link_idx);
   return R * spatialToLinearAcceleration(_a[link_idx], _v[link_idx], point);
 }

Here is the call graph for this function:

template<typename T>

Vec3< T > FloatingBaseModel< T >::getLinearAcceleration ( const int link_idx )

Definition at line 706 of file FloatingBaseModel.cpp.

References spatial::spatialToLinearAcceleration().

                                                                       {
   forwardAccelerationKinematics();
   Mat3<T> R = getOrientation(link_idx);
   return R * spatialToLinearAcceleration(_a[link_idx], _v[link_idx], Vec3<T>::Zero());
 }

Here is the call graph for this function:

template<typename T>

Vec3< T > FloatingBaseModel< T >::getLinearVelocity	(	const int	link_idx,
		const Vec3< T > &	point
	)

Definition at line 714 of file FloatingBaseModel.cpp.

References spatial::spatialToLinearVelocity().

                                                                       {
   forwardKinematics();
   Mat3<T> Rai = getOrientation(link_idx);
   return Rai * spatialToLinearVelocity(_v[link_idx], point);
 }

Here is the call graph for this function:

template<typename T>

Vec3< T > FloatingBaseModel< T >::getLinearVelocity ( const int link_idx )

Definition at line 722 of file FloatingBaseModel.cpp.

References spatial::spatialToLinearVelocity().

                                                                   {
   forwardKinematics();
   Mat3<T> Rai = getOrientation(link_idx);
   return Rai * spatialToLinearVelocity(_v[link_idx], Vec3<T>::Zero());
 }

Here is the call graph for this function:

template<typename T>

const DMat<T>& FloatingBaseModel< T >::getMassMatrix ( ) const

inline

Definition at line 261 of file FloatingBaseModel.h.

261 { return _H; }

FloatingBaseModel::_H

DMat< T > _H

Definition: FloatingBaseModel.h:282

Here is the caller graph for this function:

template<typename T >

Mat3< T > FloatingBaseModel< T >::getOrientation ( const int link_idx )

Definition at line 671 of file FloatingBaseModel.cpp.

                                                          {
   forwardKinematics();
   Mat3<T> Rai = _Xa[link_idx].template block<3, 3>(0, 0);
   Rai.transposeInPlace();
   return Rai;
 }

template<typename T>

const std::vector<int>& FloatingBaseModel< T >::getParentVector ( )

inline

Definition at line 154 of file FloatingBaseModel.h.

154 { return _parents; }

FloatingBaseModel::_parents

vector< int > _parents

Definition: FloatingBaseModel.h:240

Here is the caller graph for this function:

template<typename T>

Vec3< T > FloatingBaseModel< T >::getPosition	(	const int	link_idx,
		const Vec3< T > &	local_pos
	)

Definition at line 689 of file FloatingBaseModel.cpp.

References spatial::invertSXform(), and spatial::sXFormPoint().

 {
   forwardKinematics();
   Mat6<T> Xai = invertSXform(_Xa[link_idx]); // from link to absolute
   Vec3<T> link_pos = sXFormPoint(Xai, local_pos);
   return link_pos;
 }

Here is the call graph for this function:

template<typename T>

Vec3< T > FloatingBaseModel< T >::getPosition ( const int link_idx )

Definition at line 680 of file FloatingBaseModel.cpp.

References spatial::invertSXform(), and spatial::sXFormPoint().

 {
   forwardKinematics();
   Mat6<T> Xai = invertSXform(_Xa[link_idx]); // from link to absolute
   Vec3<T> link_pos = sXFormPoint(Xai, Vec3<T>::Zero());
   return link_pos;
 }

Here is the call graph for this function:

template<typename T>

const std::vector<SpatialInertia<T>, Eigen::aligned_allocator<SpatialInertia<T> > >& FloatingBaseModel< T >::getRotorInertiaVector ( )

inline

Definition at line 164 of file FloatingBaseModel.h.

                           {
     return _Irot;
   }

Here is the caller graph for this function:

template<typename T>

DMat< T > FloatingBaseModel< T >::invContactInertia	(	const int	gc_index,
		const D6Mat< T > &	force_directions
	)

Compute the inverse of the contact inertia matrix (mxm)

Parameters

force_directions (6xm) each column denotes a direction of interest col = [ moment in i.c.s., force in i.c.s.] e.g. if you want the cartesian inv. contact inertia force_directions = [ 0_{3x3} I_{3x3}]^T if you only want the cartesian inv. contact inertia in one direction then use the overloaded version.

Returns: the mxm inverse contact inertia J H^{-1} J^T

Definition at line 1065 of file FloatingBaseModel.cpp.

References spatial::createSXform().

                                                           {
   forwardKinematics();
   updateArticulatedBodies();
   updateForcePropagators();
 
   size_t i_opsp = _gcParent.at(gc_index);
   size_t i = i_opsp;
 
   // Rotation to absolute coords
   Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
   Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
 
   // D is a subslice of an extended force propagator matrix (See Wensing, 2012
   // ICRA)
   D6Mat<T> D = Xc.transpose() * force_directions;
 
   size_t m = force_directions.cols();
 
   DMat<T> LambdaInv = DMat<T>::Zero(m, m);
   DVec<T> tmp = DVec<T>::Zero(m);
 
   // from tips to base
   while (i > 5) {
     tmp = D.transpose() * _S[i];
     LambdaInv += tmp * tmp.transpose() / _d[i];
 
     // Apply force propagator (see Pat's ICRA 2012 paper)
     // essentially, since the joint is articulated, only a portion of the force
     // is felt on the predecessor. So, while Xup^T sends a force backwards as if
     // the joint was locked, ChiUp^T sends the force backward as if the joint
     // were free
     D = _ChiUp[i].transpose() * D;
     i = _parents[i];
   }
 
   // TODO: Only carry out the QR once within update Aritculated Bodies
   LambdaInv += D.transpose() * _invIA5.solve(D);
 
   return LambdaInv;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

T FloatingBaseModel< T >::invContactInertia	(	const int	gc_index,
		const Vec3< T > &	force_ics_at_contact
	)

Compute the inverse of the contact inertia matrix (mxm)

Parameters

force_ics_at_contact (3x1) e.g. if you want the cartesian inv. contact inertia in the z_ics force_ics_at_contact = [0 0 1]^T

Returns: the 1x1 inverse contact inertia J H^{-1} J^T

Definition at line 1017 of file FloatingBaseModel.cpp.

References spatial::createSXform().

                                                                                {
   forwardKinematics();
   updateArticulatedBodies();
   updateForcePropagators();
 
   size_t i_opsp = _gcParent.at(gc_index);
   size_t i = i_opsp;
 
   // Rotation to absolute coords
   Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
   Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
 
   // D is one column of an extended force propagator matrix (See Wensing, 2012
   // ICRA)
   SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
 
   double LambdaInv = 0;
   double tmp = 0;
 
   // from tips to base
   while (i > 5) {
     tmp = F.dot(_S[i]);
     LambdaInv += tmp * tmp / _d[i];
 
     // Apply force propagator (see Pat's ICRA 2012 paper)
     // essentially, since the joint is articulated, only a portion of the force
     // is felt on the predecessor. So, while Xup^T sends a force backwards as if
     // the joint was locked, ChiUp^T sends the force backward as if the joint
     // were free
     F = _ChiUp[i].transpose() * F;
     i = _parents[i];
   }
   LambdaInv += F.dot(_invIA5.solve(F));
   return LambdaInv;
 }

Here is the call graph for this function:

template<typename T>

DVec< T > FloatingBaseModel< T >::inverseDynamics ( const FBModelStateDerivative< T > & dState )

Computes the inverse dynamics of the system

Returns: an _nDof x 1 vector. The first six entries give the external wrengh on the base, with the remaining giving the joint torques

Definition at line 845 of file FloatingBaseModel.cpp.

References spatial::forceCrossProduct().

                                              {
   setDState(dState);
   forwardAccelerationKinematics();
 
   // Spatial force for floating base
   SVec<T> hb = _Ibody[5].getMatrix() * _v[5];
   _f[5] = _Ibody[5].getMatrix() * _a[5] + forceCrossProduct(_v[5], hb);
 
   // loop through joints
   for (size_t i = 6; i < _nDof; i++) {
     // spatial momentum
     SVec<T> hi = _Ibody[i].getMatrix() * _v[i];
     SVec<T> hr = _Irot[i].getMatrix() * _vrot[i];
 
     // spatial force
     _f[i] = _Ibody[i].getMatrix() * _a[i] + forceCrossProduct(_v[i], hi);
     _frot[i] =
         _Irot[i].getMatrix() * _arot[i] + forceCrossProduct(_vrot[i], hr);
   }
 
   DVec<T> genForce(_nDof);
   for (size_t i = _nDof - 1; i > 5; i--) {
     // Pull off compoents of force along the joint
     genForce[i] = _S[i].dot(_f[i]) + _Srot[i].dot(_frot[i]);
 
     // Propagate down the tree
     _f[_parents[i]] += _Xup[i].transpose() * _f[i];
     _f[_parents[i]] += _Xuprot[i].transpose() * _frot[i];
   }
   genForce.template head<6>() = _f[5];
   return genForce;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

DMat< T > FloatingBaseModel< T >::massMatrix ( )

Computes the Mass Matrix (H) in the inverse dynamics formulation

Returns: H (_nDof x _nDof matrix)

Definition at line 778 of file FloatingBaseModel.cpp.

References f().

                                          {
   compositeInertias();
   _H.setZero();
 
   // Top left corner is the locked inertia of the whole system
   _H.template topLeftCorner<6, 6>() = _IC[5].getMatrix();
 
   for (size_t j = 6; j < _nDof; j++) {
     // f = spatial force required for a unit qdd_j
     SVec<T> f = _IC[j].getMatrix() * _S[j];
     SVec<T> frot = _Irot[j].getMatrix() * _Srot[j];
 
     _H(j, j) = _S[j].dot(f) + _Srot[j].dot(frot);
 
     // Propagate down the tree
     f = _Xup[j].transpose() * f + _Xuprot[j].transpose() * frot;
     size_t i = _parents[j];
     while (i > 5) {
       // in here f is expressed in frame {i}
       _H(i, j) = _S[i].dot(f);
       _H(j, i) = _H(i, j);
 
       // Propagate down the tree
       f = _Xup[i].transpose() * f;
       i = _parents[i];
     }
 
     // Force on floating base
     _H.template block<6, 1>(0, j) = f;
     _H.template block<1, 6>(j, 0) = f.adjoint();
   }
   return _H;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void FloatingBaseModel< T >::resetCalculationFlags ( )

inline

Definition at line 199 of file FloatingBaseModel.h.

                                {
     _articulatedBodiesUpToDate = false;
     _kinematicsUpToDate = false;
     _forcePropagatorsUpToDate = false;
     _qddEffectsUpToDate = false;
     _accelerationsUpToDate = false;
   }

template<typename T>

void FloatingBaseModel< T >::resetExternalForces ( )

inline

Definition at line 301 of file FloatingBaseModel.h.

                              {
     for (size_t i = 0; i < _nDof; i++) {
       _externalForces[i] = SVec<T>::Zero();
     }
   }

template<typename T >

void FloatingBaseModel< T >::resizeSystemMatricies ( )

Updates the size of H, C, Cqd, G, and Js when bodies are added

Definition at line 242 of file FloatingBaseModel.cpp.

                                                  {
   _H.setZero(_nDof, _nDof);
   _C.setZero(_nDof, _nDof);
   _Cqd.setZero(_nDof);
   _G.setZero(_nDof);
   for (size_t i = 0; i < _J.size(); i++) {
     _J[i].setZero(6, _nDof);
     _Jdqd[i].setZero();
   }
 
   for (size_t i = 0; i < _Jc.size(); i++) {
     _Jc[i].setZero(3, _nDof);
     _Jcdqd[i].setZero();
   }
   _qdd_from_subqdd.resize(_nDof - 6, _nDof - 6);
   _qdd_from_base_accel.resize(_nDof - 6, 6);
   _state.q = DVec<T>::Zero(_nDof - 6);
   _state.qd = DVec<T>::Zero(_nDof - 6);
 }

template<typename T>

void FloatingBaseModel< T >::runABA	(	const DVec< T > &	tau,
		FBModelStateDerivative< T > &	dstate
	)

Definition at line 880 of file FloatingBaseModel.cpp.

References spatial::createSXform(), FBModelStateDerivative< T >::dBodyPosition, FBModelStateDerivative< T >::dBodyVelocity, spatial::forceCrossProduct(), spatial::motionCrossProduct(), FBModelStateDerivative< T >::qdd, spatial::rotationFromSXform(), and spatial::translationFromSXform().

                                                                      {
   (void)tau;
   forwardKinematics();
   updateArticulatedBodies();
 
   // create spatial vector for gravity
   SVec<T> aGravity;
   aGravity << 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];
 
   // float-base articulated inertia
   SVec<T> ivProduct = _Ibody[5].getMatrix() * _v[5];
   _pA[5] = forceCrossProduct(_v[5], ivProduct);
 
   // loop 1, down the tree
   for (size_t i = 6; i < _nDof; i++) {
     ivProduct = _Ibody[i].getMatrix() * _v[i];
     _pA[i] = forceCrossProduct(_v[i], ivProduct);
 
     // same for rotors
     SVec<T> vJrot = _Srot[i] * _state.qd[i - 6];
     _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;
     _crot[i] = motionCrossProduct(_vrot[i], vJrot);
     ivProduct = _Irot[i].getMatrix() * _vrot[i];
     _pArot[i] = forceCrossProduct(_vrot[i], ivProduct);
   }
 
   // adjust pA for external forces
   for (size_t i = 5; i < _nDof; i++) {
     // TODO add if statement (avoid these calculations if the force is zero)
     Mat3<T> R = rotationFromSXform(_Xa[i]);
     Vec3<T> p = translationFromSXform(_Xa[i]);
     Mat6<T> iX = createSXform(R.transpose(), -R * p);
     _pA[i] = _pA[i] - iX.transpose() * _externalForces.at(i);
   }
 
   // Pat's magic principle of least constraint
   for (size_t i = _nDof - 1; i >= 6; i--) {
     _u[i] = tau[i - 6] - _S[i].transpose() * _pA[i] -
             _Srot[i].transpose() * _pArot[i] - _U[i].transpose() * _c[i] -
             _Urot[i].transpose() * _crot[i];
 
     // articulated inertia recursion
     SVec<T> pa =
         _Xup[i].transpose() * (_pA[i] + _IA[i] * _c[i]) +
         _Xuprot[i].transpose() * (_pArot[i] + _Irot[i].getMatrix() * _crot[i]) +
         _Utot[i] * _u[i] / _d[i];
     _pA[_parents[i]] += pa;
   }
 
   // include gravity and compute acceleration of floating base
   SVec<T> a0 = -aGravity;
   SVec<T> ub = -_pA[5];
   _a[5] = _Xup[5] * a0;
   SVec<T> afb = _invIA5.solve(ub - _IA[5].transpose() * _a[5]);
   _a[5] += afb;
 
   // joint accelerations
   dstate.qdd = DVec<T>(_nDof - 6);
   for (size_t i = 6; i < _nDof; i++) {
     dstate.qdd[i - 6] =
         (_u[i] - _Utot[i].transpose() * _a[_parents[i]]) / _d[i];
     _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * dstate.qdd[i - 6] + _c[i];
   }
 
   // output
   RotMat<T> Rup = rotationFromSXform(_Xup[5]);
   dstate.dBodyPosition =
       Rup.transpose() * _state.bodyVelocity.template block<3, 1>(3, 0);
   dstate.dBodyVelocity = afb;
   // qdd is set in the for loop above
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void FloatingBaseModel< T >::setContactComputeFlag	(	size_t	gc_index,
		bool	flag
	)

inline

Definition at line 173 of file FloatingBaseModel.h.

                                                          {
     _compute_contact_info[gc_index] = flag;
   }

template<typename T>

void FloatingBaseModel< T >::setDState ( const FBModelStateDerivative< T > & dState )

inline

Definition at line 207 of file FloatingBaseModel.h.

                                                           {
     _dState = dState;
     _accelerationsUpToDate = false;
   }

template<typename T>

void FloatingBaseModel< T >::setGravity ( Vec3< T > & g )

inline

Set the gravity

Definition at line 171 of file FloatingBaseModel.h.

171 { _gravity = g; }

FloatingBaseModel::_gravity

Vec3< T > _gravity

Definition: FloatingBaseModel.h:239

Here is the caller graph for this function:

template<typename T>

void FloatingBaseModel< T >::setState ( const FBModelState< T > & state )

inline

Definition at line 191 of file FloatingBaseModel.h.

                                               {
     _state = state;
 
     _biasAccelerationsUpToDate = false;
     _compositeInertiasUpToDate = false;
 
     resetCalculationFlags();
   }

Here is the caller graph for this function:

template<typename T >

T FloatingBaseModel< T >::totalNonRotorMass ( )

Total mass of all bodies which are not rotors

Compute the total mass of bodies which are not rotors.

Returns

Definition at line 452 of file FloatingBaseModel.cpp.

                                           {
   T totalMass = 0;
   for (size_t i = 0; i < _nDof; i++) {
     totalMass += _Ibody[i].getMass();
   }
   return totalMass;
 }

Here is the caller graph for this function:

template<typename T >

T FloatingBaseModel< T >::totalRotorMass ( )

Total mass of all rotors

Compute the total mass of bodies which are not rotors

Returns

Definition at line 465 of file FloatingBaseModel.cpp.

                                        {
   T totalMass = 0;
   for (size_t i = 0; i < _nDof; i++) {
     totalMass += _Irot[i].getMass();
   }
   return totalMass;
 }

Here is the caller graph for this function:

template<typename T >

void FloatingBaseModel< T >::udpateQddEffects ( )

Support function for contact inertia algorithms Computes the qdd arising from "subqdd" components If you are familiar with Featherstone's sparse Op sp or jain's innovations factorization: H = L * D * L^T These subqdd components represnt the space in the middle i.e. if H^{-1} = L^{-T} * D^{-1} * L^{1} then what I am calling subqdd = L^{-1} * tau This is an awful explanation. It needs latex.

Definition at line 98 of file FloatingBaseModel.cpp.

                                             {
   if (_qddEffectsUpToDate) return;
   updateForcePropagators();
   _qdd_from_base_accel.setZero();
   _qdd_from_subqdd.setZero();
 
   // Pass for force props
   // This loop is semi-equivalent to a cholesky factorization on H
   // akin to Featherstone's sparse operational space algo
   // These computations are for treating the joint rates like a task space
   // To do so, F computes the dynamic effect of torues onto bodies down the tree
   //
   for (size_t i = 6; i < _nDof; i++) {
     _qdd_from_subqdd(i - 6, i - 6) = 1;
     SVec<T> F = (_ChiUp[i].transpose() - _Xup[i].transpose()) * _S[i];
     size_t j = _parents[i];
     while (j > 5) {
       _qdd_from_subqdd(i - 6, j - 6) = _S[j].dot(F);
       F = _ChiUp[j].transpose() * F;
       j = _parents[j];
     }
     _qdd_from_base_accel.row(i - 6) = F.transpose();
   }
   _qddEffectsUpToDate = true;
 }

template<typename T >

void FloatingBaseModel< T >::updateArticulatedBodies ( )

Support function for the ABA

Definition at line 142 of file FloatingBaseModel.cpp.

References spatial::jointXform().

                                                    {
   if (_articulatedBodiesUpToDate) return;
 
   forwardKinematics();
 
   _IA[5] = _Ibody[5].getMatrix();
 
   // loop 1, down the tree
   for (size_t i = 6; i < _nDof; i++) {
     _IA[i] = _Ibody[i].getMatrix();  // initialize
     Mat6<T> XJrot = jointXform(_jointTypes[i], _jointAxes[i],
                                _state.q[i - 6] * _gearRatios[i]);
     _Xuprot[i] = XJrot * _Xrot[i];
     _Srot[i] = _S[i] * _gearRatios[i];
   }
 
   // Pat's magic principle of least constraint (Guass too!)
   for (size_t i = _nDof - 1; i >= 6; i--) {
     _U[i] = _IA[i] * _S[i];
     _Urot[i] = _Irot[i].getMatrix() * _Srot[i];
     _Utot[i] = _Xup[i].transpose() * _U[i] + _Xuprot[i].transpose() * _Urot[i];
 
     _d[i] = _Srot[i].transpose() * _Urot[i];
     _d[i] += _S[i].transpose() * _U[i];
 
     // articulated inertia recursion
     Mat6<T> Ia = _Xup[i].transpose() * _IA[i] * _Xup[i] +
                  _Xuprot[i].transpose() * _Irot[i].getMatrix() * _Xuprot[i] -
                  _Utot[i] * _Utot[i].transpose() / _d[i];
     _IA[_parents[i]] += Ia;
   }
 
   _invIA5.compute(_IA[5]);
   _articulatedBodiesUpToDate = true;
 }

Here is the call graph for this function:

template<typename T >

void FloatingBaseModel< T >::updateForcePropagators ( )

Support function for contact inertia algorithms Comptues force propagators across each joint

Definition at line 129 of file FloatingBaseModel.cpp.

                                                   {
   if (_forcePropagatorsUpToDate) return;
   updateArticulatedBodies();
   for (size_t i = 6; i < _nDof; i++) {
     _ChiUp[i] = _Xup[i] - _S[i] * _Utot[i].transpose() / _d[i];
   }
   _forcePropagatorsUpToDate = true;
 }