lcp.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #include "lcp.h"
00017 #include <limits>
00018 #include <list>
00019 
00020 int LCP::Solve(fVec& _g, fVec& _a)
00021 {
00022         fVec g_init(n_vars);
00023         g_init.zero();
00024         return SolveEx(_g, _a, g_init);
00025 }
00026 
00027 int LCP::SolveEx(fVec& _g, fVec& _a, const fVec& _g_init, double _max_error, int _max_iteration, double _speed, int* n_iteration)
00028 {
00029         _g.resize(n_vars);
00030         _a.resize(n_vars);
00031         _g.set(_g_init);
00032         int failed = true, count = 0;;
00033         while(_max_iteration < 0 || count < _max_iteration)
00034         {
00035                 for(int i=0; i<n_vars; i++)
00036                 {
00037                         double a = 0.0;
00038                         for(int j=0; j<n_vars; j++)
00039                         {
00040                                 a += N(i, j) * _g(j);
00041                         }
00042                         double z = _g(i) - _speed*(r(i)+a)/N(i, i);
00043                         _g(i) = (z >= 0.0 ? z : 0.0);
00044                 }
00045                 _a.mul(N, _g);
00046                 _a += r;
00047                 if(_g.min_value() > -_max_error && _a.min_value() > -_max_error)
00048                 {
00049                         failed = false;
00050                         break;
00051                 }
00052                 count++;
00053         }
00054         _a.mul(N, _g);
00055         _a += r;
00056         if(n_iteration) *n_iteration = count;
00057         return failed;
00058 }
00059 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55