gcop::Mbs Class Reference

#include <mbs.h>

Inheritance diagram for gcop::Mbs:

Collaboration diagram for gcop::Mbs:

List of all members.

Public Member Functions
	Mbs (int nb, int c, bool fixed=false)
virtual	~Mbs ()
void	Init ()
double	Step (MbsState &xb, double t, const MbsState &xa, const VectorXd &u, double h, const VectorXd *p=0, MatrixXd *A=0, MatrixXd *B=0, MatrixXd *C=0)
double	EulerStep (MbsState &xb, double t, const MbsState &xa, const VectorXd &u, double h, MatrixXd *A=0, MatrixXd *B=0)
double	HeunStep (MbsState &xb, double t, const MbsState &xa, const VectorXd &u, double h, MatrixXd *A=0, MatrixXd *B=0)
double	TrapStep (MbsState &xb, double t, const MbsState &xa, const VectorXd &u, double h, MatrixXd *A, MatrixXd *B)
void	NE (VectorXd &e, const VectorXd &vdr, MbsState &xb, double t, const MbsState &xa, const VectorXd &u, double h)
void	ID (VectorXd &f, double t, const MbsState &x, const VectorXd &u)
void	Bias (VectorXd &b, double t, const MbsState &x) const
void	DBias (VectorXd &b, double t, const MbsState &xb, const MbsState &xa, double h)
void	FK (MbsState &x)
void	KStep (MbsState &xb, const MbsState &xa, double h, bool impl=true)
void	NewtonEulerJacobian (MatrixXd &De, const MbsState &xb, const MbsState &xa, double h)
void	Mass (MatrixXd &M, const MbsState &x) const
void	Acc (VectorXd &a, double t, const MbsState &x, const VectorXd &u, double h)
virtual void	Force (VectorXd &f, double t, const MbsState &x, const VectorXd &u, MatrixXd *A=0, MatrixXd *B=0)
void	Rec (MbsState &x, double h)
void	ClampPose (MbsState &x, int i) const
void	CheckLimits (MbsState &x, int i, double h) const
void	GetImpulse (double f, const MbsState &x, int i, double h) const
void	ClampVelocity (MbsState &x) const
void	print (const MbsState &x) const
Public Attributes
int	nb
	number of rigid bodies (including base body)
bool	fixed
	whether base body is fixed
vector< Body3d<> >	links
	links
vector< Joint >	joints
	joints
vector< Matrix6d >	Ips
	A'*I*A (nb) vector.
vector< int >	pis
	parent indexes
vector< vector< int > >	cs
	child lists
Vector3d	ag
	acceleration due to gravity (0, 0, -9.81) by default
VectorXd	damping
	joint damping vector
VectorXd	lbK
	joint lower bound K term
VectorXd	lbD
	joint lower bound D term
VectorXd	ubK
	joint upper bound K term
VectorXd	ubD
	joint upper bound D term
VectorXd	fsl
	spring force lower bound
VectorXd	fsu
	spring force upper bound
int	basetype
	type of base (used for interpreting base-body forces)
SE3 &	se3
	singleton reference for performing SE(3) operations
int	method
	type of integration method employed (Euler by default)
int	iters
	max number of Newton iterations used in symplectic method
bool	debug
Static Public Attributes
static const int	FIXEDBASE = 0
	fixed base
static const int	FLOATBASE = 1
	fully controllable floating base
static const int	AIRBASE = 2
	specialized free-flying base with three torques and one lift force
static const int	EULER = 1
	Euler time-stepping of dynamics.
static const int	HEUN = 2
	Heun's explicit 2nd order method.
static const int	TRAP = 3
	symplectic trapezoidal 2nd order method

---

Detailed Description

General multi-body dynamics time-stepping. The discrete mechanics is based on an implicit symplectic variational integrator which is second-order accurate and momentum-balance preserving.

Author: Marin Kobilarov marin(at)jhu.edu

---

Constructor & Destructor Documentation

Mbs::Mbs	(	int	nb,
		int	c,
		bool	fixed = false
	)

Mbs::~Mbs

(

)

[virtual]

---

Member Function Documentation

void Mbs::Acc	(	VectorXd &	a,
		double	t,
		const MbsState &	x,
		const VectorXd &	u,
		double	h
	)

Compute acceleration

Parameters:

a	acceleration
t	time
x	state
u	control inputs
h	time-step

References Bias(), debug, fixed, Force(), Mass(), and nb.

Referenced by EulerStep(), and HeunStep().

void Mbs::Bias	(	VectorXd &	b,
		double	t,
		const MbsState &	x
	)		const

Compute total bias b, i.e. such that M*a + b = B*u

Parameters:

b	bias
t	time
x	state

References gcop::SE3::Ad(), ag, cs, debug, gcop::MbsState::dgs, fixed, gcop::MbsState::gs, gcop::SE3::inv(), joints, links, nb, se3, and gcop::MbsState::vs.

Referenced by Acc(), ID(), and gcop::MbsController::Set().

void Mbs::CheckLimits	(	MbsState &	x,
		int	i,
		double	h
	)		const

References gcop::MbsState::dr, gcop::MbsState::lb, gcop::Manifold< T, _n >::lb, gcop::MbsState::r, gcop::MbsState::ub, gcop::Manifold< T, _n >::ub, and gcop::System< MbsState >::X.

Referenced by KStep().

void Mbs::ClampPose	(	MbsState &	x,
		int	i
	)		const

References gcop::MbsState::gs, gcop::Manifold< T, _n >::lb, nb, gcop::Manifold< T, _n >::ub, and gcop::System< MbsState >::X.

void Mbs::ClampVelocity

(

MbsState &

x

)

const

References gcop::MbsState::dr, fixed, gcop::Manifold< T, _n >::lb, nb, gcop::Manifold< T, _n >::ub, gcop::MbsState::vs, and gcop::System< MbsState >::X.

void Mbs::DBias	(	VectorXd &	b,
		double	t,
		const MbsState &	xb,
		const MbsState &	xa,
		double	h
	)

Discrete bias

Parameters:

b	bias
t	time
xb	state
xa	state
h	time-step

References gcop::SE3::Ad(), ag, cs, debug, gcop::MbsState::dgs, gcop::MbsState::gs, gcop::SE3::inv(), joints, links, nb, se3, gcop::SE3::tlnmu(), and gcop::MbsState::vs.

Referenced by NE().

double Mbs::EulerStep	(	MbsState &	xb,
		double	t,
		const MbsState &	xa,
		const VectorXd &	u,
		double	h,
		MatrixXd *	A = 0,
		MatrixXd *	B = 0
	)

References Acc(), gcop::MbsState::dr, fixed, fsl, fsu, KStep(), lbD, lbK, nb, ubD, ubK, gcop::MbsState::vs, gcop::MbsState::zl, and gcop::MbsState::zu.

Referenced by Step().

void Mbs::FK

(

MbsState &

x

)

Forward kinematics: given base pose and joint angles, update the poses of all bodies in the graph.

Parameters:

x

state

References gcop::MbsState::dgs, gcop::SE3::exp(), gcop::MbsState::gs, joints, nb, pis, gcop::MbsState::r, and se3.

Referenced by Rec().

void Mbs::Force	(	VectorXd &	f,
		double	t,
		const MbsState &	x,
		const VectorXd &	u,
		MatrixXd *	A = 0,
		MatrixXd *	B = 0
	)		[virtual]

Total resulting force on the system from external (e.g. gravity) and internal (control) inputs

Parameters:

f	total force f(x,u,t)
t	time
x	state
u	controls
A	jacobian Dxf
B	jacobian Duf

Reimplemented in gcop::Airbase, gcop::Airbot, gcop::Airm, gcop::Snake, and gcop::Chain1.

References AIRBASE, basetype, damping, gcop::MbsState::dr, fixed, FLOATBASE, fsl, fsu, gcop::Manifold< T, _n >::lb, lbD, lbK, nb, gcop::MbsState::r, gcop::Manifold< T, _n >::ub, ubD, ubK, gcop::System< MbsState >::X, gcop::MbsState::zl, and gcop::MbsState::zu.

Referenced by Acc(), ID(), NE(), and TrapStep().

void Mbs::GetImpulse	(	double	f,
		const MbsState &	x,
		int	i,
		double	h
	)		const

References gcop::MbsState::lb, gcop::Manifold< T, _n >::lb, gcop::MbsState::r, gcop::MbsState::ub, gcop::Manifold< T, _n >::ub, and gcop::System< MbsState >::X.

double Mbs::HeunStep	(	MbsState &	xb,
		double	t,
		const MbsState &	xa,
		const VectorXd &	u,
		double	h,
		MatrixXd *	A = 0,
		MatrixXd *	B = 0
	)

References Acc(), gcop::MbsState::dr, KStep(), nb, and gcop::MbsState::vs.

Referenced by Step().

void Mbs::ID	(	VectorXd &	f,
		double	t,
		const MbsState &	x,
		const VectorXd &	u
	)

Inverse-dynamics function. Computes the residual of the dynamical update from xa to xb given controls u. This is regarded as the discrete-time equivalent of (b - B*u)

Parameters:

f	residual
t	time
x	state
u	controls

References Bias(), Force(), and nb.

void Mbs::Init

(

)

References cs, gcop::Joint::Init(), joints, nb, and pis.

Referenced by gcop::Airbot::Airbot(), gcop::Airm::Airm(), gcop::Chain1::Chain1(), and gcop::Snake::Snake().

void Mbs::KStep	(	MbsState &	xb,
		const MbsState &	xa,
		double	h,
		bool	impl = true
	)

Kinematic-step. Given previous state xa, the next state xb is updated by taking the joint angles from xa, and joint velocities from xb, updating the joint angles in xb, and updating the xb body poses

Parameters:

xa	previous state
xb	next state
h	time-step
impl	whether to update configuration using the newly updated velocity

References gcop::Joint::a, CheckLimits(), gcop::MbsState::dgs, gcop::MbsState::dr, gcop::SE3::exp(), fixed, gcop::Joint::gci, gcop::Joint::gp, gcop::MbsState::gs, gcop::SE3::inv(), joints, gcop::SE3::log(), nb, pis, print(), gcop::MbsState::r, se3, and gcop::MbsState::vs.

Referenced by EulerStep(), HeunStep(), NE(), and TrapStep().

void Mbs::Mass	(	MatrixXd &	M,
		const MbsState &	x
	)		const

Compute the mass matrix at a given state x

Parameters:

M	mass matrix
x	state

References gcop::SE3::Ad(), gcop::MbsState::dgs, fixed, gcop::SE3::inv(), joints, links, nb, pis, and se3.

Referenced by Acc(), and gcop::MbsController::Set().

void Mbs::NE	(	VectorXd &	e,
		const VectorXd &	vdr,
		MbsState &	xb,
		double	t,
		const MbsState &	xa,
		const VectorXd &	u,
		double	h
	)

References DBias(), gcop::MbsState::dr, Force(), KStep(), nb, and gcop::MbsState::vs.

Referenced by TrapStep().

void Mbs::NewtonEulerJacobian	(	MatrixXd &	De,
		const MbsState &	xb,
		const MbsState &	xa,
		double	h
	)

References gcop::SE3::Ad(), gcop::SE3::adt(), cs, gcop::MbsState::dgs, gcop::SE3::inv(), joints, links, nb, gcop::System< MbsState >::P, pis, gcop::Joint::S, se3, gcop::SE3::tln(), and gcop::MbsState::vs.

Referenced by TrapStep().

void Mbs::print

(

const MbsState &

x

)

const [virtual]

General print function to print the state

Reimplemented from gcop::System< MbsState >.

References gcop::MbsState::dgs, gcop::MbsState::dr, gcop::MbsState::gs, gcop::MbsState::r, and gcop::MbsState::vs.

Referenced by KStep(), and TrapStep().

void Mbs::Rec	(	MbsState &	x,
		double	h
	)		[virtual]

Reconstruct the full state. Some states contain redundant parameters for efficiency and it is useful to reconstruct them to maintain a consisten state.

Parameters:

x	state
h	time-step

Reimplemented from gcop::System< MbsState >.

References gcop::MbsState::dr, gcop::SE3::exp(), FK(), gcop::MbsState::gs, gcop::SE3::inv(), gcop::SE3::log(), nb, gcop::MbsState::r, se3, and gcop::MbsState::vs.

double Mbs::Step	(	MbsState &	xb,
		double	t,
		const MbsState &	xa,
		const VectorXd &	u,
		double	h,
		const VectorXd *	p = 0,
		MatrixXd *	A = 0,
		MatrixXd *	B = 0,
		MatrixXd *	C = 0
	)

References EULER, EulerStep(), HEUN, HeunStep(), method, TRAP, and TrapStep().

double Mbs::TrapStep	(	MbsState &	xb,
		double	t,
		const MbsState &	xa,
		const VectorXd &	u,
		double	h,
		MatrixXd *	A,
		MatrixXd *	B
	)

References gcop::MbsState::dr, Force(), iters, KStep(), gcop::Manifold< T, _n >::n, nb, NE(), NewtonEulerJacobian(), print(), gcop::System< MbsState >::U, and gcop::MbsState::vs.

Referenced by Step().

---

Member Data Documentation

Vector3d gcop::Mbs::ag

acceleration due to gravity (0, 0, -9.81) by default

Referenced by Bias(), gcop::Chain1::Chain1(), and DBias().

const int gcop::Mbs::AIRBASE = 2 [static]

specialized free-flying base with three torques and one lift force

Referenced by gcop::Airbot::Airbot(), and Force().

int gcop::Mbs::basetype

type of base (used for interpreting base-body forces)

Referenced by gcop::Airbot::Airbot(), gcop::Chain::Chain(), and Force().

vector<vector<int> > gcop::Mbs::cs

child lists

Referenced by Bias(), DBias(), Init(), and NewtonEulerJacobian().

VectorXd gcop::Mbs::damping

joint damping vector

Referenced by gcop::Airbot::Force(), and Force().

bool gcop::Mbs::debug

Referenced by Acc(), Bias(), gcop::Chain1::Chain1(), and DBias().

const int gcop::Mbs::EULER = 1 [static]

Euler time-stepping of dynamics.

Referenced by Step().

bool gcop::Mbs::fixed

whether base body is fixed

Referenced by Acc(), Bias(), ClampVelocity(), EulerStep(), Force(), KStep(), Mass(), and gcop::MbsController::Set().

const int gcop::Mbs::FIXEDBASE = 0 [static]

fixed base

Referenced by gcop::Chain::Chain().

const int gcop::Mbs::FLOATBASE = 1 [static]

fully controllable floating base

Referenced by Force().

VectorXd gcop::Mbs::fsl

spring force lower bound

Referenced by EulerStep(), and Force().

VectorXd gcop::Mbs::fsu

spring force upper bound

Referenced by EulerStep(), and Force().

const int gcop::Mbs::HEUN = 2 [static]

Heun's explicit 2nd order method.

Referenced by Step().

vector<Matrix6d> gcop::Mbs::Ips

A'*I*A (nb) vector.

int gcop::Mbs::iters

max number of Newton iterations used in symplectic method

Referenced by TrapStep().

vector<Joint> gcop::Mbs::joints

joints

Referenced by gcop::Airbot::Airbot(), gcop::Airm::Airm(), Bias(), gcop::Chain::Chain(), gcop::Chain1::Chain1(), DBias(), FK(), Init(), KStep(), Mass(), NewtonEulerJacobian(), and gcop::Snake::Snake().

VectorXd gcop::Mbs::lbD

joint lower bound D term

Referenced by EulerStep(), and Force().

VectorXd gcop::Mbs::lbK

joint lower bound K term

Referenced by EulerStep(), and Force().

vector<Body3d<> > gcop::Mbs::links

links

Referenced by gcop::Airbot::Airbot(), gcop::AirbotView::AirbotView(), gcop::Airm::Airm(), gcop::AirmView::AirmView(), Bias(), gcop::Chain::Chain(), gcop::Chain1::Chain1(), gcop::ChainView::ChainView(), DBias(), Mass(), NewtonEulerJacobian(), gcop::Snake::Snake(), and gcop::SnakeView::SnakeView().

int gcop::Mbs::method

type of integration method employed (Euler by default)

Referenced by Step().

int gcop::Mbs::nb

number of rigid bodies (including base body)

Referenced by Acc(), gcop::Airbot::Airbot(), Bias(), gcop::ChainView::ChainView(), ClampPose(), ClampVelocity(), DBias(), EulerStep(), FK(), gcop::Airbot::Force(), Force(), HeunStep(), ID(), Init(), KStep(), Mass(), NE(), NewtonEulerJacobian(), Rec(), gcop::MbsView::Render(), gcop::MbsController::Set(), TrapStep(), and gcop::ChainView::~ChainView().

vector<int> gcop::Mbs::pis

parent indexes

Referenced by gcop::Airbot::Airbot(), gcop::Airm::Airm(), gcop::Chain::Chain(), gcop::Chain1::Chain1(), FK(), Init(), KStep(), Mass(), NewtonEulerJacobian(), and gcop::Snake::Snake().

SE3& gcop::Mbs::se3

singleton reference for performing SE(3) operations

Referenced by gcop::Airbot::Airbot(), gcop::Airm::Airm(), Bias(), gcop::Chain::Chain(), gcop::Chain1::Chain1(), DBias(), FK(), KStep(), Mass(), NewtonEulerJacobian(), Rec(), and gcop::Snake::Snake().

const int gcop::Mbs::TRAP = 3 [static]

symplectic trapezoidal 2nd order method

Referenced by Step().

VectorXd gcop::Mbs::ubD

joint upper bound D term

Referenced by EulerStep(), and Force().

VectorXd gcop::Mbs::ubK

joint upper bound K term

Referenced by EulerStep(), and Force().

---

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

• lib/systems/mbs.h
• lib/systems/mbs.cc