Your suggestions definitely steered me into the right direction (I think)
I have a first version working. It’s not perfect, but the balancing on the ramp works good enough for now. I have the problem though that the controller ist not stable yet. After a short time, it always results in oscillations that cause the robot to fall to one side. I also have a pretty good idea why these oscillations occur, but I don’t know how to fix them. Maybe you or someone else here has an idea. Unfortunately I have to elaborate a bit to explain the problem, but please bear with me:
So like you suggested, I calculated the transformation and the jacobian, that map the two knee actuators to these two values:
- The resulting robot angle (assuming the ground is flat)
- The average height
Because both knee actuators are connected to the same ODrive, I’m able to do this calculation on the ODrive itself. I then also added a simple PD controller with separate gains for the robot angle and the height.
So far, this works great and if I, for example, set the gains for the angle to zero, the robot stays at the same height, but is freely movable to either side. Here is a plot of a run, where I did that:
What you can see here is the robot angle, as calculated by the joint angles, in black. (This is calculated on the ODrive)
Plotted in blue is the imu angle reading. This is only sampled with about 50Hz, because the imu sensor isn’t connected directly to the ODrive.
Because both wheels stayed on the ground in this run, you’d expect both values to be about the same, but you can see that the imu sensor lags a bit behind and also seems to overshoot a bit.
For comparison, here is a run, where I enabled both gains (so the robot tries to keep the leg lengths the same):
In this run, I moved the robot over a ramp, such that one leg is lifted, just like in the video in the first post. As you can see, the robot angle is successfully kept very close to zero (the small displacement is from the fall at the end of the ramp). And the imu sensor shows how the robot moves to the side, as it goes over the ramp.
Ok, now to implement the balancing as shown in the video, I changed the force, that is applied to the robot angle in such a way, that the imu sensor is kept close to zero. A lot of trial and error showed, that a high p value, a low d value and zero i produce the best results. Here is one run:
You can see, that the imu sensor value (blue) stays kinda close to zero and the robot angle (black) successfully compensates for the ramp. I also plotted the force, that is applied to the robot angle in red here.
So, this works all great so far. Now we come to the problem. Here is a run with no ramp:
You can see, that an oscillation starts out of nowhere. It then stops, but only because I then start to hold it tight to prevent a fall.
Here is another oscillation
Now the reason for this seems to me to be the delay of the imu sensor (blue and black should be about equal here). You can see that the frequency is such that the force (red) aligns with the derivative of the robot angle (dt of black). But how do I prevent this?
I don’t think I have cpu time left on the ODrive to read an imu sensor there directly, and I’m not sure if that would prevent the delay.
If I reduce the P gain or increase the D gain of the imu pid controller so much that the oscillations stop, it also stops being effective on the ramp.
So, sorry for the lengthy description, but I’m sort of stuck on this problem right know. I’m happy to share more details if requested.