SixDOFConstraint Motor setting for PID controller

[almic]

Foreword: This is more of a "I want this and I'm doing it, but does anyone else want it / think it is useful?"

The Idea

Add a new mode, like velocity and position, that turns motors into a one or two stage PID controller. In one-stage-mode, it measures the angle and computes a velocity from the PID parameters and targets that velocity. It deviates from just a Position mode because it can accumulate error and derivative every velocity step, has the proportional parameter, instead of one calculation in the setup phase. In the two-stage-mode, it takes the output target of the velocity controller and passes it to an acceleration controller that measures the current iteration axis velocity to compute an acceleration, added to the current joint velocity and used as the target velocity for the motor.

My Motivation

I am looking at a setup for a robotic simulation, basically a three segment leg that uses motors to follow an IK target. I am using Godot. Been bashing my head into it for way too long, I ported all the relevant code for a SixDOFConstraint into GDScript and used a debugger to get full Jolt parity (stepping line-by-line, one in Jolt, one in Godot) try to solve why motors are hard to get moving.

The main issue is that there is a small mass object (the femur, I am modelling an ant-structure) that connects the rest of the leg to the main body. Here is a diagram of the bone structure, and mass is proportional to segment length, so bigger means more mass.

I have highlighted with yellow circles the joints. From left to right is the core body, femur, tibia, and tarsus. As you can see, most of the X axis (pointing out of the screen) transfers through the femur because of the mass differences, and is effectively "sunk" into the core. This means even with 16 or 32, or 100 velocity steps, the motors barely even begin to approach their target velocity. This isn't just a problem with getting up to speed, it mainly becomes a problem when they try to stop, because it takes much much longer to slow down because most of the torque sinks into the more massive bodies and struggles to change actual joint velocity. Overshoot happens constantly with this setup and is unacceptable for my goal.

I was using a PID in my own code and simulating ahead to attempt to correct for this. Mainly it is just because of my setup, the large mass disparity just makes this a hard thing to solve iteratively. If I want 1r/s, I would have to target 20+r/s on the femur, for instance. This has always fallen apart when adding the third segment, the tarsus, because it just puts too much on the femur. It will either oscillate or look like it gives up just as it reaches the target angle because it drops the velocity. It also became apparent that I'm just not working in the real physics sim, so there are external variables that always mess up my predictions just enough to make it worthless.

More

I'm going to start implementing this now for a custom build to see how well it works. The main driver is the mass disparity which sinks a lot of the work the motor is doing, and because it only accounts for the difference each step, the approach is fractional (like 10% relative progress each step, which is unacceptable for my situation). With a PID controller, it can deliberately attempt to overshoot the goal, or at least use the derivative/ integral term to approach faster. I expect to get large returns from a direct implementation instead of an external one, because it will be working directly with the simulation and essentially have "all the information" as it iterates.

If this sounds like something that should be added to Jolt directly, I will be trying to make it fit with current API so maybe it can be a PR in the future.

[almic]

An update: I have successfully implemented a PID Controller in the SixDOFConstraint, and it did help me achieve better motor response. However, it was still unacceptable in performance. I would say it solved ~30% of the problem I was having. It let me stay at my target 16 velocity steps and crank up the motor velocities about half-way to what I wanted, but it was still unstable at times and would begin to hunt if I went any higher. My previous result was 1-2 proportional velocity on an "out-of-band" solution before instability, and the integrated PID got me to 5-6, so it really did help.

However, the lion's share of the problem was addressed by changing my mass ratios between the leg segments. The femur now has 70% of the entire leg mass, and the tibia and tarsus each have 15%. This got me exactly what I wanted, very high velocity responses with no chasing at all. I am using 16 proportional for velocity and acceleration at 1.5P and 2.0D. I don't make use of the integral parameter because the simulation doesn't have inherent error that the proportional term can't correct.

Left this update in case anyone may want my implementation, or has the same issue as me. In the end, the mass ratios between the leg segments was the primary problem. A small mass between two large masses just makes it very hard for the system to converge to their target velocities, because most of the rotational velocity leaks away into the other large mass via the small mass.

[jrouwe]

I would indeed recommend tweaking the masses like you did. Note that I'm in the process of making a change that should make the simulation behave much better when large mass ratios are involved (still a lot of work to do, so I don't know when it will be done yet).

W.r.t. the PID controller: Do you have the source code somewhere so that I can take a look at it?

[almic]

here is my commit that adds this, it is on a fork of Godot so you have to ignore some of that. I tried to follow the code style in case you ever wanted to look at it. It's only implemented in SixDOFConstraint. I wrote it on 5.5 or something and then updated to 5.6 so some of the spring stuff could probably be improved to use existing methods. I use a version of PID that doesn't divide the derivative term by delta, which I read is the recommended way if delta is typically tiny (<1 second).

almic/godot@efd4aeb

I regularly rewrite history of this fork, so I'll try to remember to come back and update the commit link in case it moves.

Edit: i see I corrected the 5.6 commit instead of the other way around, but it was generally just accepting the 5.6 changes and fixing syntax issues

[jrouwe]

I've looked at your changes:

• PIDController::mOutput seems to be a redundant variable. You're not calling GetOutput and the only other usage is right before it is calculated.
• SixDOFConstraint::SolveVelocityConstraint calls mMotorPIDAcceleration[axis].Update(..., inDeltaTime) but that function is called PhysicsSetings::mNumVelocitySteps times per step, so I don't think you're getting what you want there w.r.t. inDeltaTime.

What I would probably try is to create a PhysicsStepListener that configures SixDOFConstraint::SetTargetVelocity and if you need to control acceleration too you can configure e.g. MotorSettings::SetTorqueLimits on SixDOFConstraint::GetMotorSettings. That way you don't need any changes to Jolt.