#9 Basic self-balancing WITH torque (part 5 - NEW library applied)

I have started to implement the New library (Spatial Forces block) into my basic self-balancing system, and created contact between all points of the robot's parts and the plane but of course deleting the OLD library blocks of the "Contact forces".

I connected them and did a subsystem to keep it organised on the final system.

Inside the subsystem:

Outside the subsystem (view in the main system):

However, now even with the system set up to have a plane and some physics, the simulation still doesn't run correctly, or better said, it doesn't run how I expect it to run. When the simulation is launched, the robot falls on the ground, after the wheels move a little bit, but then they stop rotating/moving and nothing else happens.

With a Step input that goes to 1radiant initially (as a disturbance) then after 0.1 seconds (reaction time) it goes to 0(zero) radiants, and the PID only had a proportional gain value of 5, to simply observe the error and behavior:

The simulated result I managed to get is the following, which was somewhat more realistic than previous but at the same time it was not something which I expected:

The graph for the Error (Actual Output), showing that in around 0.5 seconds it reached the desired Output Angle, with an initial disturbance of 1 raiants which lasted for exactly 0.1 seconds, which despite it being correctly displayed in the graph I still wasn't getting what I was expecting to happen, the robot staying upright and using "up" as 0 degrees/radiants as setpoint to follow:

I have also checked the Torque Output Graph after the PID controller which was running at a value of 5 for the proportional gain only, and the graph is indicating that there is some torque involved for a very brief moment, but then in less than 0.5 seconds it settles to 0 torque output:

I didn't understand why that was happening (error graph showing that desired values were reaching their destination, but actual simulation and torque were showing almost like a loss of "energy" or a "shutdown" or my robot). I kept doing different tries, and the PID controller was allowing me to tune it, and it was having the same behavior just with better results on the graphs, so I seem to be stuck.

In order to do some more trials and errors, I wanted to check my circumstances but with different values, so I set some parameters to the Step Input (Desired Output/Angle) to start straight away and get a desired output of 45 degrees (since I didn't use here a gain to convert I just put it in radiants which is 0.78539816 rad just like in the Figure below), then the final value as 0 (zero) degrees with a Step Time (reaction time after it reached the initial desired output) of 1 second. The PID was not automatically tuned, but it has a proportional gain of 5 and both integral and derivate gains were left to 0 (zero).

The simulated result which I got shows how it first goes to 45 degrees, then to 0 degrees, and here it was almost like a revelation, because the angle which I was measuring, I realised it was the angle between the wheels and the actual chassis, and NOT the angle between the robot and the world, hence the torque was dropping to 0 (zero) all this time, because it was in fact reaching the desired values!

In the following video, the wheels go first to reach a 45 deg angle between wheels' axis and chassis' axis, then after the Step time, it goes to reach that "final value" of 0 degrees:

Same again for the Error graph (final output comparing the desired angle VS actual angle):

My conclusion here is that all what I was doing so far, was working well between system and simulation, including the physics, however it was not doing what I was expecting it to do from a VISUAL point of view.

For next I could try to see how I could sense the angle between chassis and the world.

For now my system looks like this:

A very simplified version of that same system, where the world dynamics, plane, contacts, entire robot (with wheels, chassis, input torque to revolute joint, sensing position angle as "Sensor" from the same revolute joint) everything was put together into a unique subsystem to keep the main system neat and have a more conventional "Close control loop system" is as seen as follow: