-
Notifications
You must be signed in to change notification settings - Fork 82
Home
Welcome to the invdyn wiki!
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
The main class of the library is InverseDynamicsFormulation, 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 the one 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.
The interface of the class AbstractConstraint is the following:
AbstractConstraint(string name);
virtual bool isEquality() = 0;
virtual bool isInequality() = 0;
virtual bool isBound() = 0;
virtual Matrix getMatrix() = 0;
virtual Vector getVector() = 0;
virtual Vector getLowerBound() = 0;
virtual Vector getUpperBound() = 0;
If the constraint is an equality (A x=b), then the method getMatrix returns A, and getVector returns b. If instead it is an inequality (lb <= A x <= ub), then the method getMatrix returns A, getLowerBound returns lb and getUpperBound returns ub. Finally, if the constraint is a bound (lb <= x <= ub), then the method getLowerBound returns lb and getUpperBound returns ub.
The interface of an AbstractTask is:
AbstractTask(string name, Model model);
virtual AbstractConstraint compute(double t, Vector q, Vector v, Data data, bool updateKinematics=False) = 0;
The interface of a RigidContact is:
RigidContact(string name, Vector Kp, Vector Kd, string bodyName, double regularizationTaskWeight);
virtual AbstractConstraint computeMotionTask(t, q, v, data, updateKinematics) = 0;
virtual InequalityConstraint computeForceTask(t, q, v, data, updateKinematics) = 0;
virtual Matrix computeForceGeneratorMatrix() = 0;
virtual AbstractConstraint computeForceRegularizationTask(t, q, v, data, updateKinematics) = 0;
This class allows to use different representation for the motion space and the contact 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.
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).
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.
The interface of an Hqp solver is:
HqpOutput solve(HqpData data)
where HqpData is defined as:
#typedef vector<pair<double, AbstractConstraint>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
In words, an HqpData is a list of ConstraintLevel, each of which is a list of AbstractConstraint and the associated weights. HqpOutput is defined as:
class HqpOutput
{
QpStatusFlag flag;
Vector x, lambda;
}