main.cpp

Go to the documentation of this file.

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/
 
#include <cstdlib>
#include <memory>
#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
 
#include <yarp/os/LogStream.h>
#include <yarp/os/Searchable.h>
#include <yarp/os/Property.h>
#include <yarp/os/Value.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Network.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/os/RFModule.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>
 
#include <iCub/iKin/iKinSlv.h>
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::iKin;
 
static string pathToCustomKinFile;
 
 
/************************************************************************/
class GenericLinIneqConstr : public iKinLinIneqConstr {
 protected:
  bool configured{false};
  iKinChain* chain{nullptr};
  Matrix C_orig;
  Vector lB_orig;
  Vector uB_orig;
 
  /************************************************************************/
  void clone(const iKinLinIneqConstr* obj)override {
    iKinLinIneqConstr::clone(obj);
 
    const auto* ptr = static_cast<const GenericLinIneqConstr*>(obj);
    configured = ptr->configured;
    chain = ptr->chain;
    C_orig = ptr->C_orig;
    lB_orig = ptr->lB_orig;
    uB_orig = ptr->uB_orig;
  }
 
 public:
  /************************************************************************/
  GenericLinIneqConstr(iKinChain* chain_, Searchable& options) :
      chain(chain_), C_orig(0, chain_->getN()), iKinLinIneqConstr() {
    if (options.check("LIC_num")) {
      auto LIC_num = options.find("LIC_num").asInt32();
      if (LIC_num > 0) {
        for (auto i = 0; i < LIC_num; i++) {
          ostringstream tag;
          tag << "LIC_" << i;
          Bottle& group = options.findGroup(tag.str());
          if (group.isNull()) {
            yError() << "Unable to find group \"" << tag.str() << "\"";
            return;
          }
          Bottle* row = group.find("C").asList();
          if (row == nullptr) {
            yError() << "Unable to find option \"C\" for group \"" << tag.str() << "\"";
            return;
          }
          if (row->size() != chain->getN()) {
            yError() << "Option \"C\" of group \"" << tag.str() << "\" has wrong size";
            return;
          }
          C_orig = pile(C_orig, zeros(chain->getN()));
          for (auto i = 0; i < row->size(); i++) {
            C_orig(C_orig.rows() - 1, i) = row->get(i).asFloat64();
          }
          lB_orig = cat(lB_orig, group.check("lB", Value(lowerBoundInf)).asFloat64());
          uB_orig = cat(uB_orig, group.check("uB", Value(upperBoundInf)).asFloat64());
        }
 
        yInfo() << "Detected generic linear inequalities constraints";
        configured = true;
        update(nullptr);
      }
    }
  }
 
  /************************************************************************/
  void update(void*)override {
    setActive(false);
    if (configured) {
      getC().resize(C_orig.rows(), 0);
      getlB() = lB_orig;
      getuB() = uB_orig;
      for (auto i = 0U; i < chain->getN(); i++) {
        if (chain->isLinkBlocked(i)) {
          auto v = chain->getAng(i) * C_orig.getCol(i);
          getlB() -= v;
          getuB() -= v;
        } else {
          getC() = cat(getC(), C_orig.getCol(i));
        }
      }
      setActive(true);
    }
  }
};
 
 
/************************************************************************/
class CustomCartesianSolver : public CartesianSolver {
  shared_ptr<iKinLimb> limb{nullptr};
  shared_ptr<GenericLinIneqConstr> cns{nullptr};
  shared_ptr<PartDescriptor> desc{nullptr};
 
 protected:
  /************************************************************************/
  PartDescriptor* getPartDesc(Searchable& options) {
    if (!options.check("robot")) {
      yError() << "\"robot\" option is missing!";
      return nullptr;
    }
 
    if (!options.check("NumberOfDrivers")) {
      yError() << "\"NumberOfDrivers\" option is missing!";
      return nullptr;
    }
 
    string robot = options.find("robot").asString();
    yInfo() << "Configuring solver for " << robot << " ...";
 
    Property linksOptions;
    linksOptions.fromConfigFile(pathToCustomKinFile);
    limb = shared_ptr<iKinLimb>(new iKinLimb(linksOptions));
    if (!limb->isValid()) {
      yError() << "invalid links parameters!";
      return nullptr;
    }
 
    cns = shared_ptr<GenericLinIneqConstr>(new GenericLinIneqConstr(limb->asChain(), options));
    desc = shared_ptr<PartDescriptor>(new PartDescriptor);
    desc->lmb = limb.get();
    desc->chn = limb->asChain();
    desc->cns = cns.get();
 
    bool failure = false;
    desc->num = options.find("NumberOfDrivers").asInt32();
    for (int cnt = 0; cnt < desc->num; cnt++) {
      ostringstream str;
      str << "driver_" << cnt;
      Bottle& driver = options.findGroup(str.str());
      if (driver.isNull()) {
        yError() << "\"" << str.str() << "\" option is missing!";
        failure = true;
        break;
      }
 
      if (!driver.check("Key")) {
        yError() << "\"Key\" option is missing!";
        failure = true;
        break;
      }
 
      if (!driver.check("JointsOrder")) {
        yError() << "\"JointsOrder\" option is missing!";
        failure = true;
        break;
      }
 
      string part = driver.find("Key").asString();
      bool directOrder = (driver.find("JointsOrder").asString() == "direct");
 
      Property optPart;
      optPart.put("device", "remote_controlboard");
      optPart.put("remote", "/" + robot + "/" + part);
      optPart.put("local", "/" + slvName + "/" + part);
      optPart.put("robot", robot);
      optPart.put("part", part);
      desc->prp.push_back(optPart);
      desc->rvs.push_back(!directOrder);
    }
 
    return (failure ? nullptr : desc.get());
  }
 
 public:
  /************************************************************************/
  CustomCartesianSolver(const string& name) : CartesianSolver(name) { }
};
 
 
 
/************************************************************************/
class SolverModule : public RFModule {
 protected:
  shared_ptr<CartesianSolver> slv{nullptr};
 
 public:
  /************************************************************************/
  bool configure(ResourceFinder& rf) {
    string part, slvName;
    if (rf.check("part")) {
      part = rf.find("part").asString();
    } else {
      yError() << "part option is not specified";
      return false;
    }
 
    Bottle& group = rf.findGroup(part);
    if (group.isNull()) {
      yError() << "unable to locate " << part << " definition";
      return false;
    }
 
    if (group.check("name")) {
      slvName = group.find("name").asString();
    } else {
      yError() << "name option is missing";
      return false;
    }
 
    if (group.check("CustomKinFile")) {
      yInfo() << "Custom Cartesian Solver detected!";
 
      ResourceFinder rf_kin;
      rf_kin.setDefaultContext(rf.getContext());
      rf_kin.configure(0, nullptr);
      pathToCustomKinFile = rf_kin.findFileByName(group.find("CustomKinFile").asString());
 
      slv = shared_ptr<CartesianSolver>(new CustomCartesianSolver(slvName));
    } else if ((part == "left_arm") || (part == "right_arm")) {
      slv = shared_ptr<CartesianSolver>(new iCubArmCartesianSolver(slvName));
    } else if ((part == "left_leg") || (part == "right_leg")) {
      slv = shared_ptr<CartesianSolver>(new iCubLegCartesianSolver(slvName));
    } else {
      yError() << part << " is invalid";
      return false;
    }
 
    return slv->open(group);
  }
 
  /************************************************************************/
  bool interruptModule() {
    if (slv) {
      slv->interrupt();
    }
 
    return true;
  }
 
  /************************************************************************/
  double getPeriod() {
    return 1.0;
  }
 
  /************************************************************************/
  bool updateModule() {
    if (slv->isClosed()) {
      return false;
    }
 
    if (slv->getTimeoutFlag()) {
      slv->getTimeoutFlag() = false;
      slv->suspend();
    }
 
    return true;
  }
};
 
 
/************************************************************************/
int main(int argc, char* argv[]) {
  Network yarp;
  if (!yarp.checkNetwork()) {
    yError() << "YARP server not available!";
    return EXIT_FAILURE;
  }
 
  ResourceFinder rf;
  rf.setDefaultContext("cartesianSolver");
  rf.setDefaultConfigFile("cartesianSolver.ini");
  rf.configure(argc, argv);
 
  SolverModule mod;
  return mod.runModule(rf);
}