View on GitHub

Python example of Copra

This is an how-to-use article of Copra library. To understand more on the library, you can visit here. It is a simple example written in python and base on robot locomotion problem. The example is based on this paper.

This example aims to solve a common problem in humanoid robot dynamic walk. In dynamic walk, a model predictive control is used to find a trajectory for the Center of Mass (CoM) $s$ of the robot considering its Zero-momentum Point (ZMP). The ZMP is a point on the ground which must stay inside the polygon defined by the foot of the robot. Supposing the CoM of the robot does not move vertically, the position $z$ of the CoP is \(z = s - \frac{h_{CoM}}{g}\ddot{s}\)

This equation can be written as a linear system.

\[\mathbf{\dot{x}} = \left[ \begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{array} \right]\mathbf{x} + \left[ \begin{array}{c} 0 \\ 0 \\ 1 \end{array} \right]u\]

with $\mathbf{x} = \left[s\ \dot{s}\ \ddot{s}\right]^T$ the state variable and $u = \dddot{x}$ the control variable. From it, we can easily perform the discretization.

\[\mathbf{x}_{k+1} = \left[ \begin{array}{ccc} 1 & T & \frac{T^2}{2} \\ 0 & 1 & T \\ 0 & 0 & 1 \end{array} \right]\mathbf{x}_k + \left[ \begin{array}{c} \frac{T^3}{6} \\ \frac{T^2}{2} \\ T \end{array} \right]u\]

The ZMP should also be constrained between two references values $z_{min}$ and $z_{max}$ which leads to

\[z_{min} \leq [1\ 0\ \text{-}\frac{h_{CoM}}{g}]\mathbf{s} \leq z_{max}\]

which is splitted into

\[\left\{ \begin{array}{rcl} \text{-}[1\ 0\ \text{-}\frac{h_{CoM}}{g}]\mathbf{s} & \leq & \text{-}z_{min} \\ [1\ 0\ \text{-}\frac{h_{CoM}}{g}]\mathbf{s} & \leq & z_{max} \end{array} \right.\]

And finally we have two cost functions i) A trajectory cost $\mathbf{z}^{ref}$ with a weight $Q$ and ii) a control cost with weight $R$.

The code

First of all, the headers

import numpy as np
import pyCopra as copra

then we need to create the system

A = np.identity(3)
A[0, 1] = T
A[0, 2] = T * T / 2.
A[1, 2] = T

B = np.array([[T* T * T / 6.], [T * T / 2.], [T]])

d = np.zeros((3,))
x_0 = np.zeros((3,))
ps = copra.NewPreviewSystem(A, B, d, x_0, nrStep)

Then we create the ZMP constraint

E2 = np.array([[1., 0., -h_CoM / g]])
E1 = -E2
f1 = np.array([z_min])
f2 = np.array([z_max])

traj_constr_1 = copra.NewTrajectoryConstraint(E1, f1)
traj_constr_2 = copra.NewTrajectoryConstraint(E2, f2)

Build the cost function

M = np.array([[1., 0., -h_CoM / g]])

traj_cost = copra.NewTrajectoryCost(M, -z_ref);
traj_cost.weight(Q);
traj_cost.autoSpan(); # Make the dimension consistent (z_ref size is nrSteps)

N = np.array([[1.]])
p = np.array([0.])

control_cost = copra.ControlCost(N, -p);
control_cost.weight(R);

Create the copra and solve

controller = copra.LMPC(ps)
controller.add_constraint(traj_constr_1)
controller.add_constraint(traj_constr_2)
controller.add_cost(traj_cost)
controller.add_cost(control_cost)

controller.solve();

Finally, get the results

trajectory = controller.trajectory()
jerk = controller.control()
com_pos = np.array((nrSteps,))
com_vel = np.array((nrSteps,))
com_acc = np.array((nrSteps,))
zmp_pos = np.array((nrSteps,))
for i in range(nr_steps):
    com_pos[i] = trajectory[3 * i]
    com_vel[2 * i] = trajectory[3 * i + 1]
    com_acc[2 * i] = trajectory[3 * i + 2]
    zmp_pos[i] = com_pos[i] - (h_CoM / g) * com_acc[i]