Goodbye nonlinear dynamics!

Dynamic Control

Robotics

Code Generation

Python

Generate optimized state-feedback linearization and decoupling controllers within seconds by automatically deriving Euler-Lagrange equations from URDF files.

Disclaimer

This is part of a larger project, the code of which I maintain in a monorepo which also includes modified versions of non-freely distributable URDF files and other resources. This currently prevents me from making the code public. I will eventually split up the project so that all parts described here are available as FOSS. If you're interested in this project, please contact me.

Coupled and Nonlinear

Most robots exhibit highly coupled, nonlinear dynamics: Think of a robotic arm for instance: joint on a joint on a joint ... or even worse — a walking robot — effectively multiple robotic arms attached to the same base, with all joints influencing each other.

Modified URDF from BostonDynamics

ANYmal by ANYbotics

Luckily, methods exist, which can enforce decoupled, linear dynamics onto the system via state feedback. From here, we can substitute the nonlinear dynamics

f(q, \dot{q}, u) = 0

with a simple integrator chain, without a single approximation required:

f(q, \dot{q}, u) = 0 {\implies} \begin{split} \dot{x} &= \begin{pmatrix} 0 & I \\ 0 & 0 \end{pmatrix}x + \begin{pmatrix} 0 \\ I \end{pmatrix} v \end{split}

(1)

with

x=\begin{pmatrix}q & \dot{q}\end{pmatrix}^\top

and

v

being some virtual input into the system.
The problem is that these methods require the exact equations of motion of the robot to be controlled.

Not worth the effort

You may have done this at some point, and you may recall that modelling even a 2D double pendulum by hand is tedious because the equations quickly explode in size. Here, we are dealing with

5-12

DOF systems operating in three dimensions. Most people cannot be bothered to do this and simply throw joint-wise PD control at the robot without much thought. Some seem to get around this problem by modelling the segments as massless. While this can work for basic tasks, it falls short when high-fidelity, high-precision control is needed, especially as velocities or the amount of mass in motion increase.

Odysseus + CasADi

A while ago, I wrote Odysseus, a tool for deriving equations of motion for almost arbitrary assemblies. From this we can easily derive the equations of the required control law. The control law can then be transformed into a CasADi function, which can be exported and compiled into a shared library. All that remains is to write a generic controller in C++ which can load CasADi functions as control laws.

In Action

Having done all the above, I can now generate controllers for any robot by running a single command:

python -m nlc_code_gen.models.computed_torque \
    --root-link="base" \
    --actuated-joints="arm_sh0 arm_sh1 arm_el0 arm_el1 arm_wr0" \
    --urdf="$(xacro ./standalone_arm.urdf.xacro)" \
    --output="install/drawing_demo/lib/drawing_bot_ct.so"

(2)

I added a PID controller and simulated a jump in all joints of the robotic arm in Gazebo and compared the response to the analytical solution derived from the system in (1). Here are the results (just showing one of the joints):

The MSE of the position (rad) between the linear model and the controlled robot was in the range of

10^{-5}

in this example. I also tried fitting a closed-loop system onto the response to see if I could re-identify the PID gains I used (hence the 'Fit' curve) — which is also possible with errors below

0.1\%