joint-limitator.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
#include <sot/core/joint-limitator.hh>
 
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
typedef dynamicgraph::size_type size_type;
 
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointLimitator, "JointLimitator");
 
JointLimitator::JointLimitator(const string &fName)
    : Entity(fName),
      jointSIN(NULL, "JointLimitator(" + name + ")::input(vector)::joint"),
      upperJlSIN(NULL, "JointLimitator(" + name + ")::input(vector)::upperJl"),
      lowerJlSIN(NULL, "JointLimitator(" + name + ")::input(vector)::lowerJl"),
      controlSIN(NULL,
                 "JointLimitator(" + name + ")::input(vector)::controlIN"),
      controlSOUT(boost::bind(&JointLimitator::computeControl, this, _1, _2),
                  jointSIN << upperJlSIN << lowerJlSIN << controlSIN,
                  "JointLimitator(" + name + ")::output(vector)::control"),
      widthJlSINTERN(boost::bind(&JointLimitator::computeWidthJl, this, _1, _2),
                     upperJlSIN << lowerJlSIN,
                     "JointLimitator(" + name + ")::input(vector)::widthJl")
 
{
  signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << controlSIN
                              << controlSOUT << widthJlSINTERN);
}
 
dynamicgraph::Vector &JointLimitator::computeWidthJl(dynamicgraph::Vector &res,
                                                     const sigtime_t &time) {
  sotDEBUGIN(15);
 
  const dynamicgraph::Vector UJL = upperJlSIN.access(time);
  const dynamicgraph::Vector LJL = lowerJlSIN.access(time);
  const dynamicgraph::Vector::Index SIZE = UJL.size();
  res.resize(SIZE);
 
  for (size_type i = 0; i < SIZE; ++i) {
    res(i) = UJL(i) - LJL(i);
  }
 
  sotDEBUGOUT(15);
  return res;
}
 
dynamicgraph::Vector &JointLimitator::computeControl(dynamicgraph::Vector &uOUT,
                                                     sigtime_t time) {
  sotDEBUGIN(15);
 
  const dynamicgraph::Vector &q = jointSIN.access(time);
  const dynamicgraph::Vector &UJL = upperJlSIN.access(time);
  const dynamicgraph::Vector &LJL = lowerJlSIN.access(time);
  const dynamicgraph::Vector &uIN = controlSIN.access(time);
 
  dynamicgraph::Vector::Index controlSize = uIN.size();
  uOUT.resize(controlSize);
  uOUT.setZero();
 
  dynamicgraph::Vector::Index offset = q.size() - uIN.size();
  assert(offset >= 0);
 
  for (size_type i = 0; i < controlSize; ++i) {
    double qnext = q(i + offset) + uIN(i) * 0.005;
    if ((i + offset < 6) ||  // do not take into account of freeflyer
        ((qnext < UJL(i + offset)) && (qnext > LJL(i + offset)))) {
      uOUT(i) = uIN(i);
    }
    sotDEBUG(25) << i << ": " << qnext << " in? [" << LJL(i) << "," << UJL(i)
                 << "]" << endl;
  }
 
  sotDEBUGOUT(15);
  return uOUT;
}
 
void JointLimitator::display(std::ostream &os) const {
  os << "JointLimitator <" << name << "> ... TODO";
}