View on GitHub

C++ 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 C++ 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

This equation can be written as a linear system.

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.

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

which is splitted into

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

// stl headers
#include <iostream>
#include <memory>
#include <utility>

// copra headers
#include <copra/constraints.h>
#include <copra/costFunctions.h>
#include <copra/LMPC.h>
#include <copra/PreviewSystem.h>

then we need to create the system

Eigen::Matrix3d A(Eigen::Matrix3d::Identity());
A(0, 1) = T;
A(0, 2) = T * T / 2.;
A(1, 2) = T;

Eigen::Vector3d B(T* T * T / 6., T * T / 2, T);

Eigen::Vector3d d(Eigen::Vector3d::Zero());
Eigen::Vector3d x_0(Eigen::Vector3d::Zero());
auto ps = std::make_shared<copra::PreviewSystem>(A, B, d, x_0, nrStep);

Then we create the ZMP constraint

Eigen::<double, 1, 3> E1, E2;
E2 << 1, 0, h_CoM / g;
E1 = -E2;
Eigen::<double, 1, 1> f1, f2;
f1 << z_min; 
f2 << z_max;

TrajConstr1 = std::make_shared<copra::TrajectoryConstraint>(E1, f1);
TrajConstr2 = std::make_shared<copra::TrajectoryConstraint>(E2, f2);

Build the cost function

Eigen::<double, 1, 3> M;
M << 1, 0, -h_CoM / g;

trajCost = std::make_shared<copra::TrajectoryCost>(M, -z_ref);
trajCost->weight(Q);
trajCost->autoSpan(); // Make the dimension consistent (z_ref size is nrSteps)

Eigen::<double, 1, 1> N, p;
N << 1;
p << 0;

controlCost = std::make_shared<copra::ControlCost>(N, -p);
controlCost->weight(R);

Create the copra and solve

copra::LMPC controller(ps);
controller.addConstraint(TrajConstr1);
controller.addConstraint(TrajConstr2);
controller.addCost(trajCost);
controller.addCost(controlCost);

controller.solve();

Finally, get the results

Eigen::VectorXd trajectory(controller.trajectory());
Eigen::VectorXd jerk(controller.control());
Eigen::VectorXd comPos(nrSteps);
Eigen::VectorXd comVel(nrSteps);
Eigen::VectorXd comAcc(nrSteps);
Eigen::VectorXd zmpPos(nrSteps);
for (int i = 0; i < nrSteps; ++i) {
    comPos(i) = trajectory(3 * i);
    comVel(2 * i) = trajectory(3 * i + 1);
    comAcc(2 * i) = trajectory(3 * i + 2);
    zmpPos(i) = comPos(i) - (h_CoM / g) * comAcc(i);
}