Inverse dynamics from URDF files.

Dynamic Control

Robotics

Code Generation

Generate real-time optimized MPC controllers for precise end-effector trajectory tracking 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.

Preliminaries

This is a continuation of a previous article in which I described how I am able to obtain state-feeback linearization from URDF files.
A neat fact about the resulting system, that I failed to mention in that article, is that the setup enables us to exactly follow certain joint-space trajectories. This is cool, but there are some caveats:

The trajectory must be in $\mathcal{C}^3$ (no jumps in position, velocity, acceleration)
We need to exactly know the first two derivatives — numerical differentiation may lead to instability!
It's in joint space, so not that useful for practical tasks

The following control law will stabilize the joint coordinates

q

against such a trajectory

q^\star

\begin{align} v &= \ddot{q}^\star - K_D(\dot{q}^\star - \dot{q}) - K_P(q^\star - q) - K_I \int_0^{t} q^\star - q \, dt \end{align}

(1)

Goal

My short term goal is to make the robotic arm shown in move the tip of the pencil along a desired trajectory, while keeping the pencil normal to the drawing surface.

Modified URDF from BostonDynamics

A quick and easy solution would be to employ some sort of inverse kinematics solver to compute joint-space positions for the desired positions of the pencil tip. This works and is used in the industry. I know for a fact that this has been done for sorting robots at Amazon facilities. When throughput needed to be increased, the arm started flinging packages around because it could no longer follow the trajectories sufficiently well. We can do better.

In the beginning, I mentioned that we can guarantee precise following for certain joint-space trajectories. All we need is a way to obtain one of these trajectories. Take a look at the following system with

x = (q, \dot{q}, \ddot{q})

\dot{x} = \begin{pmatrix} 0 & I & 0 \\ 0 & 0 & I \\ 0 & 0 & 0 \end{pmatrix}x + \begin{pmatrix} 0 \\ 0 \\ I \end{pmatrix} u

(2)

Any solution to the above equations satisfies the conditions listed in the beginning, which allow us to exactly follow a certain trajectory. In other words, the above system perfectly captures the dynamic constraints of the robot. By generating solutions for this system, we can also obtain all required derivatives via integration, avoiding error-prone numerical differentiation.

Of course, we want to somehow find those solutions that cause the pen tip to follow the desired path. Using ACADOS we can generate a solver for an optimal control problem with a cost function which measures the tracking error of the pencil tip. This function can be easily constructed symbolically from a URDF file using Odysseus.

Generating the solver

As before, I wrote a script which generates a solver for this type of system. It is relatively flexible, allowing for the user to tackle all sorts of end-effector tracking problems.

python -m nlc_code_gen.models.id_nmpc \
  --root-link="base" \
  --actuated-joints="arm_sh0 arm_sh1 arm_el0 arm_el1 arm_wr0" \
  --end-effectors="pen_tip" \
  --reference-type="position+heading" \
  --heading-axis="0 0 1" \
  --control-type="jerk" \
  --horizon-seconds="1.0" \
  --shooting-intervals="25" \
  --urdf="$(xacro ./urdf/standalone_arm.urdf.xacro)" \
  --output="./lib/drawing_bot_id.so"

(3)

The generated shared object contains an optimized solver along with metadata detailing the layout of the state, reference, and control vectors. It can be loaded by a controller without much additional configuration.

Results

I made the pen tip follow a straight line in the

x/y

plane. Here's what the tracking behavior in the

x

direction looks like:

The reference and actual trajectories are almost identical, so let's zoom in on the error:

The max. tracking error is somewhere around

1.5 \mathrm{mm}

along this coordinate, which is OK but not great. Keep in mind that I am still commanding trajectories at this point, not paths. This means we are measuring the distance bewteen the commanded and actual positions at each time step, not the distance between the pen tip and the path. I suspect that the robot simply cannot reach the desired position at the specified time. The path tracking error may therefore be much smaller than measured.