Public Member Functions | Static Public Member Functions | Protected Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
LCP Class Reference

#include <lcp.h>

Public Member Functions

double CheckPivotResult (const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
 
 LCP (const fMat &_NN, const fVec &_r)
 Constructor. More...
 
const fMatMref ()
 
int NumErrors ()
 
int NumLoops ()
 
const fVecQref ()
 
int Solve (fVec &g, fVec &a)
 Solve using iterative method. More...
 
int SolveEx (fVec &g, fVec &a, const fVec &g_init, double _max_error=1e-8, int _max_iteration=100, double _speed=0.5, int *n_iteration=0)
 Solve using iterative method. More...
 
int SolvePivot (fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
 Solve using Lemke's method. More...
 
int SolvePivot2 (fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
 Solve by pivot using path planning algorithm. More...
 
 ~LCP ()
 

Static Public Member Functions

static void Pivot (int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
 
static void Pivot (std::vector< int > &idx1, std::vector< int > &idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
 
static void Pivot (std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
 
static void Pivot (const fMat &M, const fVec &q, std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, int new_jr, fMat &m_jr, fVec &q_new)
 
static void Pivot (const fMat &M, const fVec &q, const fMat &oldMinv, std::vector< int > &old_w2a, std::vector< int > &old_w2g, std::vector< int > &old_z2a, std::vector< int > &old_z2g, int ys2a, int ys2g, int yr2a, int yr2g, std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, int jr, fMat &newMinv, fMat &m_jr, fVec &q_new)
 

Protected Member Functions

int check_q (const fVec &q, double max_error)
 

Static Protected Member Functions

static void pivot_body (const fMat &M12, const fMat &M1, const fMat &M2, const fMat &M12bar, const fVec &q1, const fVec &q1bar, fMat &Md12, fMat &Md1, fMat &Md2, fMat &Md12bar, fVec &qd1, fVec &qd1bar)
 

Protected Attributes

fMat M
 
fMat N
 
int n_vars
 
fVec q
 
fVec r
 

Detailed Description

Solves a linear complementarity problem (LCP): find g and a that satisfy N*g + r = a, a>=0, g>=0, a^T*g = 0.

Definition at line 32 of file lcp.h.

Constructor & Destructor Documentation

LCP::LCP ( const fMat _NN,
const fVec _r 
)
inline

Constructor.

Constructor.

Parameters
[in]_NNN matrix
[in]_rr vector

Definition at line 41 of file lcp.h.

LCP::~LCP ( )
inline

Definition at line 46 of file lcp.h.

Member Function Documentation

int LCP::check_q ( const fVec q,
double  max_error 
)
inlineprotected

Definition at line 134 of file lcp.h.

double LCP::CheckPivotResult ( const fVec q_new,
std::vector< int > &  w2a,
std::vector< int > &  w2g 
)

Definition at line 2009 of file lcp_pivot.cpp.

const fMat& LCP::Mref ( )
inline

Definition at line 121 of file lcp.h.

int LCP::NumErrors ( )

Definition at line 558 of file lcp_pivot.cpp.

int LCP::NumLoops ( )

Definition at line 555 of file lcp_pivot.cpp.

void LCP::Pivot ( int  idx1,
int  idx2,
const fMat M,
const fVec q,
fMat M_new,
fVec q_new 
)
static

Definition at line 1321 of file lcp_pivot.cpp.

void LCP::Pivot ( std::vector< int > &  idx1,
std::vector< int > &  idx2,
const fMat M,
const fVec q,
fMat M_new,
fVec q_new 
)
static

Definition at line 1196 of file lcp_pivot.cpp.

void LCP::Pivot ( std::vector< int > &  w2a,
std::vector< int > &  w2g,
std::vector< int > &  z2a,
std::vector< int > &  z2g,
const fMat M,
const fVec q,
fMat M_new,
fVec q_new 
)
static

Definition at line 1063 of file lcp_pivot.cpp.

void LCP::Pivot ( const fMat M,
const fVec q,
std::vector< int > &  w2a,
std::vector< int > &  w2g,
std::vector< int > &  z2a,
std::vector< int > &  z2g,
int  new_jr,
fMat m_jr,
fVec q_new 
)
static

Definition at line 1866 of file lcp_pivot.cpp.

void LCP::Pivot ( const fMat M,
const fVec q,
const fMat oldMinv,
std::vector< int > &  old_w2a,
std::vector< int > &  old_w2g,
std::vector< int > &  old_z2a,
std::vector< int > &  old_z2g,
int  ys2a,
int  ys2g,
int  yr2a,
int  yr2g,
std::vector< int > &  w2a,
std::vector< int > &  w2g,
std::vector< int > &  z2a,
std::vector< int > &  z2g,
int  jr,
fMat newMinv,
fMat m_jr,
fVec q_new 
)
static

Definition at line 1481 of file lcp_pivot.cpp.

void LCP::pivot_body ( const fMat M12,
const fMat M1,
const fMat M2,
const fMat M12bar,
const fVec q1,
const fVec q1bar,
fMat Md12,
fMat Md1,
fMat Md2,
fMat Md12bar,
fVec qd1,
fVec qd1bar 
)
staticprotected

Definition at line 1433 of file lcp_pivot.cpp.

const fVec& LCP::Qref ( )
inline

Definition at line 124 of file lcp.h.

int LCP::Solve ( fVec g,
fVec a 
)

Solve using iterative method.

Solve using iterative method.

Parameters
[out]gthe solution (non-basic variables)
[out]athe solution (basic variables)

Definition at line 20 of file lcp.cpp.

int LCP::SolveEx ( fVec g,
fVec a,
const fVec g_init,
double  _max_error = 1e-8,
int  _max_iteration = 100,
double  _speed = 0.5,
int n_iteration = 0 
)

Solve using iterative method.

Solve using iterative method.

Parameters
[out]gthe solution (non-basic variables)
[out]athe solution (basic variables)
[in]g_initthe initial value for g
[in]_max_errormaximum permissible error
[in]_max_iterationmaximum number of iterations
[in]_speedconvergence speed
[out]n_iterationactual number of iterations

Definition at line 27 of file lcp.cpp.

int LCP::SolvePivot ( fVec g,
fVec a,
double  _max_error,
int  _max_iteration,
int n_iteration,
std::vector< int > &  _g2w 
)

Solve using Lemke's method.

Solve using Lemke's method.

Parameters
[out]gthe solution (non-basic variables)
[out]athe solution (basic variables)
[in]_max_errormaximum permissible error
[in]_max_iterationmaximum number of iteration
[out]n_iterationactual number of iterations
[out]_g2wresulting pivot

Definition at line 698 of file lcp_pivot.cpp.

int LCP::SolvePivot2 ( fVec g,
fVec a,
double  _max_error,
int  _max_iteration,
int n_iteration,
std::vector< int > &  _g2w 
)

Solve by pivot using path planning algorithm.

Solve by pivot using path planning algorithm.

Parameters
[out]gthe solution (non-basic variables)
[out]athe solution (basic variables)
[in]_max_errormaximum permissible error
[in]_max_iterationmaximum number of iteration
[out]n_iterationactual number of iterations
[out]_g2wresulting pivot

Definition at line 562 of file lcp_pivot.cpp.

Member Data Documentation

fMat LCP::M
protected

Definition at line 149 of file lcp.h.

fMat LCP::N
protected

Definition at line 145 of file lcp.h.

int LCP::n_vars
protected

Definition at line 147 of file lcp.h.

fVec LCP::q
protected

Definition at line 150 of file lcp.h.

fVec LCP::r
protected

Definition at line 146 of file lcp.h.


The documentation for this class was generated from the following files:


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:43