iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
136 #include <cstdlib>
137 #include <memory>
138 #include <iostream>
139 #include <iomanip>
140 #include <string>
141 #include <sstream>
142 
143 #include <yarp/os/LogStream.h>
144 #include <yarp/os/Searchable.h>
145 #include <yarp/os/Property.h>
146 #include <yarp/os/Value.h>
147 #include <yarp/os/Bottle.h>
148 #include <yarp/os/Network.h>
149 #include <yarp/os/ResourceFinder.h>
150 #include <yarp/os/RFModule.h>
151 #include <yarp/sig/Vector.h>
152 #include <yarp/sig/Matrix.h>
153 #include <yarp/math/Math.h>
154 
155 #include <iCub/iKin/iKinSlv.h>
156 
157 using namespace std;
158 using namespace yarp::os;
159 using namespace yarp::sig;
160 using namespace yarp::math;
161 using namespace iCub::iKin;
162 
163 static string pathToCustomKinFile;
164 
165 
166 /************************************************************************/
168  protected:
169  bool configured{false};
170  iKinChain* chain{nullptr};
171  Matrix C_orig;
172  Vector lB_orig;
173  Vector uB_orig;
174 
175  /************************************************************************/
176  void clone(const iKinLinIneqConstr* obj)override {
177  iKinLinIneqConstr::clone(obj);
178 
179  const auto* ptr = static_cast<const GenericLinIneqConstr*>(obj);
180  configured = ptr->configured;
181  chain = ptr->chain;
182  C_orig = ptr->C_orig;
183  lB_orig = ptr->lB_orig;
184  uB_orig = ptr->uB_orig;
185  }
186 
187  public:
188  /************************************************************************/
189  GenericLinIneqConstr(iKinChain* chain_, Searchable& options) :
190  chain(chain_), C_orig(0, chain_->getN()), iKinLinIneqConstr() {
191  if (options.check("LIC_num")) {
192  auto LIC_num = options.find("LIC_num").asInt32();
193  if (LIC_num > 0) {
194  for (auto i = 0; i < LIC_num; i++) {
195  ostringstream tag;
196  tag << "LIC_" << i;
197  Bottle& group = options.findGroup(tag.str());
198  if (group.isNull()) {
199  yError() << "Unable to find group \"" << tag.str() << "\"";
200  return;
201  }
202  Bottle* row = group.find("C").asList();
203  if (row == nullptr) {
204  yError() << "Unable to find option \"C\" for group \"" << tag.str() << "\"";
205  return;
206  }
207  if (row->size() != chain->getN()) {
208  yError() << "Option \"C\" of group \"" << tag.str() << "\" has wrong size";
209  return;
210  }
211  C_orig = pile(C_orig, zeros(chain->getN()));
212  for (auto i = 0; i < row->size(); i++) {
213  C_orig(C_orig.rows() - 1, i) = row->get(i).asFloat64();
214  }
215  lB_orig = cat(lB_orig, group.check("lB", Value(lowerBoundInf)).asFloat64());
216  uB_orig = cat(uB_orig, group.check("uB", Value(upperBoundInf)).asFloat64());
217  }
218 
219  yInfo() << "Detected generic linear inequalities constraints";
220  configured = true;
221  update(nullptr);
222  }
223  }
224  }
225 
226  /************************************************************************/
227  void update(void*)override {
228  setActive(false);
229  if (configured) {
230  getC().resize(C_orig.rows(), 0);
231  getlB() = lB_orig;
232  getuB() = uB_orig;
233  for (auto i = 0U; i < chain->getN(); i++) {
234  if (chain->isLinkBlocked(i)) {
235  auto v = chain->getAng(i) * C_orig.getCol(i);
236  getlB() -= v;
237  getuB() -= v;
238  } else {
239  getC() = cat(getC(), C_orig.getCol(i));
240  }
241  }
242  setActive(true);
243  }
244  }
245 };
246 
247 
248 /************************************************************************/
250  shared_ptr<iKinLimb> limb{nullptr};
251  shared_ptr<GenericLinIneqConstr> cns{nullptr};
252  shared_ptr<PartDescriptor> desc{nullptr};
253 
254  protected:
255  /************************************************************************/
256  PartDescriptor* getPartDesc(Searchable& options) {
257  if (!options.check("robot")) {
258  yError() << "\"robot\" option is missing!";
259  return nullptr;
260  }
261 
262  if (!options.check("NumberOfDrivers")) {
263  yError() << "\"NumberOfDrivers\" option is missing!";
264  return nullptr;
265  }
266 
267  string robot = options.find("robot").asString();
268  yInfo() << "Configuring solver for " << robot << " ...";
269 
270  Property linksOptions;
271  linksOptions.fromConfigFile(pathToCustomKinFile);
272  limb = shared_ptr<iKinLimb>(new iKinLimb(linksOptions));
273  if (!limb->isValid()) {
274  yError() << "invalid links parameters!";
275  return nullptr;
276  }
277 
278  cns = shared_ptr<GenericLinIneqConstr>(new GenericLinIneqConstr(limb->asChain(), options));
279  desc = shared_ptr<PartDescriptor>(new PartDescriptor);
280  desc->lmb = limb.get();
281  desc->chn = limb->asChain();
282  desc->cns = cns.get();
283 
284  bool failure = false;
285  desc->num = options.find("NumberOfDrivers").asInt32();
286  for (int cnt = 0; cnt < desc->num; cnt++) {
287  ostringstream str;
288  str << "driver_" << cnt;
289  Bottle& driver = options.findGroup(str.str());
290  if (driver.isNull()) {
291  yError() << "\"" << str.str() << "\" option is missing!";
292  failure = true;
293  break;
294  }
295 
296  if (!driver.check("Key")) {
297  yError() << "\"Key\" option is missing!";
298  failure = true;
299  break;
300  }
301 
302  if (!driver.check("JointsOrder")) {
303  yError() << "\"JointsOrder\" option is missing!";
304  failure = true;
305  break;
306  }
307 
308  string part = driver.find("Key").asString();
309  bool directOrder = (driver.find("JointsOrder").asString() == "direct");
310 
311  Property optPart;
312  optPart.put("device", "remote_controlboard");
313  optPart.put("remote", "/" + robot + "/" + part);
314  optPart.put("local", "/" + slvName + "/" + part);
315  optPart.put("robot", robot);
316  optPart.put("part", part);
317  desc->prp.push_back(optPart);
318  desc->rvs.push_back(!directOrder);
319  }
320 
321  return (failure ? nullptr : desc.get());
322  }
323 
324  public:
325  /************************************************************************/
326  CustomCartesianSolver(const string& name) : CartesianSolver(name) { }
327 };
328 
329 
330 
331 /************************************************************************/
332 class SolverModule : public RFModule {
333  protected:
334  shared_ptr<CartesianSolver> slv{nullptr};
335 
336  public:
337  /************************************************************************/
338  bool configure(ResourceFinder& rf) {
339  string part, slvName;
340  if (rf.check("part")) {
341  part = rf.find("part").asString();
342  } else {
343  yError() << "part option is not specified";
344  return false;
345  }
346 
347  Bottle& group = rf.findGroup(part);
348  if (group.isNull()) {
349  yError() << "unable to locate " << part << " definition";
350  return false;
351  }
352 
353  if (group.check("name")) {
354  slvName = group.find("name").asString();
355  } else {
356  yError() << "name option is missing";
357  return false;
358  }
359 
360  if (group.check("CustomKinFile")) {
361  yInfo() << "Custom Cartesian Solver detected!";
362 
363  ResourceFinder rf_kin;
364  rf_kin.setDefaultContext(rf.getContext());
365  rf_kin.configure(0, nullptr);
366  pathToCustomKinFile = rf_kin.findFileByName(group.find("CustomKinFile").asString());
367 
368  slv = shared_ptr<CartesianSolver>(new CustomCartesianSolver(slvName));
369  } else if ((part == "left_arm") || (part == "right_arm")) {
370  slv = shared_ptr<CartesianSolver>(new iCubArmCartesianSolver(slvName));
371  } else if ((part == "left_leg") || (part == "right_leg")) {
372  slv = shared_ptr<CartesianSolver>(new iCubLegCartesianSolver(slvName));
373  } else {
374  yError() << part << " is invalid";
375  return false;
376  }
377 
378  return slv->open(group);
379  }
380 
381  /************************************************************************/
383  if (slv) {
384  slv->interrupt();
385  }
386 
387  return true;
388  }
389 
390  /************************************************************************/
391  double getPeriod() {
392  return 1.0;
393  }
394 
395  /************************************************************************/
396  bool updateModule() {
397  if (slv->isClosed()) {
398  return false;
399  }
400 
401  if (slv->getTimeoutFlag()) {
402  slv->getTimeoutFlag() = false;
403  slv->suspend();
404  }
405 
406  return true;
407  }
408 };
409 
410 
411 /************************************************************************/
412 int main(int argc, char* argv[]) {
413  Network yarp;
414  if (!yarp.checkNetwork()) {
415  yError() << "YARP server not available!";
416  return EXIT_FAILURE;
417  }
418 
419  ResourceFinder rf;
420  rf.setDefaultContext("cartesianSolver");
421  rf.setDefaultConfigFile("cartesianSolver.ini");
422  rf.configure(argc, argv);
423 
424  SolverModule mod;
425  return mod.runModule(rf);
426 }
427 
428 
429 
CustomCartesianSolver::getPartDesc
PartDescriptor * getPartDesc(Searchable &options)
Definition: main.cpp:256
GenericLinIneqConstr::uB_orig
Vector uB_orig
Definition: main.cpp:173
iCub::iKin::iKinLinIneqConstr
Definition: iKinIpOpt.h:69
iKinSlv.h
iCub::iKin::iCubLegCartesianSolver
Definition: iKinSlv.h:610
GenericLinIneqConstr::clone
void clone(const iKinLinIneqConstr *obj) override
Definition: main.cpp:176
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
main
int main(int argc, char *argv[])
Definition: main.cpp:31
CustomCartesianSolver
Definition: main.cpp:249
SolverModule::updateModule
bool updateModule()
Definition: main.cpp:396
iCub::iKin::CartesianSolver
Definition: iKinSlv.h:344
GenericLinIneqConstr::GenericLinIneqConstr
GenericLinIneqConstr(iKinChain *chain_, Searchable &options)
Definition: main.cpp:189
GenericLinIneqConstr
Definition: main.cpp:167
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
GenericLinIneqConstr::C_orig
Matrix C_orig
Definition: main.cpp:171
iCub::iKin
Definition: iKinFwd.h:71
GenericLinIneqConstr::lB_orig
Vector lB_orig
Definition: main.cpp:172
GenericLinIneqConstr::update
void update(void *) override
Updates internal state.
Definition: main.cpp:227
GenericLinIneqConstr::configured
bool configured
Definition: main.cpp:169
SolverModule::getPeriod
double getPeriod()
Definition: main.cpp:391
SolverModule
Definition: main.cpp:332
SolverModule::interruptModule
bool interruptModule()
Definition: main.cpp:382
iCub::iKin::PartDescriptor
Definition: iKinSlv.h:328
iCub::iKin::iCubArmCartesianSolver
Definition: iKinSlv.h:584
v
static int v
Definition: iCub_Sim.cpp:42
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
CustomCartesianSolver::CustomCartesianSolver
CustomCartesianSolver(const string &name)
Definition: main.cpp:326
scripting.argc
argc
Definition: scripting.py:184
zeros
zeros(2, 2) eye(2
SolverModule::configure
bool configure(ResourceFinder &rf)
Definition: main.cpp:338
pathToCustomKinFile
static string pathToCustomKinFile
Definition: main.cpp:163