Incorrect cartpole dynamics

[stockeh]

There are two algebraic errors that change the intended behavior for a frictionless cartpole system. The first is with the polemass_length and second is in the denominator of thetaacc.

Current implementation:

PufferLib/pufferlib/ocean/cartpole/cartpole.h

Lines 184 to 189 in 971344b

	float total_mass = env->cart_mass + env->pole_mass;
	float polemass_length = total_mass + env->pole_mass;
	float temp = (force + polemass_length * env->theta_dot * env->theta_dot * sintheta) / total_mass;
	float thetaacc = (env->gravity * sintheta - costheta * temp) /
	(env->pole_length * (4.0f / 3.0f - total_mass * costheta * costheta / total_mass));
	float xacc = temp - polemass_length * thetaacc * costheta / total_mass;

See equations 23 and 24 for the correct equations for the dynamics of the cart-pole system from Florian.

Proposed fix:

float total_mass = env->cart_mass + env->pole_mass;
/*!! (1): should be (m_p * l), not (total_mass + m_p) */
float polemass_length = env->pole_mass * env->pole_length;
float temp = (force + polemass_length * env->theta_dot * env->theta_dot * sintheta) / total_mass;

/* (2): denominator should be (m_p * cos^2(theta) / total_mass) 
        not (total_mass *  cos^2(theta) / total_mass) */
float thetaacc = (env->gravity * sintheta - costheta * temp) /
                 (env->pole_length * (4.0f / 3.0f - (env->pole_mass * costheta * costheta) / total_mass));
float xacc = temp - polemass_length * thetaacc * costheta / total_mass;

[jsuarez5341]

@xinpw8