So you would like to:
Let and
. Solving linear system
in
consists in finding a solution vector
such as
When the linear system is underconstrained, i.e , an infinity of solutions exist. The general form of the solution can be written as:
where:
This defines an affine subspace of solutions.
The point can be the minimal norm solution of the system for which we have an analytical expression in case matrix A is of plain rank:
This particular solution is usually called the pseudoinverse or minimal norm solution. There exists a weighted pseudoinverse solution, corresponding to minimization of where
is a positive semi definite matrix. The particular solution is:
can be used for a variety of effects, like priveledging certain parameters, disactivating others, etc.
When A is not of plain rank or is nearly singular, the matrice product is singular of nearly singular. A solution that minimizes the cost
can be computed by inverting
while canceling contribution of low singular values:
where is the function that enables a singular value
for inversion if larger that a given threshold and cancels it otherwise.
It is also possible to "damp" the pseudo inverse solution by choosing to compute:
where:
This is equivalent to expanding the initial linear system with the following:
which tends to attract the solution vector to the null vector, depending on the value of
.
The linear system solver in hppGik library is called ChppGikSolverBasic. The constructor ChppGikSolverBasic::ChppGikSolverBasic() requires the space dimension .
To construct a ChppGikSolverBasic object:
ChppGikSolverBasic solver(n);
A linear system is provided as an object of class CjrlGikStateConstraint. The method CjrlGikStateConstraint::jacobian() should return matrix and the method CjrlGikStateConstraint::value() should return vector
. Let
constraint
be an object of type CjrlGikStateConstraint. To compute the pseudoinverse solution of the linear system implied by constraint
do:
solver.solveTask(constraint); solution = solver.solution();
For the weighted pseudoinverse solution, use:
solver.weights(weights); solver.solveTask(constraint); solution = solver.solution();
Note that only diagonal weight matrix W is implemented. The argument of method ChppGikSolverBasic::weights() is a vector of size used to fill the diagonal of
.
The default method for dealing with singularity is the SVD thresholding described above. The threshold can be modified through access methods ChppGikSolverBasic::SVDThreshold().
For damping method, use:
solver.solveTask(constraint, lambda); solution = solver.solution();
where parameter lambda stands for damping factor .
Any of the previous code can be used to solve a single linear system. To use the same solver object to solve another independant linear system, call ChppGikSolverBasic::resetSolution() or ChppGikSolverBasic::weights() prior to calling ChppGikSolverBasic::solveTask().
ChppGikSolverBasic is also capable of solving a series of linear systems given in decreasing priority order. Next section will present an overview of the theory behind the algorithm, followed by implementation usage info.
Let and
denote two linear systems. As seen in the previous section, the general form of
is:
We have the analytical expression for :
Solving in second priority means to compute a solution to
that is within the solutions of
. Therefore
is modified to take into account
priority and becomes:
This system is solved in the same fashion as to obtain a solution point
. The new general form of solution to prioritized systems
is :
Because might become singular when projected on
, the methods used to deal with singular cases presented in previous section can still be used here. Then the point
would not strictly be solution to
, but a least-square- error solution.
ChppGikSolverBasic implements prioritized linear system resolution. The systems must be presented one at a time, the solution vector is incremented after each system resolution and the nullspace is updated.
To construct a ChppGikSolverBasic object:
ChppGikSolverBasic solver(n);
Let constraint1
, constraint2
and constraint3
be 3 CjrlGikStateConstraint objects. To solve the stack of prioritized systems do:
solver.resetSolution(); solver.solveTask(constraint1, 0.0, false, true); solver.solveTask(constraint2, 0.0, true, true); solver.solveTask(constraint3, 0.0, true, false); solution = solver.solution()
Second argument ChppGikSolverBasic::solveTask() is the damping factor which can be any positive double value (like in previous section). Argument 3 tells the method whether to project the linear system's matrix with the previous nullspace projector or not. Since
constraint1
is the first constraint in the stack, there is no need to do so, hence value false
. Argument 4 tells the method wether to compute the null space projector or not. It is set to
true
here since it will be used to compute constraint2
's solution. The exact opposite thing happens for last linear system constraint3
.
In sum, argument 3 an 4 are options to optimize first and last system computation.
ChppGikSolverBasic::setActiveParameters can be used before each ChppGikSolverBasic::solveTask() call to specify the columns of linear system matrix to be used. This can give some computation speedup.
ChppGikSolverBasic::weights() can replace ChppGikSolverBasic::resetSolution() in case the weights for the weighted pseudoinverse should be changed. The user can set zero weights for some parameters (for instance when they are not used), leading to faster computation.
Give a look to Solve a linear system first. So you have a robot, formed of several articulated bodies, for which you want to compute joint motion in order to realize a certain task. Most of the time, the task is naturally expressed in the cartesian space, like position and/or orientation of a body in the robot. The non-linear realationship between body transformations and joint angles is most of the time not invertible, especially when the number of degrees of freedom outnumbers the task dimension. This is where numerical resolution schemes come handy. Here is an outline:
WHILE task not achieved DO: (1) Compute value (vector) of task (2) Compute gradient (jacobian) of task (3) Follow task gradient descent to compute a slight change in values of joints (4) IF no change in joint values QUIT (5) Apply new joint value to robot.
Let be the vector of joint values and let
be the position of point
attached to a body in the robot and expressed in a frame
independant of robot motion (for simplicity's sake). Suppose we want point
to reach a point whose position is
in frame
. The above code becomes:
WHILE P(q) - T(q) != 0 DO: (1) Value V = eval(T(q) - P(q)) (2) Jacobian J = d (P - T) / d q (3) Slight change dq = r * pseudoinverse(J) * V (4) IF dq = 0 QUIT (5) q = q + dq
where r
is a positive real used to scale the value.
The joint update computed at step (3) is the solution to the linear system:
If r
is controlled properly through iterations, the above algorithm yields a smooth motion. The achievement of position is not guaranteed because the linear system might become singular after a few iterations, even from start. In that case the algorithm is said to be trapped in a local minimum and it cannot achieve any better.
Given the tasks, the user should implement CjrlGikStateConstraint objects that compute the jacobians and values correponding to the respective tasks. To solve the linear systems implied by step (3) ChppGikSolverBasic can be used.
However, a problem arises for a robot: joint limits. Joint limits are inequality constraints on the joint values that must constantly be verified. ChppGikSolver is a prioritized linear system solver based on ChppGikSolverBasic, it uses a ChppGikBounder object to enforce joint limits. See these objects' documentation for more details.
First construct a solver and pile up CjrlGikStateConstraint objects in a priority-decreasing vector:
ChppGikSolver solver(robot); std::vector<CjrlGikStateConstraint*> stack; stack.push_back(highest_priority_constraint); stack.push_back(mid_priority_constraint); stack.push_back(low_priority_constraint);
This code corresponds to a single iteration of the above algorithm (but with several prioritized tasks):
//Steps (1) and (2) for (unsigned int i = 0; i< stack.size();i++) { stack[i]->computeValue(); stack[i]->computeJacobian(); } //Step (3) solver.solve( stack ); //Step (4) if (norm_2(solver.solution()) < 1e-5) return; //Step (5) //Compute new configuration/velocity/acceleration vectors of robot and apply them
There is a variation of ChppGikSolver::solve() to add damping factors. See documentation for details.
See Example I for a source code using ChppGikSolver.
This section is merely about drawing the user's attention to humanoid's stability issue.
As a matter of fact, to reach something with the right hand, and supposing the humanoid is standing upon a flat ground, it needs to move while keeping its Zero Momentum Point inside its support polygon. There is NOTHING implemented in this library that provides such a guarantee. However, if the planned motion is smooth enough (does not necessarily mean slow), then keeping the center of mass's projection inside the support polygon should be enough to achieve a stable reaching motion.
See Example I for a code where center of mass and feet constraints are added to maintain stability while reaching.
Well, this is no joke, you will be able to compute all kinds of motion just reading this section. At the expense of flexibility, of course. The API shown in previous sections would be "low-level" compared to the one shown here.
Attention is focused on ChppGikGenericTask class. Objects of this class define a time scale on which the user schedules finite-time motions.
A motion is either a ChppGikPrioritizedMotion or a ChppGikLocomotionElement.
ChppGikPrioritizedMotion motions are defined by a desired constraint of class ChppGikVectorizableConstraint, a time frame (i.e a start time and end time), a priority integer (the higher the integer the lower the priority) and a "mask vector" that defines active and inactive joints for the accomplishment of the motion.
ChppGikLocomotionElement are special motions that affect feet and center of mass. ChppGikStepElement is an example of ChppGikLocomotionElement that an be used to plan stepping motion for the humanoid robot.
ChppGikGenericTask object takes care of stability of the robot. For example, to move the right hand from its current position to a given position, the user need not specify feet and center of mass tasks.
The output of ChppGikGenericTask is a stable ChppRobotMotion.
Further details can be found in ChppGikGenericTask documentation. Let us just give usage outline :
Prepare a robot and a generic task:
//Supposing object "robot" is of class CjrlHumanoidDynamicRobot, and is in double support configuration: ChppGikStandingRobot standingRobot(robot); double samplingPeriod = 5e-3; ChppGikGenericTask genericTask(standingRobot, samplingPeriod);
Schedule prioritized motion and locomotion objects in generic task:
genericTask.addElement(motion1); genericTask.addElement(step1);
Solve and retrieve solution if successful:
bool ok = genericTask.solve(); if (ok) motion = genericTask.solutionMotion();
Have a look at Dedicated Tasks for ready-to-use motion planners built upon ChppGikGenericTask.
Perfectly understandable.