dynamics.c File Reference

Introduction

Implementation of the dynamic-related functions used to implement path planning with differential constraints.

Definition in file dynamics.c.

Functions
void	GetPositionJacobian (double *p, double *x, double epsilon, boolean hasIE, Tchart *c, TDynamicSpace *ds)
	Evaluates the position Jacobian. More...
void	ComputeAcceleration (Tparameters *pr, boolean hasIE, double *x, double *u, Tchart *c, double *fullJ, TDynamicSpace *ds)
	Computes the acceleration. More...
void	DynamicStepCtResidue (TDynamicSpace *ds)
	Constant part of the projection residue. More...
double	DynamicStepResidue (Tparameters *pr, double h, double *yn, double *x, Tchart *c, TDynamicSpace *ds)
	Constant part of the projection residue. More...
void	EvaluatePermutation (TDynamicSpace *ds)
	Defines a permutation matrix. More...
boolean	ValidState (boolean negLag, TDynamicSpace *ds)
	Validades a dynamic state. More...
void	InitHandC (Tparameters *pr, Tworld *wo, TDynamicSpace *ds)
	Initializes data requred by the HandC. More...
void	ComputeHandC_FJH (Tparameters *pr, double *x, boolean computeEq, boolean computeHpv, TDynamicSpace *ds)
	Computes the function, the Jacobian and the Hessian. More...
void	ApplyExternalForces (Tparameters *pr, double *x, TDynamicSpace *ds)
	Apply the external forces to the dynamic computations. More...
void	ComputeC (double *x, double *u, double *C, TDynamicSpace *ds)
	Auxiliary function of HandC. More...
void	ComputeH (double *x, double *H, TDynamicSpace *ds)
	Auxiliary function of HandC. More...
void	HandC (Tparameters *pr, double *x, double *u, double *H, double *C, boolean computeEq, boolean computeHpv, TDynamicSpace *ds)
	H and C function. More...
void	DeleteHandC (TDynamicSpace *ds)
	Destructor. More...
void	dr (double *r, double *mA, double *vc, TDynamicSpace *ds)
	Auxiliary function for LQRComputePolicy. More...
void	dG (double *G, double *mA, double *BiRBt, TDynamicSpace *ds)
	Auxiliary function for LQRComputePolicy. More...
void	dGt (double *mA, double t, double *BiRBt, double *eA, TDynamicSpace *ds)
	Auxiliary function for LQRComputePolicy. More...
void	MechEnergy (Tparameters *pr, double *x, double *pe, double *ke, TDynamicSpace *ds)
	Computed the potential and kinetic energy of a system. More...
void	InitDynamicSpace (Tparameters *pr, boolean parallel, TJacobian *sJ, TAtlasBase *w, TDynamicSpace *ds)
	Initializes the dynamics space. More...
unsigned int	Getxdot (Tparameters *pr, double *x, double *u, double *a, TDynamicSpace *ds)
	Computes the derivative of a state. More...
double *	GetInverseActionCostMatrix (TDynamicSpace *ds)
	Returns a pointer to the cost matrix. More...
double	Time2Go (Tparameters *pr, unsigned int tree, Tchart *c, double *t, double *x, double *tg, double *xg, double *de, TDynamicSpace *ds)
	Linear approximation of the time to go. More...
double	ApproachState (Tparameters *pr, Tintegrator *intFunction, unsigned int tree, double *x, double tolq, double *q_rand, double tolg, double *goal, double *st, double *xb, double *ub, unsigned int *ns, double ***path, double ***actions, double **times, TDynamicSpace *ds)
	Approaches a given state. More...
boolean	kinoRRT (Tparameters *pr, Tintegrator *intFunction, double *pg, double *time, double *pl, unsigned int *ns, double ***path, unsigned int *da, double ***actions, double **times, TRRTStatistics *grst, TAtlasBase *w, Trrt *rrt)
	Extends a dynamic RRT until we reach a targed point. More...
boolean	kinobiRRT (Tparameters *pr, Tintegrator *intFunction, double *pg, double *time, double *pl, unsigned int *ns, double ***path, unsigned int *da, double ***actions, double **times, TRRTStatistics *grst, TAtlasBase *w, Trrt *rrt)
	Extends a dynamic bi-directional RRT until we reach a targed point. More...
boolean	kinoEST (Tparameters *pr, Tintegrator *intFunction, double *pg, double *time, double *pl, unsigned int *ns, double ***path, unsigned int *da, double ***actions, double **times, TRRTStatistics *grst, TAtlasBase *w, Trrt *rrt)
	Extends a dynamic EST until we reach a targed point. More...
void	ComputeInverseDynamics (Tparameters *pr, boolean hasIE, Tchart *c, double *x, double *acc, double *u, TDynamicSpace *ds)
	Inverse Dynamics problem. More...
unsigned int	NextDynamicState (Tparameters *pr, unsigned int tree, double delta, boolean gc, double *xn, double *yn, Tchart *c, double *u, double *h, double hmax, double *x, unsigned int *it, TDynamicSpace *ds)
	Next dynamical state. More...
unsigned int	NextDynamicStateLocalEuler (Tparameters *pr, unsigned int tree, double delta, double *xn, double *yn, Tchart *c, double *u, double *h, double hmax, double *x, TDynamicSpace *ds)
	Next dynamical state. More...
unsigned int	NextDynamicStateLocalRK4 (Tparameters *pr, unsigned int tree, double delta, double *xn, double *yn, Tchart *c, double *u, double *h, double hmax, double *x, TDynamicSpace *ds)
	Next dynamical state. More...
unsigned int	NextDynamicStateEuler (Tparameters *pr, unsigned int tree, double *h, double delta, double *x, double *u, double *xNew, TDynamicSpace *ds)
	Next dynamical state using the Euler method. More...
unsigned int	NextDynamicStateRK4 (Tparameters *pr, unsigned int tree, double *h, double delta, double *x, double *u, double *xNew, TDynamicSpace *ds)
	Next dynamical state using the RK4 method. More...
unsigned int	NumericIntegration (Tparameters *pr, Tintegrator *intFunction, unsigned int tree, double *xn, double *u, double delta, double intTime, double tolq, double *q_rand, double tolg, double *goal, unsigned int *ns, double ***path, double ***actions, double **times, double *time, double *x, TDynamicSpace *ds)
	System integration for some time interval. More...
void	Simulate (Tparameters *pr, Tintegrator *intFunction, unsigned int da, double *u, double *s, unsigned int steps, unsigned int *ns, double ***path, double ***actions, double **times, TDynamicSpace *ds)
	Dynamic simulation with a given action using the Euler method. More...
void	LinearizeDynamics (Tparameters *pr, Tchart *c, double *y0, double *x0, double *u0, double *Uc, double **mA, double **mB, double **vc, TDynamicSpace *ds)
	Computes the linear system y = Ay+Bu+c in parameter space. More...
double	LQRComputePolicy (Tparameters *pr, double *y0, double *y1, double tmax, double *mA, double *mB, double *vc, double *vd, double *iRBt, double *BiRBt, TDynamicSpace *ds)
	Determines a LQR policy in parameter space. More...
double	LQRComputePolicy_t (Tparameters *pr, double *y0, double *y1, double tmax, double *mA, double *mB, double *vc, double *vd, double *iRBt, double *BiRBt, TDynamicSpace *ds)
	Determines a LQR policy in parameter space. More...
double	LQRPolicy (Tparameters *pr, double *u, double t, TDynamicSpace *ds)
	Determines the optimal action given the policy computed in LQRComputePolicy. More...
unsigned int	ActionSpaceDimension (TDynamicSpace *ds)
	Dimension of the action space. More...
boolean	GetActionFromID (unsigned int id, double *u, TDynamicSpace *ds)
	Converts an indentifier to an action vector. More...
void	PrintDynamicsDefines (FILE *f)
	Prints defines. More...
void	DeleteDynamicSpace (TDynamicSpace *ds)
	Deletes the dynamic space. More...

Function Documentation

GetPositionJacobian()

void GetPositionJacobian	(	double *	p,
		double *	x,
		double	epsilon,
		boolean	hasIE,
		Tchart *	c,
		TDynamicSpace *	ds
	)

Evaluates the position Jacobian. Only the independent equations are relevant.

Parameters

p	The position variables.
x	The full state.
epsilon	Numerical accuracy.
hasIE	If the independent equations are already computed.
c	The chart.
ds	The dynamic space.

Definition at line 274 of file dynamics.c.

References TDynamicSpace::bJ, TDynamicSpace::bJp, TDynamicSpace::cJ, TDynamicSpace::dof, Error(), EvaluateTransposedJacobianInVector(), EvaluateTransposedJacobianSubSetInVector(), EvaluateTransposedNJacobianSubSetInVector(), FindIndependentRows(), GetChartJacobianBasis(), TDynamicSpace::J, TDynamicSpace::Jp, TDynamicSpace::ndxPE, TDynamicSpace::ne, TDynamicSpace::nipe, TDynamicSpace::nJp, TDynamicSpace::npe, TDynamicSpace::npv, TDynamicSpace::nv, TDynamicSpace::pEqs, PrintTMatrix(), TDynamicSpace::pVars, SelectMatrixColumns(), SelectMatrixRows(), TDynamicSpace::selEqs, TDynamicSpace::sJ, TDynamicSpace::sJp, TRUE, and VectorAnd().

Referenced by ComputeAcceleration(), and ComputeInverseDynamics().

ComputeAcceleration()

void ComputeAcceleration	(	Tparameters *	pr,
		boolean	hasIE,
		double *	x,
		double *	u,
		Tchart *	c,
		double *	fullJ,
		TDynamicSpace *	ds
	)

Given a dynamic space with all the relevant dynamic information, we compute the acceleration at a given state.

Parameters

pr	The set of parameters.
hasIE	TRUE if the independent equations in ds are already computed.
x	The state (including both position and velocity).
u	The action (forces, moments) to apply.
c	The current chart. Used to obtain the basis of the Jacobian (if possible).
fullJ	The Jacobian of the full system (i.e., the system including both position and velocity equations) can be easily obtained as a side product of the acceleration computation. If this pointer is not NULL it provides space where to store this Jacobian. This is exploited in NextDynamicState where the acceleration and the Jacobian of the full system are necessary.
ds	The dynamic information.

Definition at line 338 of file dynamics.c.

References TDynamicSpace::A, TDynamicSpace::bJ, TDynamicSpace::bJp, CT_EPSILON, TDynamicSpace::da, Error(), EvaluateHessianVector2(), EvaluateJacobianInVector(), EvaluateNHessianVector2(), FALSE, TDynamicSpace::g, TDynamicSpace::gamma, GetLSSolutionBuffer(), GetParameter(), GetPositionJacobian(), HandC(), HandC_Thread, TDynamicSpace::Hp, TDynamicSpace::Hpv, TDynamicSpace::Joo, TDynamicSpace::Jop, TDynamicSpace::Jot, TDynamicSpace::Jp, TDynamicSpace::lsSize, LSSolve(), TDynamicSpace::mA, MatrixVectorProduct(), TDynamicSpace::Mm, TDynamicSpace::ndxOV, TDynamicSpace::ndxPV, TDynamicSpace::ndxVV, TDynamicSpace::ne, TDynamicSpace::nHp, TDynamicSpace::nie, TDynamicSpace::nipe, TDynamicSpace::noe, TDynamicSpace::nov, TDynamicSpace::npv, TDynamicSpace::nv, TDynamicSpace::o, TDynamicSpace::pEqs, TDynamicSpace::pos, PrintMatrix(), PrintTMatrix(), PrintVector(), TDynamicSpace::pvEqs, TDynamicSpace::Qa, TDynamicSpace::qdd, RC2INDEX, TDynamicSpace::rhsA, ScaleVector(), TDynamicSpace::sHp, TDynamicSpace::sJoo, TDynamicSpace::sJop, TDynamicSpace::solA, SubMatrixFromMatrix(), SubMatrixFromTMatrix(), TRUE, and TDynamicSpace::vel.

Referenced by Getxdot(), LinearizeDynamics(), NextDynamicState(), NextDynamicStateEuler(), NextDynamicStateLocalEuler(), NextDynamicStateLocalRK4(), NextDynamicStateRK4(), and Time2Go().

DynamicStepCtResidue()

void DynamicStepCtResidue

(

TDynamicSpace *

ds

)

Computes the constant part of the projection residue. Used in the Broyden's before entering the iteration to find the solution.

NOTE: This assumes ComputeAcceleration has been called before with the current state (not passed as a parameter since all we need from it is the acceleration, stored as ds->g).

Parameters

ds	The dynamic space with the acceleration.

Definition at line 597 of file dynamics.c.

References TDynamicSpace::g, TDynamicSpace::gn, and TDynamicSpace::nv.

Referenced by NextDynamicState().

DynamicStepResidue()

double DynamicStepResidue	(	Tparameters *	pr,
		double	h,
		double *	yn,
		double *	x,
		Tchart *	c,
		TDynamicSpace *	ds
	)

Computes the constant part of Broyden's residue.

NOTE: This assumes ComputeAcceleration has been called before with x (so that the acceleration is for x is included in ds) and that the constant part of the projection error is also precomputed (and stored in ds) calling DynamicStepCtResidue

Parameters

pr	The set of parameters.
h	The integration step.
yn	Parameters in the chart of the previous point.
x	The current state and also the state used in the last call to ComputeAcceleration.
c	The current chart.
ds	The dynamic space with the acceleration at x.

Returns

The norm of the residue.

Definition at line 603 of file dynamics.c.

References TDynamicSpace::bJ, TDynamicSpace::bJp, CS_WD_EVALUATE_SUBSET_SIMP_EQUATIONS, DifferenceVector(), TDynamicSpace::dof, EvaluateSystemFromNJacobian(), TDynamicSpace::fe, TDynamicSpace::g, TDynamicSpace::gn, Manifold2Chart(), TDynamicSpace::ndxVV, TDynamicSpace::ne, TDynamicSpace::nie, TDynamicSpace::nipe, TDynamicSpace::nJp, TDynamicSpace::noe, Norm(), TDynamicSpace::nv, TDynamicSpace::oEqs, TDynamicSpace::pe, TDynamicSpace::pEqs, PrintVector(), TDynamicSpace::pvEqs, TDynamicSpace::selEqs, SumVector(), SumVectorScale(), TDynamicSpace::tp, VectorAnd(), TDynamicSpace::w, TDynamicSpace::xa, and TDynamicSpace::y.

Referenced by NextDynamicState().

EvaluatePermutation()

void EvaluatePermutation ( TDynamicSpace *  ds )

inline

Defines a permutation matrix to select particular actions in the inverse dynamics.

Parameters

ds	The dynamic space.

Definition at line 4593 of file dynamics.c.

References TDynamicSpace::a2x, TDynamicSpace::da, TDynamicSpace::npv, TDynamicSpace::P, and RC2INDEX.

Referenced by ComputeInverseDynamics().

ValidState()

boolean ValidState ( boolean  negLag, TDynamicSpace *  ds  )

inline

Checks if the the dynamic state is valid. For instance if the contact forces are the required ones. For instance, in the cable-driven robot ensures that the cables are always in tension.

This is used when computing the next dynamic step.

This assumes that ComputeAcceleration has been just used to update 'ds'. This is the case at the end of NextDynamicState which is wehre we use this function right now.

Parameters

negLag	TRUE if Lagrange multipliers should be negative.
ds	The dynamic information.

Returns

TRUE if the state is valid.

Definition at line 4641 of file dynamics.c.

References TDynamicSpace::solA, and TRUE.

Referenced by NextDynamicState(), NextDynamicStateEuler(), NextDynamicStateLocalEuler(), NextDynamicStateLocalRK4(), and NextDynamicStateRK4().

InitHandC()

void InitHandC	(	Tparameters *	pr,
		Tworld *	wo,
		TDynamicSpace *	ds
	)

Initializes the data fields in ds necessary to compute the H and C (see HandC).

Todo:

Extend the HandC procedure to deal with

Parameters

pr	The set of paremeters.
wo	The world structure (with links, joints, etc).
ds	The dynamic space to initialize.

Definition at line 664 of file dynamics.c.

References TDynamicSpace::a_grav, TDynamicSpace::auxSAM1, TDynamicSpace::auxSAM2, TDynamicSpace::auxSAV1, TDynamicSpace::auxSAV2, TDynamicSpace::auxSAV3, TDynamicSpace::avp, AXIS_H, AXIS_X, AXIS_Y, AXIS_Z, TDynamicSpace::bJHC, TDynamicSpace::cJoint, TDynamicSpace::closeTr, CT_G_AXIS, TDynamicSpace::damped, TDynamicSpace::dh, Error(), FALSE, TDynamicSpace::fe, TDynamicSpace::feHC, TDynamicSpace::fh, FIX_JOINT, TDynamicSpace::fout, TDynamicSpace::friction, TDynamicSpace::fvp, TDynamicSpace::gamma, TDynamicSpace::gammaHC, GetJointDamping(), GetJointDOF(), GetJointFriction(), GetJointPostT(), GetJointPreT(), GetJointType(), GetLinkInertialFrame(), GetLinkInertiaMatrix(), GetLinkMass(), GetParameter(), GetWorldClosureN(), GetWorldJoint2Dof(), GetWorldJointTo(), GetWorldLinkN(), GetWorldPredLink(), HandC_Thread, HTransform2SAM(), HTransformCopy(), HTransformGetElement(), HTransformIdentity(), HTransformInverse(), HTransformPrint(), HTransformProduct(), TDynamicSpace::IC, IdentitySAM(), TDynamicSpace::IM, IsConnectionSpring(), TDynamicSpace::iXaTr, TDynamicSpace::iXtreeTr, TDynamicSpace::iXupTr, TDynamicSpace::j, TDynamicSpace::j2x, TDynamicSpace::jID, JointFromID(), JointToID(), TDynamicSpace::jSgn, TDynamicSpace::l, TDynamicSpace::lID, M_G, MatrixMatrixProduct(), MatrixTMatrixProduct(), mci2rbi(), TDynamicSpace::nef, NEW, TDynamicSpace::nipe, TDynamicSpace::nj, TDynamicSpace::njx, TDynamicSpace::nl, TDynamicSpace::nlHC, TDynamicSpace::nLoops, NO_UINT, TDynamicSpace::noe, TDynamicSpace::npv, NumLoops(), TDynamicSpace::oID, TDynamicSpace::pLink, PrintMatrix(), RC2INDEX, TDynamicSpace::S, ScaleVector(), TDynamicSpace::v, TDynamicSpace::vJ, TDynamicSpace::vxS, TDynamicSpace::wo, TDynamicSpace::wr1, TDynamicSpace::wr2, TDynamicSpace::XJ, TDynamicSpace::Xtree, and TDynamicSpace::Xup.

Referenced by InitDynamicSpace().

ComputeHandC_FJH()

void ComputeHandC_FJH	(	Tparameters *	pr,
		double *	x,
		boolean	computeEq,
		boolean	computeHpv,
		TDynamicSpace *	ds
	)

Computes the function, the Jacobian and the Hessian re-using data computed in HandC.

Parameters

pr	The set of parameters.
x	The state.
computeEq	if true we compute the equations.
computeHpv	If true we compute the Hessian velocity matrix.
ds	The dynamic space with auxiliary data.

Definition at line 1025 of file dynamics.c.

References AddSubMatrix(), TDynamicSpace::auxSAM1, TDynamicSpace::auxSAM2, TDynamicSpace::auxSAV1, TDynamicSpace::avp, AXIS_H, AXIS_X, AXIS_Y, AXIS_Z, TDynamicSpace::bJHC, TDynamicSpace::cJoint, TDynamicSpace::closeTr, CS_WD_EVALUATE_SUBSET_SIMP_EQUATIONS, CT_EPSILON, TDynamicSpace::fe, TDynamicSpace::feHC, TDynamicSpace::gamma, TDynamicSpace::gammaHC, GetParameter(), TDynamicSpace::Hpv, HTransformGetElement(), HTransformInverse(), HTransformProduct(), HTransformSubstract(), HTransformXSAV(), TDynamicSpace::iXaTr, TDynamicSpace::j, TDynamicSpace::j2x, JointToID(), TDynamicSpace::Jp, MatrixFromSubMatrix(), MatrixMatrixProduct(), MatrixVectorProduct(), NEW, TDynamicSpace::nie, TDynamicSpace::nipe, TDynamicSpace::njx, TDynamicSpace::nl, TDynamicSpace::nLoops, TDynamicSpace::noe, TDynamicSpace::nov, TDynamicSpace::npv, TDynamicSpace::oEqs, TDynamicSpace::oID, TDynamicSpace::pLink, RowsDotProduct(), TDynamicSpace::S, SelectMatrixRows(), SubtractVector(), TRUE, TDynamicSpace::vxS, and TDynamicSpace::w.

Referenced by HandC().

ApplyExternalForces()

void ApplyExternalForces	(	Tparameters *	pr,
		double *	x,
		TDynamicSpace *	ds
	)

Auxiliary function of HandC that applies the external forces to the computation of C.

Parameters

pr	The set of parameters.
x	The state.
ds	The dynamic space with auxiliary data.

Definition at line 1195 of file dynamics.c.

References TDynamicSpace::auxSAV1, DeleteLinkTransforms(), TDynamicSpace::fvp, GetConnectionLinkWrench(), GetConnectLinkFrom(), GetConnectLinkTo(), GetLinkTransformsFromSolutionPoint(), HandC_Thread, HTransformPrint(), HTransformXSAV(), IsConnectionSpring(), TDynamicSpace::iXaTr, TDynamicSpace::l, TDynamicSpace::lID, TDynamicSpace::nlHC, NO_UINT, TDynamicSpace::oID, PrintMatrix(), PrintVector(), scaleSAV(), sumSAV(), TRUE, TSAMxSAV(), TDynamicSpace::wo, TDynamicSpace::wr1, and TDynamicSpace::wr2.

Referenced by HandC().

ComputeC()

void ComputeC	(	double *	x,
		double *	u,
		double *	C,
		TDynamicSpace *	ds
	)

Compute the C term of HandC

Parameters

x	The state.
u	The action.
C	The space for C
ds	The dynamic space information

Definition at line 1294 of file dynamics.c.

References TDynamicSpace::a2x, TDynamicSpace::da, TDynamicSpace::damped, TDynamicSpace::friction, TDynamicSpace::fvp, GetJointDamping(), GetJointFriction(), HandC_Thread, TDynamicSpace::j, TDynamicSpace::j2x, TDynamicSpace::jID, TDynamicSpace::lID, TDynamicSpace::njx, TDynamicSpace::nlHC, NO_UINT, TDynamicSpace::npv, TDynamicSpace::pLink, PrintMatrix(), PrintVector(), TDynamicSpace::S, SAVxSAV(), ScaleVector(), SGN, TMatrixVectorProduct(), TSAMxSAVAccum(), and TDynamicSpace::Xup.

Referenced by HandC().

ComputeH()

void ComputeH	(	double *	x,
		double *	H,
		TDynamicSpace *	ds
	)

Compute the H term of HandC

Parameters

x	The state.
H	The space for C
ds	The dynamic space information

Definition at line 1422 of file dynamics.c.

References TDynamicSpace::auxSAM1, TDynamicSpace::auxSAV1, copySAV(), TDynamicSpace::dh, TDynamicSpace::fh, HandC_Thread, TDynamicSpace::IC, TDynamicSpace::j2x, TDynamicSpace::lID, MatrixMatrixProduct(), TDynamicSpace::njx, TDynamicSpace::nlHC, NO_UINT, TDynamicSpace::npv, TDynamicSpace::pLink, PrintMatrix(), PrintVector(), RC2INDEX, TDynamicSpace::S, SAMxSAV(), SAVxSAV(), SubMatrixFromMatrix(), SubMatrixFromTMatrix(), TMatrixMatrixProduct(), TSAMxSAV(), and TDynamicSpace::Xup.

Referenced by HandC().

HandC()

void HandC	(	Tparameters *	pr,
		double *	x,
		double *	u,
		double *	H,
		double *	C,
		boolean	computeEq,
		boolean	computeHpv,
		TDynamicSpace *	ds
	)

This replicates the HandC function in the code by Featherstone. The only difference is that our C includes the action, thus, it is tau-C in Featherstone code.

Parameters

pr	The set of parameters.
x	The state.
u	The action.
H	The mass matrix to computed.
C	The action vector to comptute.
computeEq	if true we compute the equations.
computeHpv	If true we compute the Hessian velocity matrix.
ds	The dynamic space with auxiliary data.

Definition at line 1564 of file dynamics.c.

References TDynamicSpace::a_grav, ApplyExternalForces(), TDynamicSpace::auxSAM1, TDynamicSpace::auxSAM2, TDynamicSpace::auxSAV1, TDynamicSpace::auxSAV2, TDynamicSpace::auxSAV3, TDynamicSpace::avp, ComputeC(), ComputeH(), ComputeHandC_FJH(), copySAM(), copySAV(), crf(), CRFxSAM(), crm(), CRMxSAV(), TDynamicSpace::da, TDynamicSpace::fvp, GetColumn(), GetJointBasicTransform(), HandC_FJH, HandC_Thread, HTransform2SAM(), HTransformCopy(), HTransformProduct(), HTransformXSAV(), TDynamicSpace::IC, TDynamicSpace::IM, TDynamicSpace::iXaTr, TDynamicSpace::iXJTr, TDynamicSpace::iXtreeTr, TDynamicSpace::iXupTr, TDynamicSpace::j, TDynamicSpace::j2x, TDynamicSpace::jID, TDynamicSpace::jSgn, TDynamicSpace::lID, MatrixMatrixProduct(), MatrixVectorProduct(), TDynamicSpace::nef, TDynamicSpace::njx, TDynamicSpace::nlHC, NO_UINT, TDynamicSpace::nov, TDynamicSpace::npv, TDynamicSpace::nv, TDynamicSpace::pLink, PrintMatrix(), PrintVector(), TDynamicSpace::S, SAMxSAM(), SAMxSAMAccum(), SAMxSAV(), SAMxSAVAccum(), ScaleVector(), SetColumn(), sumSAV(), SumVectorScale(), TSAMxSAM(), TDynamicSpace::v, TDynamicSpace::vJ, TDynamicSpace::vxS, TDynamicSpace::XJ, TDynamicSpace::Xtree, and TDynamicSpace::Xup.

Referenced by ComputeAcceleration(), and ComputeInverseDynamics().

DeleteHandC()

void DeleteHandC

(

TDynamicSpace *

ds

)

Deletes the data allocated with InitHandC.

Parameters

ds	The dynamic space with the fields to delete.

Definition at line 2106 of file dynamics.c.

References TDynamicSpace::a_grav, TDynamicSpace::auxSAM1, TDynamicSpace::auxSAM2, TDynamicSpace::auxSAV1, TDynamicSpace::auxSAV2, TDynamicSpace::auxSAV3, TDynamicSpace::avp, TDynamicSpace::bJHC, TDynamicSpace::cJoint, TDynamicSpace::closeTr, TDynamicSpace::dh, TDynamicSpace::feHC, TDynamicSpace::fh, TDynamicSpace::fout, TDynamicSpace::fvp, TDynamicSpace::gammaHC, TDynamicSpace::IC, TDynamicSpace::IM, TDynamicSpace::iXaTr, TDynamicSpace::iXtreeTr, TDynamicSpace::iXupTr, TDynamicSpace::j2x, TDynamicSpace::jID, TDynamicSpace::jSgn, TDynamicSpace::lID, TDynamicSpace::nipe, TDynamicSpace::njx, TDynamicSpace::nlHC, TDynamicSpace::nLoops, TDynamicSpace::oID, TDynamicSpace::pLink, TDynamicSpace::S, TDynamicSpace::v, TDynamicSpace::vJ, TDynamicSpace::vxS, TDynamicSpace::wr1, TDynamicSpace::wr2, TDynamicSpace::XJ, TDynamicSpace::Xtree, and TDynamicSpace::Xup.

Referenced by DeleteDynamicSpace().

dr()

void dr	(	double *	r,
		double *	mA,
		double *	vc,
		TDynamicSpace *	ds
	)

Computes the derivative of 'r' on a given point. This is used to integrate 'r' possibly with different methods (Euler, RK4,...)

The output is stored in ds->dr_LQR.

Parameters

r	The point.
mA	Matrix A of the dynamic linearization.
vc	Free term of the dynamic linearization.
ds	The dynamic space.

Definition at line 4075 of file dynamics.c.

References AccumulateVector(), TDynamicSpace::dr_LQR, TDynamicSpace::lsSize_LQR, and MatrixVectorProduct().

Referenced by InitRRT(), LoadRRT(), and LQRComputePolicy().

dG()

void dG	(	double *	G,
		double *	mA,
		double *	BiRBt,
		TDynamicSpace *	ds
	)

Computes the derivative of 'G' on a given point. This is used to integrate 'G' possibly with different methods (Euler, RK4,...)

The output is stored in ds->dG_LQR.

Parameters

G	The point.
mA	Matrix A of the dynamic linearization.
BiRBt	Another term of the dynamic linearization.
ds	The dynamic space.

Definition at line 4081 of file dynamics.c.

References AccumulateTMatrix(), AccumulateVector(), TDynamicSpace::dG_LQR, TDynamicSpace::lsSize_LQR, and MatrixMatrixProduct().

Referenced by LQRComputePolicy().

dGt()

void dGt	(	double *	mA,
		double	t,
		double *	BiRBt,
		double *	eA,
		TDynamicSpace *	ds
	)

Computes the derivative of G in terms of time. This is used as for an alternative way to compute G (as an integration on time instead than an integration over G).

The output is stored in ds->dG_LQR.

Parameters

mA	Matrix A of the dynamic linearization.
t	The time.
BiRBt	Another term of the dynamic linearization.
eA	Space to store expm(t*A)
ds	The dynamic space.

Definition at line 4314 of file dynamics.c.

References TDynamicSpace::dG_LQR, TDynamicSpace::Gtemp_LQR, TDynamicSpace::lsSize_LQR, MatrixExponential(), MatrixMatrixProduct(), MatrixTMatrixProduct(), and SymmetrizeMatrix().

Referenced by LQRComputePolicy_t().

MechEnergy()

void MechEnergy	(	Tparameters *	pr,
		double *	x,
		double *	pe,
		double *	ke,
		TDynamicSpace *	ds
	)

This replicates part of the the EnerMo function in the code by Featherstone.

Parameters

pr	The set of parameters.
x	The state.
pe	The computed potential energy.
ke	The computed kinetic energy.
ds	The dynamic space with auxiliary data.

Definition at line 1918 of file dynamics.c.

References TDynamicSpace::a_grav, TDynamicSpace::auxSAM1, TDynamicSpace::auxSAM2, TDynamicSpace::auxSAV1, copySAM(), copySAV(), DeleteLinkTransforms(), GeneralDotProduct(), GetConnectionLinkPotentialEnergy(), GetJointBasicTransform(), GetLinkTransformsFromSolutionPoint(), HTransform2SAM(), HTransformCopy(), HTransformProduct(), HTransformXSAV(), TDynamicSpace::IC, TDynamicSpace::IM, IsConnectionSpring(), TDynamicSpace::j, TDynamicSpace::j2x, TDynamicSpace::jID, TDynamicSpace::jSgn, TDynamicSpace::l, TDynamicSpace::lID, MatrixMatrixProduct(), MatrixVectorProduct(), TDynamicSpace::nef, TDynamicSpace::njx, TDynamicSpace::nlHC, NO_UINT, TDynamicSpace::npv, TDynamicSpace::nv, TDynamicSpace::pLink, PrintMatrix(), PrintVector(), rbi2mci(), TDynamicSpace::S, SAMxSAM(), SAMxSAV(), SAVxSAV(), ScaleVector(), sumSAM(), sumSAV(), TRUE, TSAMxSAM(), TDynamicSpace::v, TDynamicSpace::vJ, TDynamicSpace::wo, TDynamicSpace::XJ, TDynamicSpace::Xtree, and TDynamicSpace::Xup.

Referenced by main().

InitDynamicSpace()

void InitDynamicSpace	(	Tparameters *	pr,
		boolean	parallel,
		TJacobian *	sJ,
		TAtlasBase *	w,
		TDynamicSpace *	ds
	)

Initializes the space to be used when computing the acceleration and during the integration.

Parameters

pr	The set of parameters of the problem.
parallel	TRUE if we allow part of the dynamic computations to be executed in parallel.
sJ	Symbolic Jacobian of the full system.
w	The structure holding the equations (position and velocity).
ds	The dynamic space to initialize.

Todo:

Include the mass matrix and force defining functions as parametres. We will do it when the corresponding interface is clear.

Definition at line 2180 of file dynamics.c.

References TDynamicSpace::A, TDynamicSpace::a2j, TDynamicSpace::a2x, TDynamicSpace::A_ID, TDynamicSpace::A_LQR, TDynamicSpace::action, ActuatedJoint(), AllocateHessianEvaluationAsVectors(), TDynamicSpace::B, TDynamicSpace::bJ, TDynamicSpace::bJp, TDynamicSpace::cJ, CopyNonDynamicsJacobian(), CopyPositionJacobian(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_POSITION_EQS, CS_WD_GET_POSITION_VARS, CS_WD_GET_POSITION_VELOCITY_EQS, CS_WD_GET_POSITION_VELOCITY_VARS, CS_WD_GET_SIMP_NHESSIAN, CS_WD_GET_SIMP_NJACOBIAN, CS_WD_GET_SIMP_TOPOLOGY, CT_N_DOF, CT_REPRESENTATION, CT_TAU, TDynamicSpace::d0_LQR, TDynamicSpace::d_LQR, TDynamicSpace::da, TDynamicSpace::dG_LQR, TDynamicSpace::dGtemp_LQR, TDynamicSpace::dof, TDynamicSpace::domain, TDynamicSpace::dr_LQR, TDynamicSpace::eA_LQR, TDynamicSpace::effort, Error(), FALSE, TDynamicSpace::fe, TDynamicSpace::fE_LQR, TDynamicSpace::g, TDynamicSpace::G_LQR, TDynamicSpace::gamma, TDynamicSpace::Gd, GET_WORLD, GetBroydenMatrixBuffer(), GetBroydenRHBuffer(), GetJacobianSize(), GetJointCost(), GetJointDOF(), GetJointEffort(), GetLSMatrixBuffer(), GetLSRHBuffer(), GetLSSolutionBuffer(), GetParameter(), GetScalarJacobian(), GetWorldActionDimension(), GetWorldJoint(), GetWorldLink(), GetWorldNJoints(), GetWorldNLinks(), TDynamicSpace::gn, TDynamicSpace::Gnew_LQR, TDynamicSpace::Gtemp_LQR, TDynamicSpace::gy, TDynamicSpace::Hp, TDynamicSpace::Hpv, IdentityMatrix(), InitBroyden(), InitHandC(), InitHessian(), InitLS(), TDynamicSpace::iR, TDynamicSpace::iRBt, TDynamicSpace::J, TDynamicSpace::j, TDynamicSpace::Joo, TDynamicSpace::Jop, TDynamicSpace::Jot, TDynamicSpace::Jp, TDynamicSpace::k1, TDynamicSpace::k2, TDynamicSpace::k3, TDynamicSpace::l, TDynamicSpace::lsSize, TDynamicSpace::lsSize_ID, TDynamicSpace::lsSize_LQR, TDynamicSpace::mA, TDynamicSpace::mA_ID, TDynamicSpace::mA_LQR, TDynamicSpace::mAt, TDynamicSpace::mB, TDynamicSpace::Mm, TDynamicSpace::ndxOE, TDynamicSpace::ndxOV, TDynamicSpace::ndxPE, TDynamicSpace::ndxPV, TDynamicSpace::ndxVE, TDynamicSpace::ndxVV, TDynamicSpace::ne, NEW, TDynamicSpace::nHp, TDynamicSpace::nie, TDynamicSpace::nipe, TDynamicSpace::nj, TDynamicSpace::nJp, TDynamicSpace::nl, TDynamicSpace::noe, TDynamicSpace::nov, TDynamicSpace::npe, TDynamicSpace::npv, NumLoops(), TDynamicSpace::nv, TDynamicSpace::o, TDynamicSpace::oEqs, ON_CUIKSYSTEM, TDynamicSpace::P, TDynamicSpace::parallel, TDynamicSpace::pe, TDynamicSpace::pEqs, TDynamicSpace::pos, TDynamicSpace::pVars, TDynamicSpace::pvEqs, TDynamicSpace::pvVars, TDynamicSpace::Qa, TDynamicSpace::qdd, TDynamicSpace::r_LQR, RC2INDEX, REP_JOINTS, TDynamicSpace::rhsA, TDynamicSpace::rhsA_ID, TDynamicSpace::rhsA_LQR, TDynamicSpace::rnew_LQR, TDynamicSpace::rtemp_LQR, TDynamicSpace::selEqs, TDynamicSpace::sHp, TDynamicSpace::sJ, TDynamicSpace::sJoo, TDynamicSpace::sJop, TDynamicSpace::sJp, TDynamicSpace::solA, TDynamicSpace::solA_ID, TDynamicSpace::solA_LQR, TOPOLOGY_S, TDynamicSpace::tp, TRUE, TDynamicSpace::vel, TDynamicSpace::w, TDynamicSpace::x0_LQR, TDynamicSpace::x_LQR, TDynamicSpace::xa, TDynamicSpace::y, and TDynamicSpace::y_LQR.

Referenced by InitAtlasRRT(), kinobiRRT(), kinoEST(), kinoRRT(), LoadAtlasRRT(), and main().

Getxdot()

unsigned int Getxdot	(	Tparameters *	pr,
		double *	x,
		double *	u,
		double *	a,
		TDynamicSpace *	ds
	)

Computes the derivative of a state. This includes the velocity, the acceleration and the derivative of the non-dynamic equations.

Parameters

pr	The set of parameters.
x	The state. This must be in the simplified form (i.e., in the internal representation used by Cuik). See WorldGenerateSimplifiedPointFromSystem to convert an input state (those typically stored in solution/trajectory files) to a simplified form.
u	The action.
a	The output acceleration.
ds	The dynamic space.

Returns

The size of the output vector (a).

Todo:

Implement the unsimplification of derivatives.

Definition at line 2598 of file dynamics.c.

References ComputeAcceleration(), FALSE, TDynamicSpace::g, and TDynamicSpace::nv.

Referenced by AtlasRRTValidateSample(), and main().

GetInverseActionCostMatrix()

double* GetInverseActionCostMatrix

(

TDynamicSpace *

ds

)

Returns a pointer to the invers of the matrix weighting the cost of each action in the LQR used a sterring method.

Parameters

ds	The dynamic space to query.

Returns

A pointer to the matrix.

Definition at line 2607 of file dynamics.c.

References TDynamicSpace::iR.

Referenced by UpdateLQRPolicy().

Time2Go()

double Time2Go	(	Tparameters *	pr,
		unsigned int	tree,
		Tchart *	c,
		double *	t,
		double *	x,
		double *	tg,
		double *	xg,
		double *	de,
		TDynamicSpace *	ds
	)

Linear approximation of the time to go from a given state to another one. This is implemented with the aim of improving the nearest-neighbour metric in RRTs. The information is better than the one given by a simple Euclidean metric but it is significantly slower to compute and it is only a first order approximation.

This is inspired in the derivation included in "An RRT-Based Algorithm for Testing and Validating Multi-Robot Controllers" by J. Kim, J. M. Esposito, and V. Kumar Robotics Science and Systems 2001.

The main difference is that we have a constraint manifold and, thus, we compute the time to go in tangent space. Consequently, we can only compute it for samples in the same tangent space (sharing the same local parameterization).

The time to go is computed as

• 

with d the distance between the goal and the current position and r the decrease ration of this distance. 'd' is computed as the Euclidean distance (between parameterizations). 'r' is more difficult to estimate. Actually an exact value for 'r' is as difficult as finding the solution to the two point boundary value problem (TPBVP) between the departing and the goal states. In general the TPBVP is hard and, thus we only find an approximation to the solution. This approximation is based on a linear approximation of the time to go evaluated at the departing state

• 

If 'd' is the Euclidean distance

• 

with . Moreover

• 

Thus,

• 

and

• 

In this context . We need to estimate . Probably the easiest estimation is to take it as 0. An alternative is to use the acceleration resulting from u=0 (null action). Yet another option is to select the action which results in lower time2go. This optimization can be approximated via sampling in the space of actions or computed in closed form via linear programming if can be expressed in linear form with respect to the actions (this is not implemented yet).

Finally, note that we compute the time to go in the parameter space (in this way v, makes some sense). Thus, 'x' is actually 't' and is the second half of 't' (the one giving the velocity parameters).

Parameters

pr	The set of parameters.
tree	The tree that will be extended. START2GOAL is extended with forward dynamics (positive times) and GOAL2START is extended with backward dynamics (negative times).
c	The chart including the sample and the goal sample.
t	The parameters of the departing state in the current chart.
x	The departing state.
tg	The parameters of the goal state in the current chart. This is the chart also including the departing state.
xg	The goal state.
de	Euclidean distance (to be used if Time2Go is not giving any information).
ds	The dynamic space.

Returns

The time to go or INF if no action can approach x to xg.

Definition at line 2612 of file dynamics.c.

References TDynamicSpace::action, ComputeAcceleration(), CT_EPSILON, TDynamicSpace::da, DifferenceVector(), TDynamicSpace::dof, FALSE, TDynamicSpace::g, GeneralDotProduct(), GetChartTangentSpace(), GetParameter(), INF, NEW, TDynamicSpace::nie, Norm(), TDynamicSpace::nv, and TMatrixVectorProduct().

Referenced by AddBranchToAtlasDynamicRRT(), GetRRTNNInChart(), and Time2GoNNToTree().

ApproachState()

double ApproachState	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		unsigned int	tree,
		double *	x,
		double	tol,
		double *	q_rand,
		double	tolg,
		double *	goal,
		double *	st,
		double *	xb,
		double *	ub,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		TDynamicSpace *	ds
	)

Simulates different actions with the aim of approaching a given goal state and identifies the action that approaches most to the goal.

Parameters

pr	The set of parameters.
intFunction	The integration function to use.
tree	The tree including the samples.
x	The state from where to start the simulation.
tol	Tolerance to reach q_rand.
q_rand	The targed state.
tolg	Tolerance to reach the goal.
goal	The goal state.
st	The time span of the best action.
xb	The best state (the closest to q_rand).
ub	The best action (the one taking from x to xb).
ns	Number of intermediate steps from x to xb.
path	Intermediated states from x to xb.
actions	Action to get to each intermediate step. Typically the same action for all.
times	Time between intermediate steps.
ds	The dynamic space.

Returns

The minimum distance to the goal, i.e., the distance between xb and the q_rand.

Definition at line 2687 of file dynamics.c.

References CT_DELTA, CT_INTEGRATION_TIME, CT_N_DYNAMIC_ACTIONS, TDynamicSpace::da, DeleteTrajectory(), DistanceTopology(), GetActionFromID(), GetParameter(), INF, NEW, NumericIntegration(), TDynamicSpace::nv, randomDouble(), and TDynamicSpace::tp.

Referenced by kinobiRRT(), and kinoRRT().

kinoRRT()

boolean kinoRRT	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		double *	pg,
		double *	time,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		unsigned int *	da,
		double ***	actions,
		double **	times,
		TRRTStatistics *	grst,
		TAtlasBase *	w,
		Trrt *	rrt
	)

Adds as many branches as necessary to the RRT (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance).

The tree extension is performed using the strategy defined in Dalibard, S., Nakhaei, A., Lamiraux, F., and Laumond, J.-P. (2009). Whole-body task planning for a humanoid robot: a way to integrate collision avoidance. In IEEE-RAS International Conference on Humanoid Robots, pages 355-360.

Parameters

pr	The set of parameters.
intFunction	The integration function.
pg	A point on the manifold to reach with the RRT. This is also a sample including only system variables.
time	Actual time used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
da	Dimension of the action space. Only used in dynamical systems. Otherwise is 0.
actions	Action executed to reach step in the soluiont. Only used in dynamical systems.
times	Time to reach each state. Only used in dynamical systems.
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
w	The base type with the equations (and possibly obstacles).
rrt	The RRT to expand.

Returns

TRUE if the path exists.

Definition at line 2749 of file dynamics.c.

References AddNodeToRRT(), ApproachState(), CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_JACOBIAN, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_DYNAMIC_GOAL_ERROR, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, TDynamicSpace::da, DeleteDynamicSpace(), DeleteJacobian(), DeleteStatistics(), DeleteTrajectory(), DistanceTopology(), TDynamicSpace::domain, Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTMode(), GetRRTNode(), GetRRTNumNodes(), INF, InitDynamicSpace(), InitStatistics(), TDynamicSpace::ne, NEW, NO_UINT, TDynamicSpace::nv, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PointInRRT(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), START2GOAL, TDynamicSpace::tp, TRUE, and Warning().

Referenced by main().

kinobiRRT()

boolean kinobiRRT	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		double *	pg,
		double *	time,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		unsigned int *	da,
		double ***	actions,
		double **	times,
		TRRTStatistics *	grst,
		TAtlasBase *	w,
		Trrt *	rrt
	)

Adds as many branches as necessary to the RRT (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance).

The tree extension is performed using the strategy defined in Dalibard, S., Nakhaei, A., Lamiraux, F., and Laumond, J.-P. (2009). Whole-body task planning for a humanoid robot: a way to integrate collision avoidance. In IEEE-RAS International Conference on Humanoid Robots, pages 355-360.

Parameters

pr	The set of parameters.
intFunction	The integration function.
pg	A point on the manifold to reach with the RRT. This is also a sample including only system variables.
time	Actual time used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
da	Dimension of the action space. Only used in dynamical systems. Otherwise is 0.
actions	Action executed to reach step in the soluiont. Only used in dynamical systems.
times	Time to reach each state. Only used in dynamical systems.
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
w	The base type with the equations (and possibly obstacles).
rrt	The RRT to expand.

Returns

TRUE if the path exists.

Definition at line 2898 of file dynamics.c.

References AddNodeToRRT(), ApproachState(), CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_JACOBIAN, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_DYNAMIC_GOAL_ERROR, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, TDynamicSpace::da, DeleteDynamicSpace(), DeleteJacobian(), DeleteStatistics(), DeleteTrajectory(), DistanceTopology(), TDynamicSpace::domain, Error(), FALSE, GetElapsedTime(), GetParameter(), GetRRTMode(), GetRRTNN(), GetRRTNode(), GetRRTNumNodes(), GOAL2START, INF, InitDynamicSpace(), InitStatistics(), TDynamicSpace::ne, NEW, TDynamicSpace::nv, PathStart2GoalInRRT(), PointInBoxTopology(), PointInRRT(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), START2GOAL, TDynamicSpace::tp, TRUE, TWO_TREES, and Warning().

Referenced by main().

kinoEST()

boolean kinoEST	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		double *	pg,
		double *	time,
		double *	pl,
		unsigned int *	ns,
		double ***	path,
		unsigned int *	da,
		double ***	actions,
		double **	times,
		TRRTStatistics *	grst,
		TAtlasBase *	w,
		Trrt *	rrt
	)

Adds as many branches as necessary to the EST (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance).

The tree extension is performed using the strategy defined in HSU et al.

Parameters

pr	The set of parameters.
intFunction	The integration function.
pg	A point on the manifold to reach with the RRT. This is also a sample including only system variables.
time	Actual time used in the planning.
pl	The length of the output path, if any. If no path is found. this is undefined.
ns	Number of steps of the output path (if any).
path	Configurations defining the output path (points on the manifold).
da	Dimension of the action space. Only used in dynamical systems. Otherwise is 0.
actions	Action executed to reach step in the soluiont. Only used in dynamical systems.
times	Time to reach each state. Only used in dynamical systems.
grst	Global RRT statistics. Used when collecting averages over different RRT executions. Otherwise is NULL.
w	The base type with the equations (and possibly obstacles).
rrt	The RRT to expand.

Returns

TRUE if the path exists.

Definition at line 3096 of file dynamics.c.

References AddNodeToRRT(), CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_JACOBIAN, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_DYNAMIC_GOAL_ERROR, CT_EPSILON, CT_GAMMA, CT_INTEGRATION_TIME, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_N_DYNAMIC_ACTIONS, TDynamicSpace::da, DeleteDynamicSpace(), DeleteJacobian(), DeleteStatistics(), DeleteTrajectory(), DistanceTopology(), TDynamicSpace::domain, Error(), FALSE, GetActionFromID(), GetElapsedTime(), GetParameter(), GetRRTMode(), GetRRTNNInBall(), GetRRTNode(), GetRRTNodeCost(), GetRRTNumNodes(), InitDynamicSpace(), InitStatistics(), TDynamicSpace::ne, NEW, NO_UINT, NumericIntegration(), TDynamicSpace::nv, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PointInRRT(), randomDouble(), randomMax(), RRT_VERBOSE, SetRRTNodeCost(), START2GOAL, TDynamicSpace::tp, TRUE, and Warning().

Referenced by main().

ComputeInverseDynamics()

void ComputeInverseDynamics	(	Tparameters *	pr,
		boolean	hasIE,
		Tchart *	c,
		double *	x,
		double *	acc,
		double *	u,
		TDynamicSpace *	ds
	)

Computes the input u for the inverse dynamics of a closed kinematic chain.

Parameters

pr	The set of parameters.
hasIE	TRUE if the independent equations in ds are already computed.
c	The chart parametrizing x.
x	The state (including both position and velocity).
acc	Desired acceleration.
u	The action (forces, moments) of the inverse dynamics (output).
ds	The dynamic information.

Definition at line 3270 of file dynamics.c.

References TDynamicSpace::A_ID, CT_EPSILON, TDynamicSpace::da, Error(), EvaluatePermutation(), FALSE, GetParameter(), GetPositionJacobian(), HandC(), TDynamicSpace::Jp, TDynamicSpace::lsSize_ID, LSSolve(), TDynamicSpace::mA_ID, MatrixVectorProduct(), TDynamicSpace::Mm, TDynamicSpace::ndxPV, TDynamicSpace::nipe, TDynamicSpace::npv, TDynamicSpace::P, TDynamicSpace::pos, TDynamicSpace::Qa, TDynamicSpace::rhsA_ID, TDynamicSpace::solA_ID, SubMatrixFromMatrix(), SubMatrixFromTMatrix(), and SubtractVector().

Referenced by ConnectDynamicStatesID(), and NextDynamicState().

NextDynamicState()

unsigned int NextDynamicState	(	Tparameters *	pr,
		unsigned int	tree,
		double	delta,
		boolean	gc,
		double *	xn,
		double *	yn,
		Tchart *	c,
		double *	u,
		double *	h,
		double	hmax,
		double *	x,
		unsigned int *	it,
		TDynamicSpace *	ds
	)

Computes next state using the implicit trapezoid method.

Parameters

pr	System parameters.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
delta	Expected step size. We try to adjust the time (h) so that the distance moved in tangent space is delta. This bound is not enforced thought (the actual displacement may be different from delta).
gc	Gravity compensation. If true the action is interpreted as an increment over the gravity compensantion.
xn	Current state.
yn	Parameters of the current state in the currrent chart.
c	Current chart.
u	Applied action.
h	The integration time (output).
hmax	Maximum value for h.
x	Next state (output).
it	Number of iterations in the Broyden. This information is returned for collecting statistics at the level of AtlasRRT (the caller of this function).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem converging to the next state, and 2 if the next state is not valid due to dynamic constraints.

Definition at line 3321 of file dynamics.c.

References TDynamicSpace::a2j, AccumulateVector(), TDynamicSpace::action, TDynamicSpace::B, BroydenStep(), BroydenUpdateJacobian(), Chart2Manifold(), ComputeAcceleration(), ComputeInverseDynamics(), CT_EPSILON, CT_MAX_NEWTON_ITERATIONS, CT_NEG_LM, TDynamicSpace::da, TDynamicSpace::dof, DynamicStepCtResidue(), DynamicStepResidue(), TDynamicSpace::effort, FALSE, TDynamicSpace::fe, TDynamicSpace::g, GetChartCenter(), GetChartTangentSpace(), GetParameter(), TDynamicSpace::gn, GOAL2START, TDynamicSpace::gy, IdentityMatrix(), INF, TDynamicSpace::J, TDynamicSpace::mB, TDynamicSpace::ndxPV, NextDynamicStateLocalRK4(), TDynamicSpace::nie, TDynamicSpace::nipe, Norm(), TDynamicSpace::npv, TDynamicSpace::nv, PrintMatrix(), PrintTMatrix(), PrintVector(), ResetBroyden(), TDynamicSpace::sJ, SubMatrixFromMatrix(), SubMatrixFromTMatrix(), SumVectorScale(), TMatrixVectorProduct(), TDynamicSpace::tp, TRUE, ValidState(), and TDynamicSpace::xa.

Referenced by ConnectDynamicStates(), ConnectDynamicStatesID(), and NewTemptativeSample().

NextDynamicStateLocalEuler()

unsigned int NextDynamicStateLocalEuler	(	Tparameters *	pr,
		unsigned int	tree,
		double	delta,
		double *	xn,
		double *	yn,
		Tchart *	c,
		double *	u,
		double *	h,
		double	hmax,
		double *	x,
		TDynamicSpace *	ds
	)

Computes next state using the explicit Euler method in local coordinates.

The difference with respect NextDynamicStateEuler is that here the final point is foced to be on the manifold. The Euler method is applied in local coordinates the final parameter is projected to the manifold.

Parameters

pr	System parameters.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
delta	Expected step size. We try to adjust the time (h) so that the distance moved in tangent space is delta. This bound is not enforced thought (the actual displacement may be different from delta).
xn	Current state.
yn	Parameters of the current state in the currrent chart.
c	Current chart.
u	Applied action.
h	The integration time (output).
hmax	Maximum value for h.
x	Next state (output).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem converging to the next state, and 2 if the next state is not valid due to dynamic constraints.

Definition at line 3509 of file dynamics.c.

References Chart2Manifold(), ComputeAcceleration(), CT_NEG_LM, TDynamicSpace::dof, FALSE, TDynamicSpace::g, GetChartTangentSpace(), GetParameter(), GOAL2START, TDynamicSpace::gy, INF, NextDynamicStateEuler(), TDynamicSpace::nie, Norm(), TDynamicSpace::nv, TDynamicSpace::sJ, SumVectorScale(), TMatrixVectorProduct(), TDynamicSpace::tp, ValidState(), and TDynamicSpace::y.

Referenced by NewTemptativeSample(), and NextDynamicStateLocalRK4().

NextDynamicStateLocalRK4()

unsigned int NextDynamicStateLocalRK4	(	Tparameters *	pr,
		unsigned int	tree,
		double	delta,
		double *	xn,
		double *	yn,
		Tchart *	c,
		double *	u,
		double *	h,
		double	hmax,
		double *	x,
		TDynamicSpace *	ds
	)

Computes next state using the explicit RK4 method in local coordinates.

The difference with respect NextDynamicStateRK4 is that here the final point is foced to be on the manifold. The RK4 method is applied in local coordinates the final parameter is projected to the manifold.

Parameters

pr	System parameters.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
delta	Expected step size. We try to adjust the time (h) so that the distance moved in tangent space is delta. This bound is not enforced thought (the actual displacement may be different from delta).
xn	Current state.
yn	Parameters of the current state in the currrent chart.
c	Current chart.
u	Applied action.
h	The integration time (output).
hmax	Maximum value for h.
x	Next state (output).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem converging to the next state, and 2 if the next state is not valid due to dynamic constraints.

Definition at line 3562 of file dynamics.c.

References Chart2Manifold(), ComputeAcceleration(), CT_NEG_LM, TDynamicSpace::dof, FALSE, TDynamicSpace::g, GetChartTangentSpace(), GetParameter(), GOAL2START, TDynamicSpace::gy, INF, TDynamicSpace::k1, TDynamicSpace::k2, TDynamicSpace::k3, NextDynamicStateLocalEuler(), NextDynamicStateRK4(), TDynamicSpace::nie, Norm(), TDynamicSpace::nv, TDynamicSpace::sJ, SumVector(), SumVectorScale(), TMatrixVectorProduct(), TDynamicSpace::tp, TRUE, ValidState(), and TDynamicSpace::y.

Referenced by NewTemptativeSample(), and NextDynamicState().

NextDynamicStateEuler()

unsigned int NextDynamicStateEuler	(	Tparameters *	pr,
		unsigned int	tree,
		double *	h,
		double	delta,
		double *	x,
		double *	u,
		double *	xNew,
		TDynamicSpace *	ds
	)

Computes next state using forward Euler method.

Parameters

pr	The set of parameters.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
h	Time interval. We need to approximate the average acceleration in this interval. It may be negative.
delta	The maximum distance between consecutive states. If not INF the time interval h will be adapted accordingly. We try to adjust the time (h) so that the distance moved in tangent space is delta. This bound is not enforced thought (the actual displacement may be different from delta).
x	Current state.
u	Applied action.
xNew	Next state (output).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem with the next state (the integration could not be fully computed up to the given integration time).

Definition at line 3654 of file dynamics.c.

References ComputeAcceleration(), CT_NEG_LM, FALSE, TDynamicSpace::g, GetParameter(), GOAL2START, INF, Norm(), TDynamicSpace::nv, SumVectorScale(), and ValidState().

Referenced by main(), NewTemptativeSample(), and NextDynamicStateLocalEuler().

NextDynamicStateRK4()

unsigned int NextDynamicStateRK4	(	Tparameters *	pr,
		unsigned int	tree,
		double *	h,
		double	delta,
		double *	x,
		double *	u,
		double *	xNew,
		TDynamicSpace *	ds
	)

Computes next state using 4th order Runge-Kutta.

Parameters

pr	The set of parameters.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
h	Time interval. We need to approximate the average acceleration in this interval. It may be negative.
delta	The maximum distance between consecutive states. If not INF the time interval h will be adapted accordingly. We try to adjust the time (h) so that the distance moved in tangent space is delta. This bound is not enforced thought (the actual displacement may be different from delta).
x	Current state.
u	Applied action.
xNew	Next state (output).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem with the next state (the integration could not be fully computed up to the given integration time).

Definition at line 3696 of file dynamics.c.

References ComputeAcceleration(), CT_NEG_LM, FALSE, TDynamicSpace::g, GetParameter(), GOAL2START, INF, TDynamicSpace::k1, TDynamicSpace::k2, TDynamicSpace::k3, Norm(), TDynamicSpace::nv, SumVector(), SumVectorScale(), TRUE, and ValidState().

Referenced by main(), NewTemptativeSample(), and NextDynamicStateLocalRK4().

NumericIntegration()

unsigned int NumericIntegration	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		unsigned int	tree,
		double *	xn,
		double *	u,
		double	delta,
		double	intTime,
		double	tolq,
		double *	q_rand,
		double	tolg,
		double *	goal,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		double *	time,
		double *	x,
		TDynamicSpace *	ds
	)

Computes the state after some time using a given numerical procedure to approximte the average acceleration in the given time interval. During the integration we check for possible non-valid dynamic states (this is actually done in the intFunction) and possible collisions with obstacles. In this case, the integration is stopped before compleating the given integration time.

Parameters

pr	System parameters.
intFunction	The function to use to compute the average acceleration. Typical procedures are the Euler or the RK4 methods.
tree	Tree being extended. If the tree is GOAL2START the integration is done back in time (although the returned integration time is positive).
xn	Current state.
u	Applied action.
delta	The time step size for each individual integration step.
intTime	Total integration time.
tolq	Tolerance for reaching q_rand.
q_rand	q_rand
tolg	Tolerance for reaching the goal.
goal	The goal.
ns	Number of steps taken to complete the integration. Only used if not NULL.
path	The states in each individual integration step. This includes all the generated states but the firt one. Only used if not NULL.
actions	Action executed to in each integration step. Right now the same action is used, but we may use a smooth transition between actions in the future. Only used if not NULL.
times	Time of each individual integration step.
time	The actual integration time: accumulated for all integration steps. Should be intTime if nothing extrange was found in intermediate states: collision... (output).
x	Next state. The last state of the integration (output).
ds	The dynamic information.

Returns

0 if the step is executed normally, 1 if there is a problem with the next state (the integration could not be fully computed up to the given integration time).

Definition at line 3756 of file dynamics.c.

References AddStep2Trajectory(), CS_WD_IN_COLLISION, CT_EPSILON, TDynamicSpace::da, DistanceTopology(), TDynamicSpace::domain, FALSE, GetParameter(), INF, InitTrajectory(), NEW, TDynamicSpace::nv, PointInBoxTopology(), TDynamicSpace::tp, TRUE, and TDynamicSpace::w.

Referenced by ApproachState(), kinoEST(), and Simulate().

Simulate()

void Simulate	(	Tparameters *	pr,
		Tintegrator *	intFunction,
		unsigned int	da,
		double *	u,
		double *	s,
		unsigned int	steps,
		unsigned int *	ns,
		double ***	path,
		double ***	actions,
		double **	times,
		TDynamicSpace *	ds
	)

Dynamic simulation from a given point applying a given action for the integration time fixed in the paremeter structure using the Euler method

Parameters

pr	The set of parameters.
intFunction	The integration function.
da	Dimension of the action space. Number of elements in u and in each vector in actions.
u	The action to apply.
s	The initial sample.
steps	Steps to simulate (each of time INTEGRATION_TIME).
ns	Number of steps of the output paht (if any).
path	Configurations defining the output path (points on the manifold).
actions	The action executed at each configuration. In this particular function this is just a repetition of the input action u.
times	The time between each configuration.
ds	The dynamic information.

Definition at line 3837 of file dynamics.c.

References ActionSpaceDimension(), AddStep2Trajectory(), CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SYSTEM_VARS, CS_WD_INIT_CD, CS_WD_REGENERATE_ORIGINAL_POINT, CS_WD_REGENERATE_SOLUTION_POINT, CT_DELTA, CT_INTEGRATION_TIME, Error(), FALSE, GetParameter(), InitTrajectory(), NEW, NumericIntegration(), TDynamicSpace::nv, START2GOAL, and TDynamicSpace::w.

Referenced by main().

LinearizeDynamics()

void LinearizeDynamics	(	Tparameters *	pr,
		Tchart *	c,
		double *	y0,
		double *	x0,
		double *	u0,
		double *	Uc,
		double **	mA,
		double **	mB,
		double **	vc,
		TDynamicSpace *	ds
	)

Computes the matrices of the linear system and store them in the chart info. Note that the linearization is made around the zero input trajectory.

Parameters

pr	The set of parameters.
c	The chart including the sample and the goal sample.
y0	The parameter where to linearize.
x0	The state where to linearize.
u0	The input where to linearize.
Uc	Basis of the current tangent space.
mA	Linearization w.r.t. the parameters (matrix 2*dof x 2*dof).
mB	Linearization w.r.t. the actions (matrix 2*dof x dof).
vc	The independent term of the linearization (vector 2*dof).
ds	The dynamic information. If the linearization is to be executed in parallel (i.e., if OPENMP is available and this function is not called from a parallel code), ds must be an array of DynamicSpaces (with the size of the available threads).

Definition at line 3921 of file dynamics.c.

References TDynamicSpace::action, Chart2Manifold(), ComputeAcceleration(), CT_EPSILON, TDynamicSpace::da, TDynamicSpace::dof, FALSE, TDynamicSpace::fE_LQR, TDynamicSpace::g, GetParameter(), NEW, TDynamicSpace::nie, TDynamicSpace::nv, TDynamicSpace::parallel, PrintMatrix(), PrintVector(), ScaleVector(), SetColumn(), TDynamicSpace::sJ, SubtractVector(), TMatrixVectorProduct(), TDynamicSpace::tp, TDynamicSpace::x0_LQR, TDynamicSpace::x_LQR, and TDynamicSpace::y_LQR.

Referenced by UpdateLQRPolicy().

LQRComputePolicy()

double LQRComputePolicy	(	Tparameters *	pr,
		double *	y0,
		double *	y1,
		double	tmax,
		double *	mA,
		double *	mB,
		double *	vc,
		double *	vd,
		double *	iRBt,
		double *	BiRBt,
		TDynamicSpace *	ds
	)

Determines an open loop LQR policy in parameter spac, i.e., find sthe optimal u(t) that connects the initial y0 with the random sample y1 for the linear system of the form y_dot = A*y+B*u+C

Parameters

pr	The set of parameters.
y0	The initial parameter.
y1	The goal parameter.
tmax	Maximum horizon.
mA	A matrix of the linear system (linearization from nonlinear system).
mB	B matrix of the linear system (linearization from nonlinear system).
vc	c vector of the linear system (linearization from nonlinear system).
vd	Not used. Just defined for compatibility with LQRComputePolicy_t
iRBt	The product of R^-1 and B^t
BiRBt	The proecut B*R^-1*B^t
ds	The dynamic information.

Returns

Optimal time (the time at which y1 will be reached, according to the computed policy).

Definition at line 4091 of file dynamics.c.

References TDynamicSpace::A_LQR, CT_DELTA, CT_TAU, TDynamicSpace::d_LQR, TDynamicSpace::da, dG(), TDynamicSpace::dG_LQR, TDynamicSpace::dof, dr(), TDynamicSpace::dr_LQR, FALSE, TDynamicSpace::G_LQR, TDynamicSpace::Gd, GeneralDotProduct(), GetParameter(), TDynamicSpace::Gnew_LQR, TDynamicSpace::Gtemp_LQR, INF, TDynamicSpace::iR, TDynamicSpace::iRBt, TDynamicSpace::lsSize_LQR, LSSolveCond(), TDynamicSpace::mA_LQR, TDynamicSpace::mAt, PrintMatrix(), PrintVector(), TDynamicSpace::r_LQR, TDynamicSpace::rhsA_LQR, TDynamicSpace::rnew_LQR, TDynamicSpace::rtemp_LQR, TDynamicSpace::solA_LQR, SubMatrixFromTMatrix(), SumVectorScale(), TDynamicSpace::topt, and TRUE.

LQRComputePolicy_t()

double LQRComputePolicy_t	(	Tparameters *	pr,
		double *	y0,
		double *	y1,
		double	tmax,
		double *	mA,
		double *	mB,
		double *	vc,
		double *	vd,
		double *	iRBt,
		double *	BiRBt,
		TDynamicSpace *	ds
	)

Alternative version of LQRComputePolicy where G is computed as an integral in time (not in G itself).

Parameters

pr	The set of parameters.
y0	The initial parameter.
y1	The goal parameter.
tmax	Maximum horizon.
mA	A matrix of the linear system (linearization from nonlinear system).
mB	B matrix of the linear system (linearization from nonlinear system).
vc	c vector of the linear system (linearization from nonlinear system).
vd	vd=A^-1*vc. Computed before to avoid repetitions.
iRBt	The product of R^-1 and B^t
BiRBt	The proecut B*R^-1*B^t
ds	The dynamic information.

Returns

Optimal time (the time at which y1 will be reached, according to the computed policy).

Definition at line 4322 of file dynamics.c.

References TDynamicSpace::A_LQR, CT_DELTA, CT_TAU, TDynamicSpace::d0_LQR, TDynamicSpace::d_LQR, TDynamicSpace::da, TDynamicSpace::dG_LQR, dGt(), TDynamicSpace::dof, TDynamicSpace::eA_LQR, FALSE, TDynamicSpace::G_LQR, TDynamicSpace::Gd, GeneralDotProduct(), GetParameter(), INF, TDynamicSpace::iR, TDynamicSpace::iRBt, TDynamicSpace::lsSize_LQR, LSSolveCond(), TDynamicSpace::mA_LQR, TDynamicSpace::mAt, MatrixVectorProduct(), Norm(), PrintMatrix(), PrintVector(), TDynamicSpace::r_LQR, TDynamicSpace::rhsA_LQR, TDynamicSpace::solA_LQR, SubMatrixFromTMatrix(), SubtractVector(), SumVector(), SumVectorScale(), TDynamicSpace::topt, and TRUE.

LQRPolicy()

double LQRPolicy	(	Tparameters *	pr,
		double *	u,
		double	t,
		TDynamicSpace *	ds
	)

Determines the optimal action for a given time from the policy computed in LQRComputePolicy.

Parameters

pr	The set of parameters.
u	The input vector (output).
t	The time t where the optimal input will be evaluated: u(t).
ds	The dynamic information.

Returns

The ratio of actions non-saturated actions (0 = none is saturated 1 = all are saturated).

Definition at line 4548 of file dynamics.c.

References TDynamicSpace::a2j, TDynamicSpace::da, TDynamicSpace::eA_LQR, TDynamicSpace::effort, Error(), TDynamicSpace::Gd, TDynamicSpace::iRBt, TDynamicSpace::lsSize_LQR, TDynamicSpace::mAt, MatrixExponential(), MatrixVectorProduct(), TDynamicSpace::rtemp_LQR, and TDynamicSpace::topt.

Referenced by NewTemptativeSample().

ActionSpaceDimension()

unsigned int ActionSpaceDimension

(

TDynamicSpace *

ds

)

Returns the dimension of the action space.

The action space can be seen as a box in Rn space with n the value returned by this function. Any point in this box is a valid action. The limits of the box are the maximum forces and torques of the actuators.

Parameters

ds	The dynamic information.

Returns

The dimension of the action space.

Definition at line 4602 of file dynamics.c.

References TDynamicSpace::da.

Referenced by AtlasRRTSimulate(), InitAtlasRRT(), and Simulate().

GetActionFromID()

boolean GetActionFromID ( unsigned int  id, double *  u, TDynamicSpace *  ds  )

inline

Defines an action given an identifier. This mapping can be fixed (the same action is returned given an identifer) or varible (a random action is returned each time the function is used with the same indentifier).

The identifier is from 0 to N_DYNAMIC_ACTIONS-1.

The size of the action vector is given by ActionSpaceDimension

Parameters

id	The action identifier.
u	The action vector to generate. The space must be allocated and deallocated by the caller.
ds	The dynamic information.

Returns

TRUE if the action can actually be defined.

Definition at line 4607 of file dynamics.c.

References TDynamicSpace::a2j, TDynamicSpace::da, TDynamicSpace::effort, FALSE, randomDouble(), and TRUE.

Referenced by AddBranchToAtlasDynamicRRT(), ApproachState(), and kinoEST().

PrintDynamicsDefines()

void PrintDynamicsDefines

(

FILE *

f

)

Prints the defines in chart.h

This is used only for debug purposes.

Parameters

f	The file where to store the values.

Definition at line 4664 of file dynamics.c.

References CUT_AT_LINK, DEBUG_DYNAMICS, DEBUG_HandC, DEBUG_HandC_FJH, DYNAMICS_ONE_ACTION_CONNECT, DYNAMICS_ONE_ACTION_EXTEND, EULER_LQR_COMPUTE_POLICY, EXPLICIT_CRF, EXPLICIT_CRM, HandC_EQ_INV, HandC_FJH, HandC_Thread, MAX_J, PRINT_J, RANDOM_DYNAMIC_ACTION, SATURATE_ACTIONS, START_BROYDEN_ON_MANIFOLD, T2G_METRIC, T2G_METRIC_INSIDE_CONNECT, T2G_METRIC_INSIDE_EXTEND, T2G_METRIC_NN_CONNECT, T2G_METRIC_NN_EXTEND, USE_HTxSAV, and USE_XUPTR.

Referenced by PrintAtlasRRTDefines().

DeleteDynamicSpace()

void DeleteDynamicSpace

(

TDynamicSpace *

ds

)

Destructor. Frees the memory included in a dynamic space strucure.

Parameters

ds	The dynamic space to delete.

Definition at line 4693 of file dynamics.c.

References TDynamicSpace::A, TDynamicSpace::a2j, TDynamicSpace::a2x, TDynamicSpace::A_ID, TDynamicSpace::A_LQR, TDynamicSpace::action, TDynamicSpace::B, TDynamicSpace::bJ, TDynamicSpace::bJp, TDynamicSpace::cJ, TDynamicSpace::d0_LQR, TDynamicSpace::d_LQR, DeleteBox(), DeleteBroyden(), DeleteHandC(), DeleteHessian(), DeleteJacobian(), DeleteLS(), DeleteNHessian(), DeleteNJacobian(), TDynamicSpace::dG_LQR, TDynamicSpace::dGtemp_LQR, TDynamicSpace::domain, TDynamicSpace::dr_LQR, TDynamicSpace::eA_LQR, TDynamicSpace::effort, TDynamicSpace::fE_LQR, FreeHessianEvaluationAsVectors(), TDynamicSpace::g, TDynamicSpace::G_LQR, TDynamicSpace::Gd, TDynamicSpace::gn, TDynamicSpace::Gnew_LQR, TDynamicSpace::Gtemp_LQR, TDynamicSpace::gy, TDynamicSpace::Hp, TDynamicSpace::Hpv, TDynamicSpace::iR, TDynamicSpace::iRBt, TDynamicSpace::J, TDynamicSpace::j, TDynamicSpace::Jot, TDynamicSpace::Jp, TDynamicSpace::k1, TDynamicSpace::k2, TDynamicSpace::k3, TDynamicSpace::l, TDynamicSpace::mAt, TDynamicSpace::Mm, TDynamicSpace::ndxOE, TDynamicSpace::ndxOV, TDynamicSpace::ndxPE, TDynamicSpace::ndxPV, TDynamicSpace::ndxVE, TDynamicSpace::ndxVV, TDynamicSpace::nHp, TDynamicSpace::nJp, TDynamicSpace::noe, TDynamicSpace::nov, TDynamicSpace::npe, TDynamicSpace::npv, TDynamicSpace::o, TDynamicSpace::oEqs, TDynamicSpace::P, TDynamicSpace::pEqs, TDynamicSpace::pos, TDynamicSpace::pVars, TDynamicSpace::pvEqs, TDynamicSpace::pvVars, TDynamicSpace::r_LQR, TDynamicSpace::rnew_LQR, TDynamicSpace::rtemp_LQR, TDynamicSpace::selEqs, TDynamicSpace::sHp, TDynamicSpace::sJoo, TDynamicSpace::sJop, TDynamicSpace::sJp, TDynamicSpace::tp, TDynamicSpace::vel, TDynamicSpace::x0_LQR, TDynamicSpace::x_LQR, TDynamicSpace::xa, TDynamicSpace::y, and TDynamicSpace::y_LQR.

Referenced by DeleteAtlasRRT(), kinobiRRT(), kinoEST(), kinoRRT(), and main().