Introduction

Functions related with dynamics. They are used to implement the planning with dynamical constraints.

Definition in file dynamics.h.

Data Structures
struct	TDynamicSpace
	Variables used in the dynamic simulation. More...

Macros
#define	DEBUG_DYNAMICS (_DEBUG>3?_DEBUG-3:0)
	Debug the dynamic procedures. More...

#define	PRINT_J 0
	Print information on the optimal LQR policy computation. More...

#define	MAX_J 0
	Define a max J. More...

#define	DEBUG_HandC (_DEBUG>4?1:0)
	Debug the HandC procedure. More...

#define	HandC_Thread 1
	Thread to debug. More...

#define	HandC_FJH 1
	Compute the position Jacobian/Hessian in HandC. More...

#define	DEBUG_HandC_FJH 0
	Debug the computation of the Jacobian/Hessian via HandC. More...

#define	HandC_EQ_INV 1
	Determines the loop equations defined in HandC. More...

#define	START_BROYDEN_ON_MANIFOLD 0
	Where to start the Broyden iteration. More...

#define	USE_HTxSAV 1
	Use the product of homogenous transforms and spatial algebra vectors. More...

#define	EXPLICIT_CRM 1
	Explicitly build the CRM matrix. More...

#define	EXPLICIT_CRF 1
	Explicitly build the CRF matrix. More...

#define	USE_XUPTR 0
	Define Xup using an homogeneous transform. More...

#define	CUT_AT_LINK 1
	Cut at links. More...

#define	EULER_LQR_COMPUTE_POLICY 0
	Integration method in LQRComputePolicy. More...

#define	LQR_COMPUTE_POLICY LQRComputePolicy_t
	Select the pocedure to compute the policy. More...

#define	SATURATE_ACTIONS 1
	LQR action saturation. More...

#define	RANDOM_DYNAMIC_ACTION 1
	Select actions randomly. More...

#define	T2G_METRIC_NN_EXTEND 0
	Use time to go metric. More...

#define	T2G_METRIC_NN_CONNECT 0
	Use time to go metric. More...

#define	T2G_METRIC_INSIDE_EXTEND ((T2G_METRIC_NN_EXTEND)&&(0))
	Use time to go metric. More...

#define	T2G_METRIC_INSIDE_CONNECT ((T2G_METRIC_NN_CONNECT)&&(0))
	Use time to go metric. More...

#define	T2G_METRIC ((T2G_METRIC_NN_EXTEND)\|\|(T2G_METRIC_NN_CONNECT))
	Use time to go metric. More...

#define	DYNAMICS_ONE_ACTION_EXTEND 0
	One random action. More...

#define	DYNAMICS_ONE_ACTION_CONNECT 0
	One random action. More...

Typedefs
typedef unsigned int	Tintegrator(Tparameters , unsigned int, double , double, double , double , double , TDynamicSpace )
	Integration function. More...

Functions
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 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)
	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	MechEnergy (Tparameters pr, double x, double pe, double ke, TDynamicSpace *ds)
	Computed the potential and kinetic energy of a system. 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...

void	SimulateRK4 (Tparameters pr, unsigned int da, double action, unsigned int steps, unsigned int ns, double path, double actions, double times, TDynamicSpace ds)
	Dynamic simulation with a given action using the RK4 method. 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...

Macro Definition Documentation

◆ DEBUG_DYNAMICS

#define DEBUG_DYNAMICS (_DEBUG>3?_DEBUG-3:0)

Set to 1 to print information about the dynamic procedures.

Definition at line 32 of file dynamics.h.

◆ PRINT_J

#define PRINT_J 0

If set we print information on the optimal LQR policy cost as computed for each time. This is an extra debug flag.

Definition at line 41 of file dynamics.h.

◆ MAX_J

#define MAX_J 0

If set we define a maximum J in LQRComputePolicy (or LQRComputePolicy_t): The optimal cost computed in this function is only accepted if it is below maxJ.

This is experimental and for normal operation should be 0.

Definition at line 52 of file dynamics.h.

◆ DEBUG_HandC

#define DEBUG_HandC (_DEBUG>4?1:0)

Set to 1 to print information about the HandC execution. We print detailed information to facilitate the comparison with the Matlab code.

Definition at line 60 of file dynamics.h.

◆ HandC_Thread

#define HandC_Thread 1

When debugging HandC we only print information about one of the threads running in parallel

Definition at line 68 of file dynamics.h.

◆ HandC_FJH

#define HandC_FJH 1

If set, the position Function/Jacobian/Hessian used to find out the acceleration is computed in HandC, re-using other computations. Otherwise it is computed from the system Jacobian. Note that we do not compute exactly the same Jacobian matrix but matrices that expand the same linear sub-space.

Typically the HandC computations are significantly faster.

Definition at line 81 of file dynamics.h.

◆ DEBUG_HandC_FJH

#define DEBUG_HandC_FJH 0

If set we print the Jacobian/Hessian computed in HandC and the Jacobian evaluated from the equations. Note that, if set, the HandC Jacobian is computed but not used (the one from the equations is stored in the corresponding matrices overwritting the HandC computations).

NOTE: To get exactly the same jacobian in both cases you should deactivate the SIMPLIFY_TS flag in trans_seq.h

Used only for debug.

Definition at line 97 of file dynamics.h.

◆ HandC_EQ_INV

#define HandC_EQ_INV 1

If set, the equation generated in HandC for each loop is A*B^-1, where A and B are alternative transforms to the loop closure link (transfroms computed from alternative paths from the root to the link).

If not set, the loop equations are computed as A-B.

Used only for experimentation. Typically this should be 1.

Definition at line 111 of file dynamics.h.

◆ START_BROYDEN_ON_MANIFOLD

#define START_BROYDEN_ON_MANIFOLD 0

If set the Broyden iteration is started froma point in the manifold: we use Euler in tangent space and then we project to the manifold. Otherwise we directly use Euler in ambient space and the initial point is not on the manifold.

Used only for experimentation. Typicaly this should be 0.

Definition at line 123 of file dynamics.h.

◆ USE_HTxSAV

#define USE_HTxSAV 1

If set, the product of homogenous transforms and spatial algebra vectors is performed without explicitly defining the spatial algebra matrix corresponding to the transform.

Definition at line 132 of file dynamics.h.

◆ EXPLICIT_CRM

#define EXPLICIT_CRM 1

If set, we explicitly build the CRM 6x6 matrix. Otherwise, we direclty use the CRMxSAV method (which is supposed to be faster).

Definition at line 140 of file dynamics.h.

◆ EXPLICIT_CRF

#define EXPLICIT_CRF 1

If set, we explicitly build the CRF 6x6 matrix. Otherwise, we direclty use the CRFxSAM method (which is supposed to be faster, but it is not).

Definition at line 149 of file dynamics.h.

◆ USE_XUPTR

#define USE_XUPTR 0

If set, Xup is also defined as an homogeneous transform. In this way, we can choose whether to use the homogenous transform or the spatial algebre matrix, depending on the case.

Definition at line 158 of file dynamics.h.

◆ CUT_AT_LINK

#define CUT_AT_LINK 1

If set, the kinematic tree used in HandC cuts the loops in the middle of some links and not in a joint. In this way all joints apperar in the kinematic tree.

Definition at line 167 of file dynamics.h.

◆ EULER_LQR_COMPUTE_POLICY

#define EULER_LQR_COMPUTE_POLICY 0

We can select the integration methods to be used in LQRComputePolicy. We can chose Euler (1), fast but innacurate in the long term or Runge-Kutta 4 (0).

Definition at line 176 of file dynamics.h.

◆ LQR_COMPUTE_POLICY

#define LQR_COMPUTE_POLICY LQRComputePolicy_t

Use the time-based (closed-form expressions) procedure.

Definition at line 184 of file dynamics.h.

◆ SATURATE_ACTIONS

#define SATURATE_ACTIONS 1

If set, actions computed with an LQR policy are saturated to the maximum value set by the user (in the world file). If set to 0, actions are not saturated.

This should be 1 and only set to 0 for debugging purposes.

If set to zero, we assume that we are performing an LQR steering test and we deactivate many options.

Definition at line 207 of file dynamics.h.

◆ RANDOM_DYNAMIC_ACTION

#define RANDOM_DYNAMIC_ACTION 1

If set, the actions are selected randomly, otherwise bang-bang control is used.

Definition at line 214 of file dynamics.h.

◆ T2G_METRIC_NN_EXTEND

#define T2G_METRIC_NN_EXTEND 0

When defining an RRT in a problem with dynamics, we can use time to go metric instead than the Euclidean metric.

Only a first order approximation of the time to go is computed (see Time2Go) and the computation is slow, but it is supposed to give more effective RRT extensions.

This is only active in dynamical systems and when NN_IN_CHART is active.

In particular this flag determines if Time2Go metric is to be used when looking for nearest neighbours when extending the RRT.

Definition at line 232 of file dynamics.h.

◆ T2G_METRIC_NN_CONNECT

#define T2G_METRIC_NN_CONNECT 0

The same as T2G_METRIC_NN_EXTEND but this determines if Time2Go metric is to be used when looking for nearest neighbours when trying to connect trees.

Definition at line 241 of file dynamics.h.

◆ T2G_METRIC_INSIDE_EXTEND

#define T2G_METRIC_INSIDE_EXTEND ((T2G_METRIC_NN_EXTEND)&&(0))

If T2G_METRIC_NN_EXTEND is active, we can additionally use the Time2Go metric inside the extension (to determine reachability sets and alike).

Definition at line 250 of file dynamics.h.

◆ T2G_METRIC_INSIDE_CONNECT

#define T2G_METRIC_INSIDE_CONNECT ((T2G_METRIC_NN_CONNECT)&&(0))

If T2G_METRIC_NN_CONNECT is active, we can additionally use the Time2Go metric inside the connection (to determine reachability sets and alike).

Definition at line 259 of file dynamics.h.

◆ T2G_METRIC

#define T2G_METRIC ((T2G_METRIC_NN_EXTEND)||(T2G_METRIC_NN_CONNECT))

TRUE if any of the time2go flatgs is active. Defined for convenience.

Definition at line 267 of file dynamics.h.

◆ DYNAMICS_ONE_ACTION_EXTEND

#define DYNAMICS_ONE_ACTION_EXTEND 0

In some modes (specially when using T2G_METRIC) it makes sense to execute just one action (generated at random or computed as you which) instead of simulating several candidate actions and selecting the best one.

In paticular this flag is used for the extension of the RRT (when aiming at a random sample).

This is typically active only when T2G_METRIC_NN_EXTEND is also active.

Definition at line 284 of file dynamics.h.

◆ DYNAMICS_ONE_ACTION_CONNECT

#define DYNAMICS_ONE_ACTION_CONNECT 0

The same as DYNAMICS_ONE_ACTION_EXTEND but used when trying to connect the two trees.

This is typically active only when T2G_METRIC_NN_CONNECT is also active.

Definition at line 295 of file dynamics.h.

Typedef Documentation

◆ Tintegrator

typedef unsigned int Tintegrator(Tparameters *, unsigned int, double *, double, double *, double *, double *, TDynamicSpace *)

Numerical integration function. Currently we have implemented the Euler and the RK4 methods.

See NextDynamicStateEuler or NextDynamicStateRK4 for an explanation of the function parameters.

Definition at line 597 of file dynamics.h.

Function Documentation

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

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

$r=-\partial{d}/\partial{t} = - (\partial{d}/\partial{x})^t \partial{x}/\partial{t}$

If 'd' is the Euclidean distance

$\partial{d}/\partial{x} = - v/d$

with $ v=tg-t $ . Moreover

$\partial{x}/\partial{t} = \dot{x}$

Thus,

$r=(v/d)^t \dot{x}$

and

$t2g=d^2/(v^t \dot{x}) = (v^t v) / (v^t \dot{x})$

In this context $\dot{x}=(\dot{q},\ddot{q})$ . We need to estimate $\ddot{q}$ . 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 $\ddot{q}$ 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 $\dot{q}$ 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().

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

◆ 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 2dof x 2dof).
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 BR^-1B^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.

◆ 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 BR^-1B^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().

◆ SimulateRK4()

void SimulateRK4	(	Tparameters *	pr,
		unsigned int	da,
		double *	action,
		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 RK4 method

Parameters

pr	The set of parameters.
da	Dimension of the action space. Number of elements in u and in each vector in actions.
action	The action to apply.
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.

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

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

Institut de Robòtica i Informàtica Industrial

Introduction

Data Structures

Macros

Typedefs

Functions

Macro Definition Documentation

◆ DEBUG_DYNAMICS

◆ PRINT_J

◆ MAX_J

◆ DEBUG_HandC

◆ HandC_Thread

◆ HandC_FJH

◆ DEBUG_HandC_FJH

◆ HandC_EQ_INV

◆ START_BROYDEN_ON_MANIFOLD

◆ USE_HTxSAV

◆ EXPLICIT_CRM

◆ EXPLICIT_CRF

◆ USE_XUPTR

◆ CUT_AT_LINK

◆ EULER_LQR_COMPUTE_POLICY

◆ LQR_COMPUTE_POLICY

◆ SATURATE_ACTIONS

◆ RANDOM_DYNAMIC_ACTION

◆ T2G_METRIC_NN_EXTEND

◆ T2G_METRIC_NN_CONNECT

◆ T2G_METRIC_INSIDE_EXTEND

◆ T2G_METRIC_INSIDE_CONNECT

◆ T2G_METRIC

◆ DYNAMICS_ONE_ACTION_EXTEND

◆ DYNAMICS_ONE_ACTION_CONNECT

Typedef Documentation

◆ Tintegrator

Function Documentation

◆ InitDynamicSpace()

◆ Getxdot()

◆ GetInverseActionCostMatrix()

◆ Time2Go()

◆ ApproachState()

◆ kinoRRT()

◆ kinobiRRT()

◆ kinoEST()

◆ MechEnergy()

◆ ComputeInverseDynamics()

◆ NextDynamicState()

◆ NextDynamicStateLocalEuler()

◆ NextDynamicStateLocalRK4()

◆ NextDynamicStateEuler()

◆ NextDynamicStateRK4()

◆ NumericIntegration()

◆ Simulate()

◆ LinearizeDynamics()

◆ LQRComputePolicy()

◆ LQRComputePolicy_t()

◆ LQRPolicy()

◆ SimulateRK4()

◆ ActionSpaceDimension()

◆ GetActionFromID()

◆ PrintDynamicsDefines()

◆ DeleteDynamicSpace()

Institut de Robòtica
i Informàtica Industrial