ChppGikSolverBasic Class Reference

This is a prioritized inverse kinematics solver. More...

#include <hpp/gik/core/solver-basic.hh>

List of all members.

Public Member Functions

 ChppGikSolverBasic (unsigned int numberParam)
 Constructor.
bool weights (vectorN &inWeights)
 Set the diagonal weights for the Weighted Pseudoinverse.
unsigned int resetSolution ()
 Clear solution.
bool setActiveParameters (const vectorN &inMask)
 Set active paramters for nest solved task.
bool solveTask (CjrlGikStateConstraint *inConstraint, double inSRcoef=0.0, bool projectJacobian=false, bool inComputeNullspace=false)
 solve one constraint in the current null space.
const vectorNsolution ()
 get the solution to last serie of calls of solveTask
void SVDThreshold (double i_threshold)
 Set the minimum singular value.
const vectorNpenroseMask () const
 Get Penrose mask vector (debug method)
 ~ChppGikSolverBasic ()
 Destructor.

Detailed Description

This is a prioritized inverse kinematics solver.

It can be used to solve a sequence of linear systems from highest to lowest priority. A linear system is provided by a CjrlGikStateConstraint object through its methods jacobian() and value(). These are assumed to be already computed.
Each linear system is solved by a pseudo inversion. The weighted pseudo inverse is used.


Constructor & Destructor Documentation

ChppGikSolverBasic::ChppGikSolverBasic ( unsigned int  numberParam)

Constructor.

Parameters:
numberParamis the size of the variables vector
ChppGikSolverBasic::~ChppGikSolverBasic ( )

Destructor.


Member Function Documentation

const vectorN& ChppGikSolverBasic::penroseMask ( ) const

Get Penrose mask vector (debug method)

Returns:
Penrose mask vector
unsigned int ChppGikSolverBasic::resetSolution ( )

Clear solution.

Reset nullspace and analyze weights to determine which parameters are being used and optimize computation accordingly. Call this prior to solving the first task of every stack of tasks.

Returns:
the number of non null weights found.
bool ChppGikSolverBasic::setActiveParameters ( const vectorN inMask)

Set active paramters for nest solved task.

Returns:
false if inMask of incorrect size
const vectorN& ChppGikSolverBasic::solution ( )

get the solution to last serie of calls of solveTask

Up to last resetSolution call

bool ChppGikSolverBasic::solveTask ( CjrlGikStateConstraint inConstraint,
double  inSRcoef = 0.0,
bool  projectJacobian = false,
bool  inComputeNullspace = false 
)

solve one constraint in the current null space.

The solution is added to ChppGikSolver::DeltaQ and ChppGikSolver::NullSpace is updated by the following procedure.

  1. get task value $v$ and jacobian $J$ from inConstraint
  2. compute residual task value: $ r \leftarrow v - J\delta q $
  3. compute projected jacobian: $ \hat{J} \leftarrow JN $
  4. singular value decomposition: $ S, U, V^t \leftarrow SVD(\hat{J}W\hat{J}^t) $
  5. compute inverse: $ (\hat{J}W\hat{J}^t)^{-1} \leftarrow US^{-1}V^t $
  6. compute pseudo inverse: $ \hat{J}^\# \leftarrow W\hat{J}^t(\hat{J}W\hat{J}^t)^{-1}$
  7. update solution: $ \delta q \leftarrow \delta q + \hat{J}^\#r $
  8. update null space: $ N \leftarrow N(I-\hat{J}^\#\hat{J}) $
    Parameters:
    inConstrainta constraint to be solved
    inSRcoefif non-zero value is given, $ (\hat{J}W\hat{J}^t)^{-1} $ is computed using SR-Inverse.
    projectJacobianset to true to project connstaint jacobian on nullspace
    inComputeNullspaceset to true to update null space projector
    Returns:
    false if task jacobian's size is not equal to number of parameters
void ChppGikSolverBasic::SVDThreshold ( double  i_threshold)

Set the minimum singular value.

If a singular value is smaller than this value, it is set to zero in the computation process of pseudo inverse.

Parameters:
i_thresholdthe minimum singular value
bool ChppGikSolverBasic::weights ( vectorN inWeights)

Set the diagonal weights for the Weighted Pseudoinverse.

This calls resetSolution() before returning true.

Returns:
false if inWeights of incorrect size