Preliminaries

This example assumes that the concepts presented in How to write a problem are somewhat known, in that we will not describe all the details like the creation of space and variables. It aims at showing a simpler case where one want to directly write a constrained linear least-squares problem, without relying on task dynamics and automatic linearization. It showcases the use of tvm::utils::AffineExpr, which offers a symbolic-like way of writing linear expressions.

In this example we consider a simplified 3-limbs robot whose mass \( m \) is concentrated at its Center of Mass (CoM). The kinematics of the limbs is abstracted, and the robot is in ponctual contact with its environment at each end of its limbs (see figure). At each contact point \( p_i \), the contact force is \( f_i \) and the contact normal is \( n_i \).

We would like to find the position \( c \) of the CoM and a set of forces such that the robot is in static equilibrium. The forces should be as close as possible to desired forces \( f_i^{des} \), and because this does not completely specify the position of the CoM, we will also require the CoM to be close to the contacts.

This can be written as a linear least-squares problem:

\( \begin{align} \min_{f_i, c}. &\ \frac{1}{2} \sum_i \left\| f_i - f_i^{des} \right\|^2 + \frac{w}{2} \sum_i \left\| c - p_i \right\|^2\\ \mbox{s.t.} &\ \sum_i R_i f_i + m g = 0 \\ &\ \sum_i \widehat{p_i} R_i f_i + m \widehat{g} c = 0\\ &\ C f_i \geq 0,\ i=1..3 \\ \end{align} \)

where \( w \) is a weight and \( R_i \) is a rotation matrix such that \( R_i z = n_i\) (with \( z \) the vertical unit vector), i.e. a rotation matrix between the world frame and a contact frame. \( \widehat{x} \) is the skew-symmetric matrix such that for any \( u \), \( \widehat{x} u = x \times u \) (cross product). \( C \) is a matrix expressing the linearized Coulomb friction cone (see e.g. here). The two equality constraints are the Newton-Euler equations where the moments are taken at the origin of the world frames. The forces \( f_i \) are expressed in the local contact frame and brought back in the world frame through the \( R_i \).

Note that

\( \frac{1}{2} \sum_i \left\| f_i - f_i^{des} \right\|^2 + \frac{w}{2} \sum_i \left\| c - p_i \right\|^2 = \frac{1}{2}\left\| \begin{bmatrix} f_1 - f_1^{des} \\ f_2 - f_2^{des} \\ \vdots \\ c - p_1 \\ c - p_2 \\ \vdots \end{bmatrix} \right\|^2_W \) where \( W \) is a diagonal matrix with 1 and w on the diagonal.

Implementation outline

Our main function will be leastSquares3points, and consist of 3 main steps

Declaration of the problem data and variables
Writing of the problem
Solving of the problem

We will use three helper functions

Matrix3d rotationFromZ(Vector3d v), which creates a rotation matrix R such that R*Vector3d::UnitZ() == v
Matrix3d hat(const Vector3d& v), which returns the skew- symmetric matrix \( \widehat{v} \) as described above
MatrixXd discretizedFrictionCone(double mu), which creates the matrix \( C \) for a four-sided linearized cone with friction coefficient mu.

We won't details these methods further.

Implementation details

Given a 3-dimensionnal space Space S(3), we create the following data for the first contact point:

  // Data for first contact point
  Vector3d p1(-1, -1, 0);                  // Contact point position
  Vector3d n1(1, 1, 1);                    // Normal vector to contact surface
  MatrixXd R1 = rotationFromZ(n1);         // Rotation between world frame and a local contact frame
  Vector3d f1des = Vector3d::Zero();       // Desired force for f1
  VariablePtr f1 = S.createVariable("f1"); // Variable creation

Here we chose to set the desired force to 0, which will have the effect of trying to minimize the force.

The same is done for the second and third contact points.

Next, we create the data about the weight and CoM:

  const double m = 1;                    // Mass
  const Vector3d g(0, 0, -9.81);         // Gravity vector
  VariablePtr c = S.createVariable("c"); // Center of mass variable

and define a matrix for the friction cone (we use the same 0.6 friction coefficient for all contacts):

MatrixXd C = discretizedFrictionCone(0.6); // Matrix of a discretized cone with friction coefficient 0.6

tvm::LinearizedControlProblem can be used to write linear least-squares problem, with a method add that forgoes the need for task dynamics. After creating an instance pb, we can populate it.

First we add the Newton-Euler equations as constraints (priority level 0):

  pb.add(R1 * f1 + R2 * f2 + R3 * f3 + m * g == 0., PriorityLevel(0)); // Newton equation
  pb.add(hat(p1) * R1 * f1 + hat(p2) * R2 * f2 + hat(p3) * R3 * f3 + m * hat(g) * c == 0.,
         PriorityLevel(0));               // Euler equation

Since these are linear expression, we can write them directly. TVM is providing, with the implicit use of the tvm::utils::AffineExpr class and operator overload, a way to write such expressions in a symbolic-like way.

Then the friction constraints can be added in the same way

  pb.add(C * f1 >= 0., PriorityLevel(0)); // Friction cone constraints
  pb.add(C * f2 >= 0., PriorityLevel(0));
  pb.add(C * f3 >= 0., PriorityLevel(0));

Finally the objectives are added (priority level 1):

  pb.add(f1 == f1des, PriorityLevel(1)); // Desired forces
  pb.add(f2 == f2des, PriorityLevel(1));
  pb.add(f3 == f3des, PriorityLevel(1));
  pb.add(c == p1, {PriorityLevel(1), Weight(1e-4)}); // Desired CoM position
  pb.add(c == p2, {PriorityLevel(1), Weight(1e-4)});
  pb.add(c == p3, {PriorityLevel(1), Weight(1e-4)});

We use the weight \( w = 10^{-4} \).

Note that, by default, constraints at any level are to be solved in the least- square sense, so that \( f_i = f_i^{des} \) becomes \( \left\| f_i - f_i^{des} \right\| \).

The last thing to do is to chose a resolution scheme and solve the problem:

  // Creating the resolution scheme
  scheme::WeightedLeastSquares solver(solver::DefaultLSSolverOptions().verbose(true));
 
  // And solving the problem
  bool found = solver.solve(pb);

Example file

example/LeastSquaresExample.cpp