lcp.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
16 #ifndef __LCP_H__
17 #define __LCP_H__
18 
19 #include <fMatrix.h>
20 #include <vector>
21 #include <assert.h>
22 
23 //#define PIVOT_MINIMUM_ONLY // only for test
24 //#define INVERSE_FROM_SCRATCH // turn off to compute inverse incrementally
25 #define ACTIVATE_SAME_STATE // activate same state check during planning
26 
32 class LCP
33 {
34 public:
36 
41  LCP(const fMat& _NN, const fVec& _r): N(_NN), r(_r) {
42  assert(N.row() == N.col());
43  assert(N.row() == r.size());
44  n_vars = N.col();
45  }
46  ~LCP() {
47  }
48 
50 
55  int Solve(fVec& g, fVec& a);
56 
58 
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);
69 
71 
80  int SolvePivot(fVec& g, fVec& a, double _max_error, int _max_iteration, int* n_iteration, std::vector<int>& _g2w);
81 
83 
92  int SolvePivot2(fVec& g, fVec& a, double _max_error, int _max_iteration, int* n_iteration, std::vector<int>& _g2w);
93 
94  static void Pivot(int idx1, int idx2,
95  const fMat& M, const fVec& q,
96  fMat& M_new, fVec& q_new);
97  static void Pivot(std::vector<int>& idx1, std::vector<int>& idx2,
98  const fMat& M, const fVec& q,
99  fMat& M_new, fVec& q_new);
100  static void Pivot(std::vector<int>& w2a, std::vector<int>& w2g,
101  std::vector<int>& z2a, std::vector<int>& z2g,
102  const fMat& M, const fVec& q,
103  fMat& M_new, fVec& q_new);
104  static void Pivot(const fMat& M, const fVec& q,
105  std::vector<int>& w2a, std::vector<int>& w2g,
106  std::vector<int>& z2a, std::vector<int>& z2g,
107  int new_jr,
108  fMat& m_jr, fVec& q_new);
109  static void Pivot(const fMat& M, const fVec& q,
110  const fMat& oldMinv,
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,
116  int jr,
117  fMat& newMinv, fMat& m_jr, fVec& q_new);
118 
119  double CheckPivotResult(const fVec& q_new, std::vector<int>& w2a, std::vector<int>& w2g);
120 
121  const fMat& Mref() {
122  return M;
123  }
124  const fVec& Qref() {
125  return q;
126  }
127 
128  int NumLoops();
129  int NumErrors();
130 
131 protected:
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);
133 
134  int check_q(const fVec& q, double max_error) {
135  for(int i=0; i<n_vars; i++)
136  {
137  if(q(i) < -max_error)
138  {
139  return false;
140  }
141  }
142  return true;
143  }
144 
147  int n_vars;
148 
151 };
152 
153 #endif
double CheckPivotResult(const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
Definition: lcp_pivot.cpp:2009
int NumErrors()
Definition: lcp_pivot.cpp:558
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
Definition: lcp_pivot.cpp:1321
int col() const
Returns the number of columns.
Definition: fMatrix.h:154
int check_q(const fVec &q, double max_error)
Definition: lcp.h:134
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)
Definition: lcp_pivot.cpp:1433
png_uint_32 i
Definition: png.h:2735
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.
Definition: lcp.cpp:27
Generic matrix/vector classes.
LCP(const fMat &_NN, const fVec &_r)
Constructor.
Definition: lcp.h:41
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke&#39;s method.
Definition: lcp_pivot.cpp:698
fVec r
Definition: lcp.h:146
int NumLoops()
Definition: lcp_pivot.cpp:555
string a
int Solve(fVec &g, fVec &a)
Solve using iterative method.
Definition: lcp.cpp:20
Vector of generic size.
Definition: fMatrix.h:491
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
Definition: lcp.h:32
int n_vars
Definition: lcp.h:147
fMat M
Definition: lcp.h:149
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.
Definition: lcp_pivot.cpp:562
const fVec & Qref()
Definition: lcp.h:124
fVec q
Definition: lcp.h:150
const fMat & Mref()
Definition: lcp.h:121
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
fMat N
Definition: lcp.h:145
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
~LCP()
Definition: lcp.h:46


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04