Skip to content
Andrea Del Prete edited this page Jan 30, 2019 · 11 revisions

Welcome to the TSID wiki!

Here you can find the slides of a class about TSID that has been given in January 2019 (covering both theory and implementation using this library).

Library Design

This document tries to justify the design choice behind this library. The goal of this library is to create inverse-dynamics controllers for legged robots. We suppose that an inverse-dynamics control algorithm always boils down to solving a Hierarchical Least-Squares Program (HLSP), that can be seen as a cascade of standard Least-Squares Programs (LSP). A LSP is a special case of Quadratic Program (QP), in which the cost function is the 2-norm of a linear function, and the equality/inequality constraints are linear (in this document the term linear is used loosely to mean also affine). The key concepts of this library are:

  • Constraint: a linear equality or inequality; a HLSP is defined as a collection of constraints with difference priority levels and weights.
  • Task: a control objective for the robot, which is used at each control cycle to generate a constraint. Note that constraints are purely mathematical objects that are independent of the concept of robot, while Tasks are instead robot-related objects.
  • Rigid Contact: a description of a rigid contact between a body of the robot and another object; the main difference between a task and a rigid contact is that a rigid contact is associated to reaction forces, while a task is not.
  • Trajectory: a multi-dimensional function of time describing the motion of an object and its time derivatives
  • QpSolver: an algorithm capable of solving a hierarchical QP

Problem Formulation

The main class of the library is InverseDynamicsFormulationBase, which offers the following interface:

bool addMotionTask(MotionTask task, double weight, int priorityLevel, double transition_duration=0.0);
bool addForceTask(ForceTask task, double weight, int priorityLevel, double transition_duration=0.0);
bool addTorqueTask(TorqueTask task, double weight, int priorityLevel, double transition_duration=0.0);        
bool addRigidContact(AbstractRigidContact contact);
bool removeTask(string task_name, double transition_duration=0.0);
bool removeRigidContact(string contact_name, double transition_duration=0.0);
virtual HqpData computeProblemData(double time, Vector q, Vector v) = 0;

The method computeProblemData needs to be implemented by child classes, and it will depend on the chosen inverse-dynamics formulation. Its output is a data structure representing an HQP. At the moment, the only inverse-dynamics formulation implemented is InverseDynamicsFormulationAccForce, which is described in "Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics". This formulation uses the robot accelerations (base+joints) and the contact forces as decision variables. The joint torques are removed from the problem to reduce the number of decision variables, and so make computation faster. Note that this does not affect the expressiveness of the problem though, because one can still express the joint torques as linear functions of accelerations and forces.

Constraint

The interface of the class ConstraintBase is the following:

ConstraintBase(string name);
virtual bool isEquality() = 0;
virtual bool isInequality() = 0;
virtual bool isBound() = 0;
virtual Matrix matrix() = 0;
virtual Vector vector() = 0;
virtual Vector lowerBound() = 0;
virtual Vector upperBound() = 0;

If the constraint is an equality (A x=b), then the method matrix returns A, and vector returns b. If instead it is an inequality (lb <= A x <= ub), then the method matrix returns A, lowerBound returns lb and upperBound returns ub. Finally, if the constraint is a bound (lb <= x <= ub), then the method lowerBound returns lb and upperBound returns ub.

Task

The interface of an TaskBase is:

TaskBase(string name, Model model);
virtual AbstractConstraint compute(double t, Vector q, Vector v, Data data, bool updateKinematics=False) = 0;    

The class TaskBase is the parent of all classes representing a task. Three kinds of task exist:

  • TaskMotion: computes a constraint that is a linear function of the robot accelerations
  • TaskContactForce: computes a constraint that is a linear function of the contact forces
  • TaskActuation: computes a constraint that is a linear function of the joint torques

Tasks can compute either equality constraints, or bounds, or inequality constraints. For instance, the task TaskComEquality computes an equality constraint to specify a desired acceleration of the center of mass (CoM) of the robot. Similarly, the task TaskJointPosture specifies the desired joint accelerations, and the task TaskSE3Equality specifies the desired acceleration for a frame attached to one of the links of the robot. The task TaskJointBounds (not implemented yet) would specify instead the joint acceleration bounds in order to satisfy the joint position/velocity/acceleration limits.

Rigid Contact

The interface of a ContactBase is:

ContactBase(string name, Vector Kp, Vector Kd, string bodyName, double regularizationTaskWeight);
virtual ConstraintBase computeMotionTask(t, q, v, data, updateKinematics) = 0;
virtual InequalityConstraint computeForceTask(t, q, v, data, updateKinematics) = 0;
virtual Matrix computeForceGeneratorMatrix() = 0;
virtual ConstraintBase computeForceRegularizationTask(t, q, v, data, updateKinematics) = 0;

This class allows to use different representations for the motion space and the force space. For instance, in case of a unilateral plane contact (with polygonal shape), the motion constraint is 6d, because the body in contact cannot move in any direction. The minimal representation for the reaction force would then be 6d as well (i.e. a 3d force and a 3d moment). However, it may be hard to write the friction cone constraints using this representation, especially if the shape of the contact is irregular (i.e. it is not a rectangle). It would be much easier to write the friction constraints by representing the reaction force as a collection of 3d forces applied at the vertices of the (polygonal) contact surface. However this representation is redundant, e.g., in case of a 4-vertex contact surface you would have 12 force variables rather than 6. This redundancy is not an issue for the force variables, but it may be an issue for the motion constraint. Indeed, representing the 6d motion constraint as 4 constraints of dimension 3 (one for each surface vertex) could result in numerical issues if the solver does not handle redundant equality constraints (which is the case for the solver eigQuadProg). This is why we introduced the concept of force-generator matrix, which is a matrix mapping the chosen force variables to the minimal force representation. For instance, in the example above the force-generator matrix would map the 12d force variables into a 6d force.
The method computeForceRegularizationTask is thought for adding a task to regularize the contact forces (for instance to keep them close to the center of the friction cone).

At the moment, only one kind of contact constraint is implemented, which is Contact6d.

Trajectory

The interface of a Trajectory is:

Trajectory(string name, double t_init, double dt)
TrajectorySample operator()(double time)
TrajectorySample compute_next()

where TrajectorySample is defined as:

class TrajectorySample
{
    Vector pos, vel, acc;
}

The operator () can be used to access the value of a trajectory at an arbitrary time. However, for standard use in control, the more efficient version compute_next is provided, which just has to compute the value of the trajectory at the next time step. Note that position and velocity vectors may have different sizes.

Hqp Solver

The interface of SolverHQPBase is:

virtual void resize(unsigned int n, unsigned int neq, unsigned int nin) = 0;
virtual HqpOutput solve(HqpData data) = 0;

where HqpData is defined as:

#typedef vector<pair<double, AbstractConstraint>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData

In words, a HqpData is a list of ConstraintLevel's, each of which is a list of AbstractConstraint's and the associated weights. HqpOutput is defined as:

class HqpOutput
{
     QpStatusFlag flag;
     Vector x, lambda;
}

At the moment, several solvers are implemented, but none of them support a hierarchy. This means that the HQP problems can only have two hierarchy levels. The constraints in the highest priority level are treated as hard constraint, whereas those in the lowest priority level are treated as a cost to minimize. All the solvers implemented are based on the standard QP solver EiQuadProg, a modified version of uQuadProg++, working with Eigen data structures. In order to improve computation times, two new versions of this solver have been developed:

  • EiquadprogFast: an optimized version of the solver, working with dynamic matrix sizes (but all dynamic memory allocation is performed when resizing the problem)
  • RtEiquadprog: an optimized version of the solver, working with matrix with size known at compile time

You can look at this issue for more details about the solvers' performance.

Clone this wiki locally