/************************************************************************/ /* dynamics for a torso and thigh flying through the air. */ /************************************************************************/ #include #include #include #include "dynamics.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 TORSOTOHIP -0.5 #define HIPTOTHIGHCOM -0.25 // #define GRAVITY 9.81 #define GRAVITY 0.0 /* Handy macros */ #define SQ(x) ((x)*(x)) // Timestep of integrator #define DEFAULT_TIMESTEP 0.01 /**********************************************************************/ /* Globals */ double default_initial_state[N_STATE_DIMENSIONS] = { 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 TorsotoHip = TORSOTOHIP; double HiptoThighCOM = HIPTOTHIGHCOM; double Itorso = (TORSO_MASS*SQ(TORSO_LENGTH)/12); /* Icom */ double Ithigh = (THIGH_MASS*SQ(THIGH_LENGTH)/12); /* Icom */ double x, xd, xdd; double y, yd, ydd; double a0, a0d, a0dd; double hipL, hipLd, hipLdd; double tauHipL; double a00, a01, a02, a03; double a10, a11, a12, a13; double a20, a21, a22, a23; double a30, a31, a32, a33; double b0, b1, b2, b3; double determinant; double c0, s0, cHl, sHl, c0Hl, s0Hl; double G = GRAVITY; c0 = cos( a0 ); s0 = sin( a0 ); cHl = cos( hipL ); sHl = sin( hipL ); c0Hl = cos(a0 + hipL); s0Hl = sin(a0 + hipL); x = state[S_X]; y = state[S_Y]; a0 = state[S_A0]; hipL = state[S_HIPL]; xd = state[S_XD]; yd = state[S_YD]; a0d = state[S_A0D]; hipLd = state[S_HIPLD]; tauHipL = action[A_HIPL]; /* Solve A*acceleration = B */ /* First row */ a00 = Mthigh + Mtorso; a01 = 0; a02 = -(Mthigh*(TorsotoHip*c0 + HiptoThighCOM*c0Hl)); a03 = -(HiptoThighCOM*Mthigh*c0Hl); /* fx is zero */ b0 = -( Mthigh*(SQ(a0d)*TorsotoHip*s0 + SQ(a0d + hipLd)*HiptoThighCOM*s0Hl) ); /* Second row */ a10 = 0; a11 = Mthigh + Mtorso; a12 = -(Mthigh*(TorsotoHip*s0 + HiptoThighCOM*s0Hl)); a13 = -(HiptoThighCOM*Mthigh*s0Hl); /* fy is zero */ b1 = -( G*(Mthigh + Mtorso) - SQ(a0d)*Mthigh*TorsotoHip*c0 - SQ(a0d + hipLd)*HiptoThighCOM*Mthigh*c0Hl ); /* Third row */ a20 = -(Mthigh*(TorsotoHip*c0 + HiptoThighCOM*c0Hl)); a21 = -(Mthigh*(TorsotoHip*s0 + HiptoThighCOM*s0Hl)); a22 = Ithigh + Itorso + SQ(HiptoThighCOM)*Mthigh + Mthigh*SQ(TorsotoHip) + 2*HiptoThighCOM*Mthigh*TorsotoHip*cHl; a23 = Ithigh + SQ(HiptoThighCOM)*Mthigh + HiptoThighCOM*Mthigh*TorsotoHip*cHl; /* tau0 is zero */ b2 = -( -(Mthigh*(G*TorsotoHip*s0 + HiptoThighCOM*(hipLd*(2*a0d + hipLd)*TorsotoHip*sHl + G*s0Hl))) ); /* Fourth row */ a30 = -(HiptoThighCOM*Mthigh*c0Hl); a31 = -(HiptoThighCOM*Mthigh*s0Hl); a32 = Ithigh + SQ(HiptoThighCOM)*Mthigh + HiptoThighCOM*Mthigh*TorsotoHip*cHl; a33 = Ithigh + SQ(HiptoThighCOM)*Mthigh; b3 = tauHipL -( HiptoThighCOM*Mthigh*(SQ(a0d)*TorsotoHip*sHl - G*s0Hl) ); determinant = (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); xdd = b0*(-(a13*a22*a31) + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 + a11*a22*a33) + b1*(a03*a22*a31 - a02*a23*a31 - a03*a21*a32 + a01*a23*a32 + a02*a21*a33 - a01*a22*a33) + b2*(-(a03*a12*a31) + a02*a13*a31 + a03*a11*a32 - a01*a13*a32 - a02*a11*a33 + a01*a12*a33) + b3*(a03*a12*a21 - a02*a13*a21 - a03*a11*a22 + a01*a13*a22 + a02*a11*a23 - a01*a12*a23); ydd = b0*(a13*a22*a30 - a12*a23*a30 - a13*a20*a32 + a10*a23*a32 + a12*a20*a33 - a10*a22*a33) + b1*(-(a03*a22*a30) + a02*a23*a30 + a03*a20*a32 - a00*a23*a32 - a02*a20*a33 + a00*a22*a33) + b2*(a03*a12*a30 - a02*a13*a30 - a03*a10*a32 + a00*a13*a32 + a02*a10*a33 - a00*a12*a33) + b3*(-(a03*a12*a20) + a02*a13*a20 + a03*a10*a22 - a00*a13*a22 - a02*a10*a23 + a00*a12*a23); a0dd = b0*(-(a13*a21*a30) + a11*a23*a30 + a13*a20*a31 - a10*a23*a31 - a11*a20*a33 + a10*a21*a33) + b1*(a03*a21*a30 - a01*a23*a30 - a03*a20*a31 + a00*a23*a31 + a01*a20*a33 - a00*a21*a33) + b2*(-(a03*a11*a30) + a01*a13*a30 + a03*a10*a31 - a00*a13*a31 - a01*a10*a33 + a00*a11*a33) + b3*(a03*a11*a20 - a01*a13*a20 - a03*a10*a21 + a00*a13*a21 + a01*a10*a23 - a00*a11*a23); hipLdd = b0*(a12*a21*a30 - a11*a22*a30 - a12*a20*a31 + a10*a22*a31 + a11*a20*a32 - a10*a21*a32) + b1*(-(a02*a21*a30) + a01*a22*a30 + a02*a20*a31 - a00*a22*a31 - a01*a20*a32 + a00*a21*a32) + b2*(a02*a11*a30 - a01*a12*a30 - a02*a10*a31 + a00*a12*a31 + a01*a10*a32 - a00*a11*a32) + b3*(-(a02*a11*a20) + a01*a12*a20 + a02*a10*a21 - a00*a12*a21 - a01*a10*a22 + a00*a11*a22); result[S_X] = xdd/determinant; result[S_Y] = ydd/determinant; result[S_A0] = a0dd/determinant; result[S_HIPL] = hipLdd/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 < 4; 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; } /**********************************************************************/