/************************************************************************/
/* SDFAST constants for "in air" model sra (Sarcos Roll Air) */

/* More readable sizes */
#define AIR_N_Q 7
#define AIR_N_U 7
#define AIR_N_STATES (AIR_N_Q+AIR_N_U)
#define AIR_N_CONTROLS 4

/* Indices for sdfast state */
#define AIR_X 0
#define AIR_Z 1
#define AIR_ROLL 2
#define AIR_L_HIP 3
#define AIR_L_KNEE 4
#define AIR_R_HIP 5
#define AIR_R_KNEE 6

#define AIR_XD 7
#define AIR_ZD 8
#define AIR_ROLLD 9
#define AIR_L_HIPD 10
#define AIR_L_KNEED 11
#define AIR_R_HIPD 12
#define AIR_R_KNEED 13

// Body index definitions (for sdpointf, etc.)
#define AIR_WORLD_BODY -1
#define AIR_TORSO_BODY 0
#define AIR_L_THIGH_BODY 1
#define AIR_L_CALF_BODY 2
#define AIR_R_THIGH_BODY 3
#define AIR_R_CALF_BODY 4

// Joint index definitions (for sdhinget, etc.)
#define AIR_ROOT_JOINT 0
#define AIR_L_HIP_JOINT 1
#define AIR_L_KNEE_JOINT 2
#define AIR_R_HIP_JOINT 3
#define AIR_R_KNEE_JOINT 4

/************************************************************************/
/* Single support SDFAST model srss */

/* Sizes */
#define SS_N_Q 5
#define SS_N_U 5
#define SS_N_STATES (SS_N_Q+SS_N_U)
#define SS_N_CONTROLS 5
#define SS_N_JOINTS (SS_N_Q+10) /* overestimate for safety */

/* Parameters */
#define SS_CTOL	1e-5	/* constraint tolerance */

/* Indices for sdfast state */
#define SS_ANKLE1 0
#define SS_KNEE1 1
#define SS_HIP1 2
#define SS_HIP2 3
#define SS_KNEE2 4
#define SS_ANKLE1D 5
#define SS_KNEE1D 6
#define SS_HIP1D 7
#define SS_HIP2D 8
#define SS_KNEE2D 9

// Body index definitions (for sdpointf, etc.)
#define SS_GND_BODY -1
#define SS_CALF1_BODY 0
#define SS_THIGH1_BODY 1
#define SS_TORSO_BODY 2
#define SS_THIGH2_BODY 3
#define SS_CALF2_BODY 4

/* Joints */
#define SS_ANKLE1_JOINT 0
#define SS_KNEE1_JOINT 1
#define SS_HIP1_JOINT 2
#define SS_HIP2_JOINT 3
#define SS_KNEE2_JOINT 4

/************************************************************************/
/* Double support SDFAST model srds */

/* Sizes */
#define DS_N_Q 5
#define DS_N_U 5
#define DS_N_STATES (DS_N_Q+DS_N_U)
#define DS_N_CONTROLS 6
#define DS_N_JOINTS (DS_N_Q+10) /* overestimate for safety */

/* Parameters */
#define DS_CTOL	1e-5	/* constraint tolerance */

/* Indices for sdfast state */
#define DS_ANKLE1 0
#define DS_KNEE1 1
#define DS_HIP1 2
#define DS_HIP2 3
#define DS_KNEE2 4
#define DS_ANKLE1D 5
#define DS_KNEE1D 6
#define DS_HIP1D 7
#define DS_HIP2D 8
#define DS_KNEE2D 9

// Body index definitions (for sdpointf, etc.)
#define DS_GND_BODY -1
#define DS_CALF1_BODY 0
#define DS_THIGH1_BODY 1
#define DS_TORSO_BODY 2
#define DS_THIGH2_BODY 3
#define DS_CALF2_BODY 4
#define DS_LOOP_BODY 5

/* Joints */
#define DS_ANKLE1_JOINT 0
#define DS_KNEE1_JOINT 1
#define DS_HIP1_JOINT 2
#define DS_HIP2_JOINT 3
#define DS_KNEE2_JOINT 4
#define DS_ANKLE2_JOINT 5

/************************************************************************/
