Python implementations of Control and Optimization algorithms for simulated underactuated systems and walking robots
Inspired by the MIT's course 6.832 - Underactuated Robotics of Russ Tedrake. The corresponding edX course as well as the course notes largely assisted in studying the mathematical background of the showcased algorithms.
For benchmarking and simulation purposes, some well-known low-dimensional systems (frequently used in the literature) are implemented. Both fully-actuated and underactuated versions are tested.
| Acrobot | Simple Pendulum | Cart-Pole | ||
|---|---|---|---|---|
 |
 |
 |
||
| Rimless Wheel | ||||
 |
Fully-actuated control becomes trivial when using feedback linearization by cancelling-out the complex dynamics of the system and converting it to a linear system (feedback equivalent).
For a second-order control-affine system
the feedback control
makes the system equivalent to allowing
us to plug-in a simple PD controller with pole placement and transition the system
to the desired state.
Swing-up control for pendulum and acrobot:
 |
 |
 |
Stabilize a non-linear system around a fixed point by applying a Linear Quadratic Regulator (LQR) on a linearized version of the system. The system's initial state needs to be in the region of attraction of the fixed point.
LQR is a feedback controller derived from Dynamic Programming for linear systems and quadratic cost on state and control signal.
 |
 |
 |
To apply LQR, we first have to bring the system near the fixed point.
To achieve this, energy shaping controller pumps or draws energy from/to
the system until it reaches the energy of the desired state. This is
usually done by calculating the error between the
system's energy
and the desired energy
(that is
)
and expressing its time derivative in the form
Then, the control signal
guarantees that the energy error is non-increasing (since
).
Theoretically, this should suffice but small perturbations (either physical or due to numerical errors) can move the system far from the goal state.
In this situation, once the system is near the fixed point (inside its basin of attraction) the controller can switch from energy shaping to LQR and stabilize the system.
 |
 |
The simplest model of legged robot. Assumes there will always be a swing leg in position at the time of collision. It uses pendula for legs and moves only thanks to the gravity. With appropriate initial leg angle and angular velocity the wheel reaches periodic stability (limit cycle) and walks down the slope.
The foot standing on the ground is modeled with a simple pendulum (systems/pendulum.py)
with initial angle near the upward position. Using the simple
collision detection logic that the pendulum's angle (from the upward position)
is
the pendulum's position is changed during the simulation and is
placed in the position of next leg to hit the ground.