Go to the documentation of this file.
25 #define ACTIVATE_SAME_STATE // activate same state check during planning
68 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);
80 int SolvePivot(
fVec& g,
fVec&
a,
double _max_error,
int _max_iteration,
int* n_iteration, std::vector<int>& _g2w);
92 int SolvePivot2(
fVec& g,
fVec&
a,
double _max_error,
int _max_iteration,
int* n_iteration, std::vector<int>& _g2w);
94 static void Pivot(
int idx1,
int idx2,
97 static void Pivot(std::vector<int>& idx1, std::vector<int>& idx2,
100 static void Pivot(std::vector<int>& w2a, std::vector<int>& w2g,
101 std::vector<int>& z2a, std::vector<int>& z2g,
105 std::vector<int>& w2a, std::vector<int>& w2g,
106 std::vector<int>& z2a, std::vector<int>& z2g,
111 std::vector<int>& old_w2a, std::vector<int>& old_w2g,
112 std::vector<int>& old_z2a, std::vector<int>& old_z2g,
113 int ys2a,
int ys2g,
int yr2a,
int yr2g,
114 std::vector<int>& w2a, std::vector<int>& w2g,
115 std::vector<int>& z2a, std::vector<int>& z2g,
132 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);
137 if(
q(
i) < -max_error)
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke's method.
LCP(const fMat &_NN, const fVec &_r)
Constructor.
double CheckPivotResult(const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
int size() const
Size of the vector (same as row()).
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.
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.
int check_q(const fVec &q, double max_error)
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
int row() const
Returns the number of rows.
int col() const
Returns the number of columns.
Generic matrix/vector classes.
int Solve(fVec &g, fVec &a)
Solve using iterative method.
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)
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03