lcp.cpp
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 #include "lcp.h"
17 #include <limits>
18 #include <list>
19 
20 int LCP::Solve(fVec& _g, fVec& _a)
21 {
22  fVec g_init(n_vars);
23  g_init.zero();
24  return SolveEx(_g, _a, g_init);
25 }
26 
27 int LCP::SolveEx(fVec& _g, fVec& _a, const fVec& _g_init, double _max_error, int _max_iteration, double _speed, int* n_iteration)
28 {
29  _g.resize(n_vars);
30  _a.resize(n_vars);
31  _g.set(_g_init);
32  int failed = true, count = 0;;
33  while(_max_iteration < 0 || count < _max_iteration)
34  {
35  for(int i=0; i<n_vars; i++)
36  {
37  double a = 0.0;
38  for(int j=0; j<n_vars; j++)
39  {
40  a += N(i, j) * _g(j);
41  }
42  double z = _g(i) - _speed*(r(i)+a)/N(i, i);
43  _g(i) = (z >= 0.0 ? z : 0.0);
44  }
45  _a.mul(N, _g);
46  _a += r;
47  if(_g.min_value() > -_max_error && _a.min_value() > -_max_error)
48  {
49  failed = false;
50  break;
51  }
52  count++;
53  }
54  _a.mul(N, _g);
55  _a += r;
56  if(n_iteration) *n_iteration = count;
57  return failed;
58 }
59 
double min_value()
Returns the minimum value.
Definition: fMatrix.cpp:1494
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
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
fVec r
Definition: lcp.h:146
Solves a Linear Complementarity Problem (LCP)
string a
int Solve(fVec &g, fVec &a)
Solve using iterative method.
Definition: lcp.cpp:20
void resize(int i)
Change the size.
Definition: fMatrix.h:511
Vector of generic size.
Definition: fMatrix.h:491
int n_vars
Definition: lcp.h:147
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
fMat N
Definition: lcp.h:145


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