#include <DynamicsSimulator.h>

Inheritance diagram for DynamicsSimulator< T >:

Collaboration diagram for DynamicsSimulator< T >:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	DynamicsSimulator (FloatingBaseModel< T > &model, bool useSpringDamper=false)

void	step (T dt, const DVec< T > &tau, T kp, T kd)
	Initialize simulator with given model. More...

void	runABA (const DVec< T > &tau)
	Simulate forward one step. More...

void	forwardKinematics ()
	Do forward kinematics for feet. More...

void	integrate (T dt)
	Integrate to find new _state. More...

void	setState (const FBModelState< T > &state)

void	setHoming (const RobotHomingInfo< T > &homing)

const FBModelState< T > &	getState () const

const FBModelStateDerivative< T > &	getDState () const

void	setAllExternalForces (const vectorAligned< SVec< T >> &forces)

void	addCollisionPlane (T mu, T rest, T height)

void	addCollisionBox (T mu, T rest, T depth, T width, T height, const Vec3< T > &pos, const Mat3< T > &ori)

void	addCollisionMesh (T mu, T rest, T grid_size, const Vec3< T > &left_corner_loc, const DMat< T > &height_map)

size_t	getNumBodies ()

const size_t &	getTotalNumGC ()

const Vec3< T > &	getContactForce (size_t idx)

const FloatingBaseModel< T > &	getModel ()

Private Member Functions
void	updateCollisions (T dt, T kp, T kd)

Private Attributes
FBModelState< T >	_state
	Update ground collision list. More...

FBModelStateDerivative< T >	_dstate

FloatingBaseModel< T > &	_model

vector< CollisionPlane< T > >	_collisionPlanes

ContactConstraint< T > *	_contact_constr

SVec< T >	_lastBodyVelocity

bool	_useSpringDamper

RobotHomingInfo< T >	_homing

Detailed Description

template<typename T>
class DynamicsSimulator< T >

Class (containing state) for dynamics simulation of a floating-base system

Definition at line 40 of file DynamicsSimulator.h.

Constructor & Destructor Documentation

template<typename T>

DynamicsSimulator< T >::DynamicsSimulator	(	FloatingBaseModel< T > &	model,
		bool	useSpringDamper = `false`
	)

Initialize the dynamics simulator by allocating memory for ABA matrices

Definition at line 17 of file DynamicsSimulator.cpp.

References DynamicsSimulator< T >::_contact_constr, DynamicsSimulator< T >::_dstate, DynamicsSimulator< T >::_lastBodyVelocity, DynamicsSimulator< T >::_model, DynamicsSimulator< T >::_state, and DynamicsSimulator< T >::_useSpringDamper.

     : _model(model), _useSpringDamper(useSpringDamper) {
   if (_useSpringDamper) {
     _contact_constr = new ContactSpringDamper<T>(&_model);
   } else {
     _contact_constr = new ContactImpulse<T>(&_model);
   }
 
   _state.bodyVelocity = SVec<T>::Zero();
   _state.bodyPosition = Vec3<T>::Zero();
   _state.bodyOrientation = Quat<T>::Zero();
   _state.q = DVec<T>::Zero(_model._nDof - 6);
   _state.qd = DVec<T>::Zero(_model._nDof - 6);
   _dstate.qdd = DVec<T>::Zero(_model._nDof - 6);
   _lastBodyVelocity.setZero();
 }

Member Function Documentation

template<typename T>

void DynamicsSimulator< T >::addCollisionBox	(	T	mu,
		T	rest,
		T	depth,
		T	width,
		T	height,
		const Vec3< T > &	pos,
		const Mat3< T > &	ori
	)

inline

Definition at line 97 of file DynamicsSimulator.h.

                                                                {
     _contact_constr->AddCollision(
         new CollisionBox<T>(mu, rest, depth, width, height, pos, ori));
   }

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::addCollisionMesh	(	T	mu,
		T	rest,
		T	grid_size,
		const Vec3< T > &	left_corner_loc,
		const DMat< T > &	height_map
	)

inline

Definition at line 104 of file DynamicsSimulator.h.

                                                    {
     _contact_constr->AddCollision(
         new CollisionMesh<T>(mu, rest, grid_size, left_corner_loc, height_map));
   }

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::addCollisionPlane	(	T	mu,
		T	rest,
		T	height
	)

inline

Definition at line 92 of file DynamicsSimulator.h.

                                                  {
     _contact_constr->AddCollision(new CollisionPlane<T>(mu, rest, height));
   }

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::forwardKinematics ( )

inline

Do forward kinematics for feet.

Definition at line 53 of file DynamicsSimulator.h.

53 { _model.forwardKinematics(); }

DynamicsSimulator::_model

FloatingBaseModel< T > & _model

Definition: DynamicsSimulator.h:124

Here is the caller graph for this function:

template<typename T>

const Vec3<T>& DynamicsSimulator< T >::getContactForce ( size_t idx )

inline

Definition at line 114 of file DynamicsSimulator.h.

                                              {
     return _contact_constr->getGCForce(idx);
   }

Here is the caller graph for this function:

template<typename T>

const FBModelStateDerivative<T>& DynamicsSimulator< T >::getDState ( ) const

inline

Get the most recently calculated state derivative (updated by runABA)

Returns

Definition at line 80 of file DynamicsSimulator.h.

80 { return _dstate; }

DynamicsSimulator::_dstate

FBModelStateDerivative< T > _dstate

Definition: DynamicsSimulator.h:123

Here is the caller graph for this function:

template<typename T>

const FloatingBaseModel<T>& DynamicsSimulator< T >::getModel ( )

inline

Definition at line 118 of file DynamicsSimulator.h.

118 { return _model; }

DynamicsSimulator::_model

FloatingBaseModel< T > & _model

Definition: DynamicsSimulator.h:124

Here is the caller graph for this function:

template<typename T>

size_t DynamicsSimulator< T >::getNumBodies ( )

inline

Definition at line 111 of file DynamicsSimulator.h.

111 { return _model._nDof; }

DynamicsSimulator::_model

FloatingBaseModel< T > & _model

Definition: DynamicsSimulator.h:124

Here is the caller graph for this function:

template<typename T>

const FBModelState<T>& DynamicsSimulator< T >::getState ( ) const

inline

Get the state of the robot

Returns

Definition at line 74 of file DynamicsSimulator.h.

74 { return _state; }

DynamicsSimulator::_state

FBModelState< T > _state

Update ground collision list.

Definition: DynamicsSimulator.h:122

Here is the caller graph for this function:

template<typename T>

const size_t& DynamicsSimulator< T >::getTotalNumGC ( )

inline

Definition at line 113 of file DynamicsSimulator.h.

113 { return _model._nGroundContact; }

DynamicsSimulator::_model

FloatingBaseModel< T > & _model

Definition: DynamicsSimulator.h:124

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::integrate ( T dt )

Integrate to find new _state.

Integrate the floating base state

Parameters

dt timestep

Definition at line 87 of file DynamicsSimulator.cpp.

References DynamicsSimulator< T >::_contact_constr, DynamicsSimulator< T >::_dstate, DynamicsSimulator< T >::_lastBodyVelocity, DynamicsSimulator< T >::_state, DynamicsSimulator< T >::_useSpringDamper, spatial::createSXform(), ori::integrateQuat(), ori::integrateQuatImplicit(), ori::quaternionToRotationMatrix(), spatial::rotationFromSXform(), and ori::X.

                                          {
   if (_useSpringDamper) {
     Vec3<T> omegaBody = _state.bodyVelocity.template block<3, 1>(0, 0);
     Mat6<T> X = createSXform(quaternionToRotationMatrix(_state.bodyOrientation),
                              _state.bodyPosition);
     RotMat<T> R = rotationFromSXform(X);
     Vec3<T> omega0 = R.transpose() * omegaBody;
 
     // actual integration
     _state.qd += _dstate.qdd * dt;
     _state.q += _state.qd * dt;
 
     _state.bodyVelocity += _dstate.dBodyVelocity * dt;
     _state.bodyPosition += _dstate.dBodyPosition * dt;
     _state.bodyOrientation = integrateQuat(_state.bodyOrientation, omega0, dt);
   } else {
     // actual integration
     // Velocity Update by integrating acceleration
     _state.qd += _dstate.qdd * dt;
     _state.bodyVelocity += _dstate.dBodyVelocity * dt;
 
     // Contact Constraint Velocity Updated
     _contact_constr->UpdateQdot(_state);
 
     // Prepare body velocity integration
     RotMat<T> R_body = quaternionToRotationMatrix(_state.bodyOrientation);
 
     _dstate.dBodyPosition =
         R_body.transpose() * _state.bodyVelocity.template block<3, 1>(3, 0);
     Vec3<T> omegaBody = _state.bodyVelocity.template block<3, 1>(0, 0);
 
     // Position Update
     _state.q += _state.qd * dt;
     _state.bodyPosition += _dstate.dBodyPosition * dt;
     _state.bodyOrientation =
         integrateQuatImplicit(_state.bodyOrientation, omegaBody, dt);
     _dstate.dBodyVelocity = (_state.bodyVelocity - _lastBodyVelocity) / dt;
     _lastBodyVelocity = _state.bodyVelocity;
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::runABA ( const DVec< T > & tau )

inline

Simulate forward one step.

Find _dstate with the articulated body algorithm

Definition at line 50 of file DynamicsSimulator.h.

50 { _model.runABA(tau, _dstate); }

DynamicsSimulator::_dstate

FBModelStateDerivative< T > _dstate

Definition: DynamicsSimulator.h:123

DynamicsSimulator::_model

FloatingBaseModel< T > & _model

Definition: DynamicsSimulator.h:124

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::setAllExternalForces ( const vectorAligned< SVec< T >> & forces )

inline

Add external forces. These are added on top of the ground reaction forces The i-th force is the spatial force applied to body i. This is cleared after each step of the simulator.

Definition at line 87 of file DynamicsSimulator.h.

                                                                   {
     _model._externalForces = forces;
   }

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::setHoming ( const RobotHomingInfo< T > & homing )

inline

Definition at line 66 of file DynamicsSimulator.h.

                                                    {
     _homing = homing;
   }

Here is the caller graph for this function:

template<typename T>

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

inline

Set the state of the robot being simulated

Definition at line 61 of file DynamicsSimulator.h.

                                               {
     _state = state;
     _model.setState(state);  // force recalculate dynamics
   }

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::step	(	T	dt,
		const DVec< T > &	tau,
		T	kp,
		T	kd
	)

Initialize simulator with given model.

Take one simulation step

Parameters

dt	: timestep duration
tau	: joint torques

Definition at line 41 of file DynamicsSimulator.cpp.

References DynamicsSimulator< T >::_homing, DynamicsSimulator< T >::_model, DynamicsSimulator< T >::_state, f(), spatial::forceToSpatialForce(), DynamicsSimulator< T >::forwardKinematics(), DynamicsSimulator< T >::integrate(), ori::quatToso3(), ori::rotationMatrixToQuaternion(), ori::rpyToRotMat(), DynamicsSimulator< T >::runABA(), and DynamicsSimulator< T >::updateCollisions().

                                                                      {
   // fwd-kin on gc points
   // compute ground contact forces
   // aba
   // integrate
   forwardKinematics();           // compute forward kinematics
   updateCollisions(dt, kp, kd);  // process collisions
   // Process Homing
   if( _homing.active_flag) {
 
     Mat3<T> R10_des = rpyToRotMat(_homing.rpy);              // R10_des
     Mat3<T> R10_act = _model.getOrientation(5).transpose();  // R10
     Mat3<T> eR01 = R10_des.transpose()*R10_act;              // eR * R01 = R01_des
     
     Vec4<T> equat = rotationMatrixToQuaternion(eR01.transpose());
     Vec3<T> angle_axis = quatToso3(equat); // in world frame
 
     Vec3<T> p = _model.getPosition(5);
     Vec3<T> f = _homing.kp_lin*(_homing.position - p)-_homing.kd_lin*_model.getLinearVelocity(5);
 
     // Note: External forces are spatial forces in the {0} frame. 
     _model._externalForces.at(5) += forceToSpatialForce(f,p);
     _model._externalForces.at(5).head(3) += _homing.kp_ang*angle_axis - _homing.kd_ang*_model.getAngularVelocity(5);
 
   }
 
 
   runABA(tau);                   // dynamics algorithm
   integrate(dt);                 // step forward
 
   _model.setState(_state);
   _model.resetExternalForces();  // clear external forces
   _model.resetCalculationFlags();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void DynamicsSimulator< T >::updateCollisions	(	T	dt,
		T	kp,
		T	kd
	)

private

Definition at line 77 of file DynamicsSimulator.cpp.

References DynamicsSimulator< T >::_contact_constr, and DynamicsSimulator< T >::_model.

                                                             {
   _model.forwardKinematics();
   _contact_constr->UpdateExternalForces(kp, kd, dt);
 }

Here is the caller graph for this function:

Member Data Documentation

template<typename T>

vector<CollisionPlane<T> > DynamicsSimulator< T >::_collisionPlanes

private

Definition at line 125 of file DynamicsSimulator.h.

template<typename T>

ContactConstraint<T>* DynamicsSimulator< T >::_contact_constr

private

Definition at line 126 of file DynamicsSimulator.h.

template<typename T>

FBModelStateDerivative<T> DynamicsSimulator< T >::_dstate

private

Definition at line 123 of file DynamicsSimulator.h.

template<typename T>

RobotHomingInfo<T> DynamicsSimulator< T >::_homing

private

Definition at line 130 of file DynamicsSimulator.h.

template<typename T>

SVec<T> DynamicsSimulator< T >::_lastBodyVelocity

private

Definition at line 127 of file DynamicsSimulator.h.

template<typename T>

FloatingBaseModel<T>& DynamicsSimulator< T >::_model

private

Definition at line 124 of file DynamicsSimulator.h.

template<typename T>

FBModelState<T> DynamicsSimulator< T >::_state

private

Update ground collision list.

Definition at line 122 of file DynamicsSimulator.h.

template<typename T>

bool DynamicsSimulator< T >::_useSpringDamper

private

Definition at line 128 of file DynamicsSimulator.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

template<typename T> class DynamicsSimulator< T >

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<typename T>
class DynamicsSimulator< T >