MTSOS: writing dynamics functions

In order to compile MTSOS the user must provide a function for calculating the dynamics given a point in configuration space s_i . Although it is possible to write the dynamics in MATLAB, doing so will incur a significant performance penalty, so users are encouraged to write their dynamics functions directly in C.

For details about the form the dynamics take, users should see Minimum-Time Speed Optimization Over a Fixed Path.

MATLAB

If the user must write the function in MATLAB, change the compiler to point to dynamics_matlab.c instead of dynamics_front.c. This will cause the algorithm to look to dynamics_mat.m for the dynamics. The name of this file can be changed in dynamics_matlab.c.

[R M C d] = dynamics_mat(S,S_prime,variables)

Inputs

`S`	a matrix consisting of the points in configuration space where the dynamics must be evaluated
`S_prime`	a matrix of the derivatives of with respect to , evaulated at the same points as `S`; although not a matrix of 's, treat it this way for the purposes of evaluation of `C`
`variables`	a vector provided by the user at the call to `MTSOS` in the optional parameter `variables`

Outputs

`R`	a length vector consisting of the , , matrices evaluated at the configurations in `S` in column first order and concatenated
`M`	a length vector consisting of the , , matrices evaluated at the configurations in `S` in column first order and concatenated
`C`	a length vector consisting of the , , matrices evaluated at the configurations in `S` in column first order and concatenated (recall that $C(q,dot{q}) = C_1(q)dot{q_1} + C_2(q)dot{q_2} + ldots + C_p(q)dot{q_p}$ , where $C_i: mbox{bf R}^p to mbox{bf R}^{p times p}$ for and $dot{q_i}$ is the ith element of the vector ) C should be evaluated using `S` as and `S_prime` in place of
`d`	a length vector consisting of the , matrices evaluated at the configurations in `S` and concatenated

C

int so_dynamics(const double* S_middle, const double* S_prime, int S_length, int State_size,: int U_size, const double* variables, int variables_length, double* R_dynamics,
double* M_dynamics, double* C_dynamics, double* d_dynamics)

Inputs

Note: these are all constant.

`double* S_middle`	a length array consisting of the points in configuration space where the dynamics must be evaluated, concatenated
`double* S_prime`	a length array consisting of the derivatives of with respect to , evaulated at the same points as `S_middle`, concatenated; although not an array of 's, it should be treated this way for the purposes of evaluation of `C`
`int S_length`	, the number of points in `S`
`int State_size`	, the size of the configuration space
`int U_size`	, the number of control inputs
`double* variables`	an array provided by the user at the call to `MTSOS` in the optional parameter `variables`
`int variables_length`	the length of the array `variables`

Outputs

Allocated, but unitialized, arrays are provided for the user to populate. In particular, if an array needs to be zero, the values must be set to zero, do not assume any values in the arrays.

`RETURN int`	1: no error occurred 0 : an error occurred, and the dynamics cannot be evaluated
`double* R_dynamics`	a length array consisting of the , , matrices evaluated at the configurations `S_middle` in column first order and concatenated
`double* M_dynamics`	a length array consisting of the , , matrices evaluated at the configurations in `S_middle` in column first order and concatenated
`double* C_dynamics`	a length array consisting of the , , matrices evaluated at the configurations in `S` in column first order and concatenated (recall that $C(q,dot{q}) = C_1(q)dot{q_1} + C_2(q)dot{q_2} + ldots + C_p(q)dot{q_p}$ , where $C_i: mbox{bf R}^p to mbox{bf R}^{p times p}$ for and $dot{q_i}$ is the ith element of the vector ) C should be evaluated using `S_middle` as and `S_prime` in place of
`double* d_dynamics`	a length array consisting of the , matrices evaluated at the configurations in `S_middle` and concatenated