/************************************************************************/ /* dynamics for a torso and thigh flying through the air. */ /************************************************************************/ #include #include #include #include "dynamics-leg.h" /************************************************************************/ /**********************************************************************/ /* Defines */ /* Parameters */ #define TORSO_LENGTH (1.0) // length of link #define TORSO_MASS (1.0) // mass of link #define THIGH_LENGTH (0.5) // length of link #define THIGH_MASS (0.5) // mass of link #define CALF_LENGTH (0.5) // length of link #define CALF_MASS (0.5) // mass of link #define TORSOTOHIP -0.5 #define HIPTOKNEE (-THIGH_LENGTH) #define KNEETOANKLE (-CALF_LENGTH) #define HIPTOTHIGHCOM -0.25 #define KNEETOCALFCOM -0.25 // #define GRAVITY 9.81 #define GRAVITY 0.0 /* Handy macros */ #define SQ(x) ((x)*(x)) // Timestep of integrator #define DEFAULT_TIMESTEP 0.0001 /**********************************************************************/ /* Globals */ double default_initial_state[N_STATE_DIMENSIONS] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; /**********************************************************************/ /* Don't need to do any initialization */ int init_dynamics() { return 1; } /**********************************************************************/ Dynamics *create_dynamics() { Dynamics *result; int i; result = (Dynamics *) malloc( sizeof( Dynamics ) ); if ( result == NULL ) { fprintf( stderr, "Can't allocate dynamics.\n" ); exit( -1 ); } set_state( result, 0.0, default_initial_state ); result->timestep = DEFAULT_TIMESTEP; result->seed = 1; /* Set random number generator seed */ return result; } /**********************************************************************/ int set_state( Dynamics *d, double time, double *state ) { int i; d->time = time; for ( i = 0; i < N_STATE_DIMENSIONS; i++ ) d->state[i] = state[i]; return 1; } /**********************************************************************/ void dynamics( double *state, double *action, double *result ) { /* Slightly faster to have these as variables than defines. Go figure */ double Mtorso = TORSO_MASS; double Mthigh = THIGH_MASS; double Mcalf = CALF_MASS; double TorsotoHip = TORSOTOHIP; double HiptoKnee = HIPTOKNEE; double HiptoThighCOM = HIPTOTHIGHCOM; double KneetoCalfCOM = KNEETOCALFCOM; double KneetoAnkle = KNEETOANKLE; double Itorso = (TORSO_MASS*SQ(TORSO_LENGTH)/12); /* Icom */ double Ithigh = (THIGH_MASS*SQ(THIGH_LENGTH)/12); /* Icom */ double Icalf = (CALF_MASS*SQ(CALF_LENGTH)/12); /* Icom */ double x, xd, xdd; double y, yd, ydd; double a0, a0d, a0dd; double hipL, hipLd, hipLdd; double kneeL, kneeLd, kneeLdd; double tauHipL; double tauKneeL; double a00, a01, a02, a03, a04; double a10, a11, a12, a13, a14; double a20, a21, a22, a23, a24; double a30, a31, a32, a33, a34; double a40, a41, a42, a43, a44; double b0, b1, b2, b3, b4; double determinant; double c0, s0, cHl, sHl, cKl, sKl, c0Hl, s0Hl; double cHlKl, sHlKl, c0HlKl, s0HlKl; double G = GRAVITY; double FGroundXL = 0; double FGroundYL = 0; c0 = cos( a0 ); s0 = sin( a0 ); cHl = cos( hipL ); sHl = sin( hipL ); cKl = cos( kneeL ); sKl = sin( kneeL ); c0Hl = cos(a0 + hipL); s0Hl = sin(a0 + hipL); c0HlKl = cos(a0 + hipL + kneeL); s0HlKl = sin(a0 + hipL + kneeL); cHlKl = cos(hipL + kneeL); sHlKl = sin(hipL + kneeL); x = state[S_X]; y = state[S_Y]; a0 = state[S_A0]; hipL = state[S_HIPL]; kneeL = state[S_KNEEL]; xd = state[S_XD]; yd = state[S_YD]; a0d = state[S_A0D]; hipLd = state[S_HIPLD]; kneeLd = state[S_KNEELD]; tauHipL = action[A_HIPL]; tauKneeL = action[A_KNEEL]; /* Solve A*acceleration = B */ /* First row */ a00 = Mcalf + Mthigh + Mtorso; a01 = 0; a02 = -((Mcalf + Mthigh)*TorsotoHip*c0) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*c0Hl - KneetoCalfCOM*Mcalf*c0HlKl; a03 = -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*c0Hl) - KneetoCalfCOM*Mcalf*c0HlKl; a04 = -(KneetoCalfCOM*Mcalf*c0HlKl); /* fx is zero */ b0 = -( FGroundXL + SQ(a0d)*(Mcalf + Mthigh)*TorsotoHip*s0 + SQ(a0d + hipLd)*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)* s0Hl + SQ(a0d)*KneetoCalfCOM*Mcalf* s0HlKl + 2*a0d*hipLd*KneetoCalfCOM*Mcalf* s0HlKl + SQ(hipLd)*KneetoCalfCOM*Mcalf* s0HlKl + 2*a0d*kneeLd*KneetoCalfCOM*Mcalf* s0HlKl + 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf* s0HlKl + SQ(kneeLd)*KneetoCalfCOM*Mcalf* s0HlKl ); /* Second row */ a10 = 0; a11 = Mcalf + Mthigh + Mtorso; a12 = -((Mcalf + Mthigh)*TorsotoHip*s0) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*s0Hl - KneetoCalfCOM*Mcalf*s0HlKl; a13 = -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*s0Hl) - KneetoCalfCOM*Mcalf*s0HlKl; a14 = -(KneetoCalfCOM*Mcalf*s0HlKl); /* fy is zero */ b1 = -( FGroundYL + G*Mcalf + G*Mthigh + G*Mtorso - SQ(a0d)*(Mcalf + Mthigh)*TorsotoHip*c0 - SQ(a0d + hipLd)*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)* c0Hl - SQ(a0d)*KneetoCalfCOM*Mcalf* c0HlKl - 2*a0d*hipLd*KneetoCalfCOM*Mcalf* c0HlKl - SQ(hipLd)*KneetoCalfCOM*Mcalf* c0HlKl - 2*a0d*kneeLd*KneetoCalfCOM*Mcalf* c0HlKl - 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf* c0HlKl - SQ(kneeLd)*KneetoCalfCOM*Mcalf* c0HlKl ); /* Third row */ a20 = -((Mcalf + Mthigh)*TorsotoHip*c0) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*c0Hl - KneetoCalfCOM*Mcalf*c0HlKl; a21 = -((Mcalf + Mthigh)*TorsotoHip*s0) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*s0Hl - KneetoCalfCOM*Mcalf*s0HlKl; a22 = Icalf + Ithigh + Itorso + SQ(HiptoKnee)*Mcalf + SQ(KneetoCalfCOM)*Mcalf + SQ(HiptoThighCOM)*Mthigh + Mcalf*SQ(TorsotoHip) + Mthigh*SQ(TorsotoHip) + 2*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*cHl + 2*HiptoKnee*KneetoCalfCOM*Mcalf*cKl + 2*KneetoCalfCOM*Mcalf*TorsotoHip*cHlKl; a23 = Icalf + Ithigh + SQ(HiptoKnee)*Mcalf + SQ(KneetoCalfCOM)*Mcalf + SQ(HiptoThighCOM)*Mthigh + (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*cHl + 2*HiptoKnee*KneetoCalfCOM*Mcalf*cKl + KneetoCalfCOM*Mcalf*TorsotoHip*cHlKl; a24 = Icalf + SQ(KneetoCalfCOM)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*cKl + KneetoCalfCOM*Mcalf*TorsotoHip*cHlKl; /* tau0 is zero */ b2 = -( -(FGroundXL*TorsotoHip*c0) - FGroundXL*HiptoKnee*c0Hl - FGroundXL*KneetoAnkle*c0HlKl - FGroundYL*TorsotoHip*s0 - G*Mcalf*TorsotoHip*s0 - G*Mthigh*TorsotoHip*s0 - 2*a0d*hipLd*HiptoKnee*Mcalf*TorsotoHip*sHl - SQ(hipLd)*HiptoKnee*Mcalf*TorsotoHip*sHl - 2*a0d*hipLd*HiptoThighCOM*Mthigh*TorsotoHip*sHl - SQ(hipLd)*HiptoThighCOM*Mthigh*TorsotoHip*sHl - FGroundYL*HiptoKnee*s0Hl - G*HiptoKnee*Mcalf*s0Hl - G*HiptoThighCOM*Mthigh*s0Hl - 2*a0d*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*sKl - 2*hipLd*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*sKl - HiptoKnee*SQ(kneeLd)*KneetoCalfCOM*Mcalf*sKl - 2*a0d*hipLd*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - SQ(hipLd)*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - 2*a0d*kneeLd*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - SQ(kneeLd)*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - FGroundYL*KneetoAnkle*s0HlKl - G*KneetoCalfCOM*Mcalf*s0HlKl ); /* Fourth row */ a30 = -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*c0Hl) - KneetoCalfCOM*Mcalf*c0HlKl; a31 = -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*s0Hl) - KneetoCalfCOM*Mcalf*s0HlKl; a32 = Icalf + Ithigh + SQ(HiptoKnee)*Mcalf + SQ(KneetoCalfCOM)*Mcalf + SQ(HiptoThighCOM)*Mthigh + (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*cHl + 2*HiptoKnee*KneetoCalfCOM*Mcalf*cKl + KneetoCalfCOM*Mcalf*TorsotoHip*cHlKl; a33 = Icalf + Ithigh + SQ(HiptoKnee)*Mcalf + SQ(KneetoCalfCOM)*Mcalf + SQ(HiptoThighCOM)*Mthigh + 2*HiptoKnee*KneetoCalfCOM*Mcalf*cKl; a34 = Icalf + SQ(KneetoCalfCOM)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*cKl; b3 = tauHipL -( -(FGroundXL*HiptoKnee*c0Hl) - FGroundXL*KneetoAnkle*c0HlKl + SQ(a0d)*HiptoKnee*Mcalf*TorsotoHip*sHl + SQ(a0d)*HiptoThighCOM*Mthigh*TorsotoHip*sHl - FGroundYL*HiptoKnee*s0Hl - G*HiptoKnee*Mcalf*s0Hl - G*HiptoThighCOM*Mthigh*s0Hl - 2*a0d*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*sKl - 2*hipLd*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*sKl - HiptoKnee*SQ(kneeLd)*KneetoCalfCOM*Mcalf*sKl + SQ(a0d)*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - FGroundYL*KneetoAnkle*s0HlKl - G*KneetoCalfCOM*Mcalf*s0HlKl ); /* Fifth row */ a40 = -(KneetoCalfCOM*Mcalf*c0HlKl); a41 = -(KneetoCalfCOM*Mcalf*s0HlKl); a42 = Icalf + SQ(KneetoCalfCOM)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*cKl + KneetoCalfCOM*Mcalf*TorsotoHip*cHlKl; a43 = Icalf + SQ(KneetoCalfCOM)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*cKl; a44 = Icalf + SQ(KneetoCalfCOM)*Mcalf; b4 = tauKneeL -( -(FGroundXL*KneetoAnkle*c0HlKl) + SQ(a0d + hipLd)*HiptoKnee*KneetoCalfCOM*Mcalf*sKl + SQ(a0d)*KneetoCalfCOM*Mcalf*TorsotoHip*sHlKl - FGroundYL*KneetoAnkle*s0HlKl - G*KneetoCalfCOM*Mcalf*s0HlKl ); /****************************************************************************/ determinant = (a04*a13*a22*a31*a40 - a03*a14*a22*a31*a40 - a04*a12*a23*a31*a40 + a02*a14*a23*a31*a40 + a03*a12*a24*a31*a40 - a02*a13*a24*a31*a40 - a04*a13*a21*a32*a40 + a03*a14*a21*a32*a40 + a04*a11*a23*a32*a40 - a01*a14*a23*a32*a40 - a03*a11*a24*a32*a40 + a01*a13*a24*a32*a40 + a04*a12*a21*a33*a40 - a02*a14*a21*a33*a40 - a04*a11*a22*a33*a40 + a01*a14*a22*a33*a40 + a02*a11*a24*a33*a40 - a01*a12*a24*a33*a40 - a03*a12*a21*a34*a40 + a02*a13*a21*a34*a40 + a03*a11*a22*a34*a40 - a01*a13*a22*a34*a40 - a02*a11*a23*a34*a40 + a01*a12*a23*a34*a40 - a04*a13*a22*a30*a41 + a03*a14*a22*a30*a41 + a04*a12*a23*a30*a41 - a02*a14*a23*a30*a41 - a03*a12*a24*a30*a41 + a02*a13*a24*a30*a41 + a04*a13*a20*a32*a41 - a03*a14*a20*a32*a41 - a04*a10*a23*a32*a41 + a00*a14*a23*a32*a41 + a03*a10*a24*a32*a41 - a00*a13*a24*a32*a41 - a04*a12*a20*a33*a41 + a02*a14*a20*a33*a41 + a04*a10*a22*a33*a41 - a00*a14*a22*a33*a41 - a02*a10*a24*a33*a41 + a00*a12*a24*a33*a41 + a03*a12*a20*a34*a41 - a02*a13*a20*a34*a41 - a03*a10*a22*a34*a41 + a00*a13*a22*a34*a41 + a02*a10*a23*a34*a41 - a00*a12*a23*a34*a41 + a04*a13*a21*a30*a42 - a03*a14*a21*a30*a42 - a04*a11*a23*a30*a42 + a01*a14*a23*a30*a42 + a03*a11*a24*a30*a42 - a01*a13*a24*a30*a42 - a04*a13*a20*a31*a42 + a03*a14*a20*a31*a42 + a04*a10*a23*a31*a42 - a00*a14*a23*a31*a42 - a03*a10*a24*a31*a42 + a00*a13*a24*a31*a42 + a04*a11*a20*a33*a42 - a01*a14*a20*a33*a42 - a04*a10*a21*a33*a42 + a00*a14*a21*a33*a42 + a01*a10*a24*a33*a42 - a00*a11*a24*a33*a42 - a03*a11*a20*a34*a42 + a01*a13*a20*a34*a42 + a03*a10*a21*a34*a42 - a00*a13*a21*a34*a42 - a01*a10*a23*a34*a42 + a00*a11*a23*a34*a42 - a04*a12*a21*a30*a43 + a02*a14*a21*a30*a43 + a04*a11*a22*a30*a43 - a01*a14*a22*a30*a43 - a02*a11*a24*a30*a43 + a01*a12*a24*a30*a43 + a04*a12*a20*a31*a43 - a02*a14*a20*a31*a43 - a04*a10*a22*a31*a43 + a00*a14*a22*a31*a43 + a02*a10*a24*a31*a43 - a00*a12*a24*a31*a43 - a04*a11*a20*a32*a43 + a01*a14*a20*a32*a43 + a04*a10*a21*a32*a43 - a00*a14*a21*a32*a43 - a01*a10*a24*a32*a43 + a00*a11*a24*a32*a43 + a02*a11*a20*a34*a43 - a01*a12*a20*a34*a43 - a02*a10*a21*a34*a43 + a00*a12*a21*a34*a43 + a01*a10*a22*a34*a43 - a00*a11*a22*a34*a43 + a03*a12*a21*a30*a44 - a02*a13*a21*a30*a44 - a03*a11*a22*a30*a44 + a01*a13*a22*a30*a44 + a02*a11*a23*a30*a44 - a01*a12*a23*a30*a44 - a03*a12*a20*a31*a44 + a02*a13*a20*a31*a44 + a03*a10*a22*a31*a44 - a00*a13*a22*a31*a44 - a02*a10*a23*a31*a44 + a00*a12*a23*a31*a44 + a03*a11*a20*a32*a44 - a01*a13*a20*a32*a44 - a03*a10*a21*a32*a44 + a00*a13*a21*a32*a44 + a01*a10*a23*a32*a44 - a00*a11*a23*a32*a44 - a02*a11*a20*a33*a44 + a01*a12*a20*a33*a44 + a02*a10*a21*a33*a44 - a00*a12*a21*a33*a44 - a01*a10*a22*a33*a44 + a00*a11*a22*a33*a44); xdd = b0*( a14*a23*a32*a41 - a13*a24*a32*a41 - a14*a22*a33*a41 + a12*a24*a33*a41 + a13*a22*a34*a41 - a12*a23*a34*a41 - a14*a23*a31*a42 + a13*a24*a31*a42 + a14*a21*a33*a42 - a11*a24*a33*a42 - a13*a21*a34*a42 + a11*a23*a34*a42 + a14*a22*a31*a43 - a12*a24*a31*a43 - a14*a21*a32*a43 + a11*a24*a32*a43 + a12*a21*a34*a43 - a11*a22*a34*a43 - a13*a22*a31*a44 + a12*a23*a31*a44 + a13*a21*a32*a44 - a11*a23*a32*a44 - a12*a21*a33*a44 + a11*a22*a33*a44 ) + b1*( -(a04*a23*a32*a41) + a03*a24*a32*a41 + a04*a22*a33*a41 - a02*a24*a33*a41 - a03*a22*a34*a41 + a02*a23*a34*a41 + a04*a23*a31*a42 - a03*a24*a31*a42 - a04*a21*a33*a42 + a01*a24*a33*a42 + a03*a21*a34*a42 - a01*a23*a34*a42 - a04*a22*a31*a43 + a02*a24*a31*a43 + a04*a21*a32*a43 - a01*a24*a32*a43 - a02*a21*a34*a43 + a01*a22*a34*a43 + a03*a22*a31*a44 - a02*a23*a31*a44 - a03*a21*a32*a44 + a01*a23*a32*a44 + a02*a21*a33*a44 - a01*a22*a33*a44 ) + b2*( a04*a13*a32*a41 - a03*a14*a32*a41 - a04*a12*a33*a41 + a02*a14*a33*a41 + a03*a12*a34*a41 - a02*a13*a34*a41 - a04*a13*a31*a42 + a03*a14*a31*a42 + a04*a11*a33*a42 - a01*a14*a33*a42 - a03*a11*a34*a42 + a01*a13*a34*a42 + a04*a12*a31*a43 - a02*a14*a31*a43 - a04*a11*a32*a43 + a01*a14*a32*a43 + a02*a11*a34*a43 - a01*a12*a34*a43 - a03*a12*a31*a44 + a02*a13*a31*a44 + a03*a11*a32*a44 - a01*a13*a32*a44 - a02*a11*a33*a44 + a01*a12*a33*a44 ) + b3*( -(a04*a13*a22*a41) + a03*a14*a22*a41 + a04*a12*a23*a41 - a02*a14*a23*a41 - a03*a12*a24*a41 + a02*a13*a24*a41 + a04*a13*a21*a42 - a03*a14*a21*a42 - a04*a11*a23*a42 + a01*a14*a23*a42 + a03*a11*a24*a42 - a01*a13*a24*a42 - a04*a12*a21*a43 + a02*a14*a21*a43 + a04*a11*a22*a43 - a01*a14*a22*a43 - a02*a11*a24*a43 + a01*a12*a24*a43 + a03*a12*a21*a44 - a02*a13*a21*a44 - a03*a11*a22*a44 + a01*a13*a22*a44 + a02*a11*a23*a44 - a01*a12*a23*a44 ) + b4*( a04*a13*a22*a31 - a03*a14*a22*a31 - a04*a12*a23*a31 + a02*a14*a23*a31 + a03*a12*a24*a31 - a02*a13*a24*a31 - a04*a13*a21*a32 + a03*a14*a21*a32 + a04*a11*a23*a32 - a01*a14*a23*a32 - a03*a11*a24*a32 + a01*a13*a24*a32 + a04*a12*a21*a33 - a02*a14*a21*a33 - a04*a11*a22*a33 + a01*a14*a22*a33 + a02*a11*a24*a33 - a01*a12*a24*a33 - a03*a12*a21*a34 + a02*a13*a21*a34 + a03*a11*a22*a34 - a01*a13*a22*a34 - a02*a11*a23*a34 + a01*a12*a23*a34 ); ydd = b0*( -(a14*a23*a32*a40) + a13*a24*a32*a40 + a14*a22*a33*a40 - a12*a24*a33*a40 - a13*a22*a34*a40 + a12*a23*a34*a40 + a14*a23*a30*a42 - a13*a24*a30*a42 - a14*a20*a33*a42 + a10*a24*a33*a42 + a13*a20*a34*a42 - a10*a23*a34*a42 - a14*a22*a30*a43 + a12*a24*a30*a43 + a14*a20*a32*a43 - a10*a24*a32*a43 - a12*a20*a34*a43 + a10*a22*a34*a43 + a13*a22*a30*a44 - a12*a23*a30*a44 - a13*a20*a32*a44 + a10*a23*a32*a44 + a12*a20*a33*a44 - a10*a22*a33*a44 ) + b1*( a04*a23*a32*a40 - a03*a24*a32*a40 - a04*a22*a33*a40 + a02*a24*a33*a40 + a03*a22*a34*a40 - a02*a23*a34*a40 - a04*a23*a30*a42 + a03*a24*a30*a42 + a04*a20*a33*a42 - a00*a24*a33*a42 - a03*a20*a34*a42 + a00*a23*a34*a42 + a04*a22*a30*a43 - a02*a24*a30*a43 - a04*a20*a32*a43 + a00*a24*a32*a43 + a02*a20*a34*a43 - a00*a22*a34*a43 - a03*a22*a30*a44 + a02*a23*a30*a44 + a03*a20*a32*a44 - a00*a23*a32*a44 - a02*a20*a33*a44 + a00*a22*a33*a44 ) + b2*( -(a04*a13*a32*a40) + a03*a14*a32*a40 + a04*a12*a33*a40 - a02*a14*a33*a40 - a03*a12*a34*a40 + a02*a13*a34*a40 + a04*a13*a30*a42 - a03*a14*a30*a42 - a04*a10*a33*a42 + a00*a14*a33*a42 + a03*a10*a34*a42 - a00*a13*a34*a42 - a04*a12*a30*a43 + a02*a14*a30*a43 + a04*a10*a32*a43 - a00*a14*a32*a43 - a02*a10*a34*a43 + a00*a12*a34*a43 + a03*a12*a30*a44 - a02*a13*a30*a44 - a03*a10*a32*a44 + a00*a13*a32*a44 + a02*a10*a33*a44 - a00*a12*a33*a44 ) + b3*( a04*a13*a22*a40 - a03*a14*a22*a40 - a04*a12*a23*a40 + a02*a14*a23*a40 + a03*a12*a24*a40 - a02*a13*a24*a40 - a04*a13*a20*a42 + a03*a14*a20*a42 + a04*a10*a23*a42 - a00*a14*a23*a42 - a03*a10*a24*a42 + a00*a13*a24*a42 + a04*a12*a20*a43 - a02*a14*a20*a43 - a04*a10*a22*a43 + a00*a14*a22*a43 + a02*a10*a24*a43 - a00*a12*a24*a43 - a03*a12*a20*a44 + a02*a13*a20*a44 + a03*a10*a22*a44 - a00*a13*a22*a44 - a02*a10*a23*a44 + a00*a12*a23*a44 ) + b4*( -(a04*a13*a22*a30) + a03*a14*a22*a30 + a04*a12*a23*a30 - a02*a14*a23*a30 - a03*a12*a24*a30 + a02*a13*a24*a30 + a04*a13*a20*a32 - a03*a14*a20*a32 - a04*a10*a23*a32 + a00*a14*a23*a32 + a03*a10*a24*a32 - a00*a13*a24*a32 - a04*a12*a20*a33 + a02*a14*a20*a33 + a04*a10*a22*a33 - a00*a14*a22*a33 - a02*a10*a24*a33 + a00*a12*a24*a33 + a03*a12*a20*a34 - a02*a13*a20*a34 - a03*a10*a22*a34 + a00*a13*a22*a34 + a02*a10*a23*a34 - a00*a12*a23*a34 ); a0dd = b0*( a14*a23*a31*a40 - a13*a24*a31*a40 - a14*a21*a33*a40 + a11*a24*a33*a40 + a13*a21*a34*a40 - a11*a23*a34*a40 - a14*a23*a30*a41 + a13*a24*a30*a41 + a14*a20*a33*a41 - a10*a24*a33*a41 - a13*a20*a34*a41 + a10*a23*a34*a41 + a14*a21*a30*a43 - a11*a24*a30*a43 - a14*a20*a31*a43 + a10*a24*a31*a43 + a11*a20*a34*a43 - a10*a21*a34*a43 - a13*a21*a30*a44 + a11*a23*a30*a44 + a13*a20*a31*a44 - a10*a23*a31*a44 - a11*a20*a33*a44 + a10*a21*a33*a44 ) + b1*( -(a04*a23*a31*a40) + a03*a24*a31*a40 + a04*a21*a33*a40 - a01*a24*a33*a40 - a03*a21*a34*a40 + a01*a23*a34*a40 + a04*a23*a30*a41 - a03*a24*a30*a41 - a04*a20*a33*a41 + a00*a24*a33*a41 + a03*a20*a34*a41 - a00*a23*a34*a41 - a04*a21*a30*a43 + a01*a24*a30*a43 + a04*a20*a31*a43 - a00*a24*a31*a43 - a01*a20*a34*a43 + a00*a21*a34*a43 + a03*a21*a30*a44 - a01*a23*a30*a44 - a03*a20*a31*a44 + a00*a23*a31*a44 + a01*a20*a33*a44 - a00*a21*a33*a44 ) + b2*( a04*a13*a31*a40 - a03*a14*a31*a40 - a04*a11*a33*a40 + a01*a14*a33*a40 + a03*a11*a34*a40 - a01*a13*a34*a40 - a04*a13*a30*a41 + a03*a14*a30*a41 + a04*a10*a33*a41 - a00*a14*a33*a41 - a03*a10*a34*a41 + a00*a13*a34*a41 + a04*a11*a30*a43 - a01*a14*a30*a43 - a04*a10*a31*a43 + a00*a14*a31*a43 + a01*a10*a34*a43 - a00*a11*a34*a43 - a03*a11*a30*a44 + a01*a13*a30*a44 + a03*a10*a31*a44 - a00*a13*a31*a44 - a01*a10*a33*a44 + a00*a11*a33*a44 ) + b3*( -(a04*a13*a21*a40) + a03*a14*a21*a40 + a04*a11*a23*a40 - a01*a14*a23*a40 - a03*a11*a24*a40 + a01*a13*a24*a40 + a04*a13*a20*a41 - a03*a14*a20*a41 - a04*a10*a23*a41 + a00*a14*a23*a41 + a03*a10*a24*a41 - a00*a13*a24*a41 - a04*a11*a20*a43 + a01*a14*a20*a43 + a04*a10*a21*a43 - a00*a14*a21*a43 - a01*a10*a24*a43 + a00*a11*a24*a43 + a03*a11*a20*a44 - a01*a13*a20*a44 - a03*a10*a21*a44 + a00*a13*a21*a44 + a01*a10*a23*a44 - a00*a11*a23*a44 ) + b4*( a04*a13*a21*a30 - a03*a14*a21*a30 - a04*a11*a23*a30 + a01*a14*a23*a30 + a03*a11*a24*a30 - a01*a13*a24*a30 - a04*a13*a20*a31 + a03*a14*a20*a31 + a04*a10*a23*a31 - a00*a14*a23*a31 - a03*a10*a24*a31 + a00*a13*a24*a31 + a04*a11*a20*a33 - a01*a14*a20*a33 - a04*a10*a21*a33 + a00*a14*a21*a33 + a01*a10*a24*a33 - a00*a11*a24*a33 - a03*a11*a20*a34 + a01*a13*a20*a34 + a03*a10*a21*a34 - a00*a13*a21*a34 - a01*a10*a23*a34 + a00*a11*a23*a34 ); hipLdd = b0*( -(a14*a22*a31*a40) + a12*a24*a31*a40 + a14*a21*a32*a40 - a11*a24*a32*a40 - a12*a21*a34*a40 + a11*a22*a34*a40 + a14*a22*a30*a41 - a12*a24*a30*a41 - a14*a20*a32*a41 + a10*a24*a32*a41 + a12*a20*a34*a41 - a10*a22*a34*a41 - a14*a21*a30*a42 + a11*a24*a30*a42 + a14*a20*a31*a42 - a10*a24*a31*a42 - a11*a20*a34*a42 + a10*a21*a34*a42 + a12*a21*a30*a44 - a11*a22*a30*a44 - a12*a20*a31*a44 + a10*a22*a31*a44 + a11*a20*a32*a44 - a10*a21*a32*a44 ) + b1*( a04*a22*a31*a40 - a02*a24*a31*a40 - a04*a21*a32*a40 + a01*a24*a32*a40 + a02*a21*a34*a40 - a01*a22*a34*a40 - a04*a22*a30*a41 + a02*a24*a30*a41 + a04*a20*a32*a41 - a00*a24*a32*a41 - a02*a20*a34*a41 + a00*a22*a34*a41 + a04*a21*a30*a42 - a01*a24*a30*a42 - a04*a20*a31*a42 + a00*a24*a31*a42 + a01*a20*a34*a42 - a00*a21*a34*a42 - a02*a21*a30*a44 + a01*a22*a30*a44 + a02*a20*a31*a44 - a00*a22*a31*a44 - a01*a20*a32*a44 + a00*a21*a32*a44 ) + b2*( -(a04*a12*a31*a40) + a02*a14*a31*a40 + a04*a11*a32*a40 - a01*a14*a32*a40 - a02*a11*a34*a40 + a01*a12*a34*a40 + a04*a12*a30*a41 - a02*a14*a30*a41 - a04*a10*a32*a41 + a00*a14*a32*a41 + a02*a10*a34*a41 - a00*a12*a34*a41 - a04*a11*a30*a42 + a01*a14*a30*a42 + a04*a10*a31*a42 - a00*a14*a31*a42 - a01*a10*a34*a42 + a00*a11*a34*a42 + a02*a11*a30*a44 - a01*a12*a30*a44 - a02*a10*a31*a44 + a00*a12*a31*a44 + a01*a10*a32*a44 - a00*a11*a32*a44 ) + b3*( a04*a12*a21*a40 - a02*a14*a21*a40 - a04*a11*a22*a40 + a01*a14*a22*a40 + a02*a11*a24*a40 - a01*a12*a24*a40 - a04*a12*a20*a41 + a02*a14*a20*a41 + a04*a10*a22*a41 - a00*a14*a22*a41 - a02*a10*a24*a41 + a00*a12*a24*a41 + a04*a11*a20*a42 - a01*a14*a20*a42 - a04*a10*a21*a42 + a00*a14*a21*a42 + a01*a10*a24*a42 - a00*a11*a24*a42 - a02*a11*a20*a44 + a01*a12*a20*a44 + a02*a10*a21*a44 - a00*a12*a21*a44 - a01*a10*a22*a44 + a00*a11*a22*a44 ) + b4*( -(a04*a12*a21*a30) + a02*a14*a21*a30 + a04*a11*a22*a30 - a01*a14*a22*a30 - a02*a11*a24*a30 + a01*a12*a24*a30 + a04*a12*a20*a31 - a02*a14*a20*a31 - a04*a10*a22*a31 + a00*a14*a22*a31 + a02*a10*a24*a31 - a00*a12*a24*a31 - a04*a11*a20*a32 + a01*a14*a20*a32 + a04*a10*a21*a32 - a00*a14*a21*a32 - a01*a10*a24*a32 + a00*a11*a24*a32 + a02*a11*a20*a34 - a01*a12*a20*a34 - a02*a10*a21*a34 + a00*a12*a21*a34 + a01*a10*a22*a34 - a00*a11*a22*a34 ); kneeLdd = b0*( a13*a22*a31*a40 - a12*a23*a31*a40 - a13*a21*a32*a40 + a11*a23*a32*a40 + a12*a21*a33*a40 - a11*a22*a33*a40 - a13*a22*a30*a41 + a12*a23*a30*a41 + a13*a20*a32*a41 - a10*a23*a32*a41 - a12*a20*a33*a41 + a10*a22*a33*a41 + a13*a21*a30*a42 - a11*a23*a30*a42 - a13*a20*a31*a42 + a10*a23*a31*a42 + a11*a20*a33*a42 - a10*a21*a33*a42 - a12*a21*a30*a43 + a11*a22*a30*a43 + a12*a20*a31*a43 - a10*a22*a31*a43 - a11*a20*a32*a43 + a10*a21*a32*a43 ) + b1*( -(a03*a22*a31*a40) + a02*a23*a31*a40 + a03*a21*a32*a40 - a01*a23*a32*a40 - a02*a21*a33*a40 + a01*a22*a33*a40 + a03*a22*a30*a41 - a02*a23*a30*a41 - a03*a20*a32*a41 + a00*a23*a32*a41 + a02*a20*a33*a41 - a00*a22*a33*a41 - a03*a21*a30*a42 + a01*a23*a30*a42 + a03*a20*a31*a42 - a00*a23*a31*a42 - a01*a20*a33*a42 + a00*a21*a33*a42 + a02*a21*a30*a43 - a01*a22*a30*a43 - a02*a20*a31*a43 + a00*a22*a31*a43 + a01*a20*a32*a43 - a00*a21*a32*a43 ) + b2*( a03*a12*a31*a40 - a02*a13*a31*a40 - a03*a11*a32*a40 + a01*a13*a32*a40 + a02*a11*a33*a40 - a01*a12*a33*a40 - a03*a12*a30*a41 + a02*a13*a30*a41 + a03*a10*a32*a41 - a00*a13*a32*a41 - a02*a10*a33*a41 + a00*a12*a33*a41 + a03*a11*a30*a42 - a01*a13*a30*a42 - a03*a10*a31*a42 + a00*a13*a31*a42 + a01*a10*a33*a42 - a00*a11*a33*a42 - a02*a11*a30*a43 + a01*a12*a30*a43 + a02*a10*a31*a43 - a00*a12*a31*a43 - a01*a10*a32*a43 + a00*a11*a32*a43 ) + b3*( -(a03*a12*a21*a40) + a02*a13*a21*a40 + a03*a11*a22*a40 - a01*a13*a22*a40 - a02*a11*a23*a40 + a01*a12*a23*a40 + a03*a12*a20*a41 - a02*a13*a20*a41 - a03*a10*a22*a41 + a00*a13*a22*a41 + a02*a10*a23*a41 - a00*a12*a23*a41 - a03*a11*a20*a42 + a01*a13*a20*a42 + a03*a10*a21*a42 - a00*a13*a21*a42 - a01*a10*a23*a42 + a00*a11*a23*a42 + a02*a11*a20*a43 - a01*a12*a20*a43 - a02*a10*a21*a43 + a00*a12*a21*a43 + a01*a10*a22*a43 - a00*a11*a22*a43 ) + b4*( a03*a12*a21*a30 - a02*a13*a21*a30 - a03*a11*a22*a30 + a01*a13*a22*a30 + a02*a11*a23*a30 - a01*a12*a23*a30 - a03*a12*a20*a31 + a02*a13*a20*a31 + a03*a10*a22*a31 - a00*a13*a22*a31 - a02*a10*a23*a31 + a00*a12*a23*a31 + a03*a11*a20*a32 - a01*a13*a20*a32 - a03*a10*a21*a32 + a00*a13*a21*a32 + a01*a10*a23*a32 - a00*a11*a23*a32 - a02*a11*a20*a33 + a01*a12*a20*a33 + a02*a10*a21*a33 - a00*a12*a21*a33 - a01*a10*a22*a33 + a00*a11*a22*a33 ); result[S_X] = xdd/determinant; result[S_Y] = ydd/determinant; result[S_A0] = a0dd/determinant; result[S_HIPL] = hipLdd/determinant; result[S_KNEEL] = kneeLdd/determinant; } /**********************************************************************/ /**********************************************************************/ int integrate_one_step( Dynamics *d, double *action, double *next_state ) { int i; int n_pos; double accelerations[N_STATE_DIMENSIONS]; double new_velocity; n_pos = N_STATE_DIMENSIONS/2; dynamics( d->state, action, accelerations ); for( i = 0; i < 5; i++ ) { new_velocity = d->state[i + n_pos] + accelerations[i]*d->timestep; d->state[i] += (new_velocity + d->state[i + n_pos])*d->timestep/2; d->state[i + n_pos] = new_velocity; } d->time += d->timestep; if ( next_state != NULL ) { for( i = 0; i < N_STATE_DIMENSIONS; i++ ) next_state[i] = d->state[i]; } } /**********************************************************************/ int integrate_one_step_alt( Dynamics *d, double *state, double *action, double *next_state ) { set_state( d, 0.0, state ); integrate_one_step( d, action, next_state ); return 0; } /**********************************************************************/ double one_step_cost( Dynamics *d, double *state, double *action ) { // Not implemented yet. return 0.0; } /**********************************************************************/