Elastica Controllers

[mstoelzle]

This issue shall describe the approach to implement some simple (model-based) controllers for the simulated soft robotic arm. Obviously, first #25 needs to be completed.

To get the soft robot moving, it makes sense to implement some simple PID / PD controller in configuration-space. A PD controller for set-point regulation would look like this:

Then, we should consider some simple model-based controllers. In particular interesting for set-point regulation would be:

\tau = Kq + G(q) + K_\mathrm{p} (\bar{q}-q) - K_\mathrm{d} \dot{q}

where the gains of the feedback component can be reduced or even set to zero.

This will require the identification of the elasticity matrix K and the gravitational term G(q). While we can set the gravity to zero for the start, we can identify the (diagonal) constant matrix K with the following strategy:

1. Apply a torque tau to the system.
2. Wait for the system to reach steady-state
3. Compute the K with K=tau/q
4. We can do that for a few points, and identify our optimal fit for K with least-squares