Cart-pole system: how to apply the correct physics

Hi there!

I have worked on a simple 2D game. It consists on the cart-pole balance task, Roughly, one has to keep a pole that is attached to a cart pointing up during an arbitrary period of time. To beat gravity that pulls the pole down, it is necessary to impose the right amount of force upon the pole by moving the cart. This kind of task has been considered a benchmark in algorithm learning and its physics is described in this paper (http://coneural.org/florian/papers/05_cart_pole.pdf).

To do that I am using mouse inputs.

First approach I locked the cart displacement in one dimension and used Input.mousePosition.x to control it. To create the cart-pole system I added "hinge joint" to the cart (both the cart and the pole have rigidbody). Since only movement in x-axis was allowed, the pole could rotate only around the z-axis. The problem with this approach is that the pole seems to be insensitive to FORCES generated by the cart. That is , gravity starts its job it is almost impossible to correct pole trajectory. The pole is dragged by the cart, but there is almost no influence of the cart on the pole ROTATION.

Second approach Considering that it did not work out as I expected, I used AddTorque() taking cart acceleration as argument. Something on these lines:

 rb.AddTorque(new Vector3(0, 0, 1) * cartAcceleration, ForceMode.Acceleration);

However, it did not work either.

Third approach

After toying around with many variations of the two first approaches, I decided to code all the physics by myself using the equations (23) and (24) of the paper mentioned above. However, after the pole displaces more than (roughly) 10 degrees from the vertical line it starts to spin out all over the places. The angle seems to increase (or decrease) continuously and suddenly it jumps to more than one cycle sometimes. In addition, it loses its constraint to be "attached" to the cart and translates as well.

Here is one of the versions of the code that I have tried. It refers to the a script attached to the Pole that reads the values of interest from another script attached to the Cart.

 using UnityEngine;
 using System.Collections.Generic;
 
 public class Pole : MonoBehaviour 
 {
       // in order: cart speed, cart acceleration, pole angular velocity, pole angular acceleration
        List<float> states = new List<float>(){0, 0, 0, 0}; 
     List<float> newStates = new List<float>();
     float massCart;
     float massPole;
     float poleLength;
     float theta;
     float theta_dot;
     float theta_ddot;
     float positionX;
     float positionY;
     float cartX;
     float cartY;
     GameObject cart;
     float cartX_dot;
     float cartX_ddot;
     float sign;
 
     void Start () 
     {
         cart = GameObject.Find("Cart");
         massPole = GetComponent<Rigidbody>().mass;
         poleLength = GetComponent<Transform>().transform.localScale.y;
         theta = GetComponent<Transform>().transform.localEulerAngles.z;
         massCart = cart.GetComponent<Rigidbody>().mass;
     }
 
     void FixedUpdate () 
     {
         cartX = cart.GetComponent<Transform>().transform.position.x;
         cartY = cart.GetComponent<Transform>().transform.position.y;
         sign = Mathf.Sign(states[1]);
         newStates = CalcDerivs(states, massCart, massPole, poleLength, theta, sign);
         theta = theta + newStates[2] * Time.deltaTime;
         states[2] = states[2] + newStates[3] * Time.deltaTime; // angular vel
         states[3] = newStates[3];    // angular accel
         positionX = cartX + Mathf.Sin(theta) * poleLength;
         positionY = cartY + Mathf.Cos(theta) * poleLength;
         transform.position = new Vector3(positionX, positionY, 0);
         states[0] = GameObject.Find("Cart").GetComponent<CartMotion>().velocityN;
         states[1] = GameObject.Find("Cart").GetComponent<CartMotion>().cartAcceleration;
         transform.localEulerAngles =new Vector3(0, 0, Mathf.Rad2Deg * theta);
 
     }
 
     static List<float> CalcDerivs(List<float> states, float massCart, 
         float massPole, float poleLength, float theta, float sign)
     {
         float gravity = 9.81f;
         float force = states[1] * (massCart + massPole);
         float force = states[1] * (massCart + massPole) -
             (massPole * poleLength *
                 (Mathf.Pow(states[2], 2) * Mathf.Sin(theta) - states[3] * Mathf.Cos(theta)));
         float numerator = gravity * Mathf.Sin(theta) + Mathf.Cos(theta) *
             ((-sign * force - massPole * poleLength * Mathf.Pow(states[2], 2) * Mathf.Sin(theta)) /
                 (massPole + massCart));
         float denominator = poleLength *
             ((4 / 3) -
                 (massPole * Mathf.Pow(Mathf.Cos(theta), 2) /
                     (massPole + massCart)));
 
         float theta_ddot = numerator / denominator;
         List<float> newStates = new List<float>() { states[0], states[1], states[2], theta_ddot, force };
 
         return newStates;
     }
 }

A side note: there are some variable that I do not use in this part of the code, e.g. states[0]. I have checked by printing results of many different variables that "cartAcceleration" and "force" do not change erratically as theta (the angle of the pole does). The reason why I prefer something like the third approach is the fact that I can make explicit relations with other objects with specific physics like that.

I appreciate any help.