feature-joint-limits.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* --- SOT --- */
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-joint-limits.hh>
using namespace std;
 
#include <../src/feature/feature-joint-limits-command.h>
 
#include <sot/core/factory.hh>
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits, "FeatureJointLimits");
 
const double FeatureJointLimits::THRESHOLD_DEFAULT = .9;
 
FeatureJointLimits::FeatureJointLimits(const string &fName)
    : FeatureAbstract(fName),
      threshold(THRESHOLD_DEFAULT)
 
      ,
      jointSIN(NULL,
               "sotFeatureJointLimits(" + name + ")::input(vector)::joint"),
      upperJlSIN(NULL,
                 "sotFeatureJointLimits(" + name + ")::input(vector)::upperJl"),
      lowerJlSIN(NULL,
                 "sotFeatureJointLimits(" + name + ")::input(vector)::lowerJl"),
      widthJlSINTERN(
          boost::bind(&FeatureJointLimits::computeWidthJl, this, _1, _2),
          upperJlSIN << lowerJlSIN,
          "sotFeatureJointLimits(" + name + ")::input(vector)::widthJl") {
  errorSOUT.addDependency(jointSIN);
  errorSOUT.addDependency(upperJlSIN);
  errorSOUT.addDependency(lowerJlSIN);
 
  signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << widthJlSINTERN);
 
  // Commands
  //
  std::string docstring;
  // Actuate
  docstring =
      "    \n"
      "    Actuate\n"
      "    \n";
  addCommand("actuate",
             new command::featureJointLimits::Actuate(*this, docstring));
}
 
/* --------------------------------------------------------------------- */
 
void FeatureJointLimits::addDependenciesFromReference(void) {}
void FeatureJointLimits::removeDependenciesFromReference(void) {}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
size_type &FeatureJointLimits::getDimension(size_type &dim, sigtime_t time) {
  sotDEBUG(25) << "# In {" << endl;
 
  const Flags &fl = selectionSIN.access(time);
  const Matrix::Index NBJL = upperJlSIN.access(time).size();
 
  dim = 0;
  for (Matrix::Index i = 0; i < NBJL; ++i)
    if (fl(static_cast<size_type>(i))) dim++;
 
  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}
 
Vector &FeatureJointLimits::computeWidthJl(Vector &res, const sigtime_t &time) {
  sotDEBUGIN(15);
 
  const Vector UJL = upperJlSIN.access(time);
  const Vector LJL = lowerJlSIN.access(time);
  const Vector::Index SIZE = UJL.size();
  res.resize(SIZE);
 
  for (Vector::Index i = 0; i < SIZE; ++i) {
    res(i) = UJL(i) - LJL(i);
  }
 
  sotDEBUGOUT(15);
  return res;
}
 
Matrix &FeatureJointLimits::computeJacobian(Matrix &J, sigtime_t time) {
  sotDEBUG(15) << "# In {" << endl;
 
  const std::size_t SIZE = dimensionSOUT.access(time);
  const Vector q = jointSIN.access(time);
  const Flags &fl = selectionSIN(time);
  // const std::size_t SIZE_FF=SIZE+freeFloatingSize;
  const Vector::Index SIZE_TOTAL = q.size();
  const Vector WJL = widthJlSINTERN.access(time);
  J.resize(SIZE, SIZE_TOTAL);
  J.setZero();
 
  std::size_t idx = 0;
  for (size_type i = 0; i < SIZE_TOTAL; ++i) {
    if (fl(i)) {
      if (fabs(WJL(i)) > 1e-3)
        J(idx, i) = 1 / WJL(i);
      else
        J(idx, i) = 1.;
      idx++;
    }
  }
  //   if( 0!=freeFloatingIndex )
  //     for( std::size_t i=0;i<freeFloatingIndex;++i )
  //       {
  //    if( fabs(WJL(i))>1e-3 ) J(i,i)=1/WJL(i); else J(i,i)=1.;
  //       }
 
  //   if( SIZE!=freeFloatingIndex )
  //     for( std::size_t i=freeFloatingIndex;i<SIZE;++i )
  //       {
  //    if( fabs(WJL(i))>1e-3 ) J(i,i+freeFloatingSIZE)=1/WJL(i);
  //    else J(i,i)=1.;
  //       }
 
  sotDEBUG(15) << "# Out }" << endl;
  return J;
}
 
Vector &FeatureJointLimits::computeError(Vector &error, sigtime_t time) {
  sotDEBUGIN(15);
 
  const Flags &fl = selectionSIN(time);
  const Vector q = jointSIN.access(time);
  const Vector UJL = upperJlSIN.access(time);
  const Vector LJL = lowerJlSIN.access(time);
  const Vector WJL = widthJlSINTERN.access(time);
  const size_type SIZE = dimensionSOUT.access(time);
  const Vector::Index SIZE_TOTAL = q.size();
 
  sotDEBUG(25) << "q = " << q << endl;
  sotDEBUG(25) << "ljl = " << LJL << endl;
  sotDEBUG(25) << "Wjl = " << WJL << endl;
  sotDEBUG(25) << "dim = " << SIZE << endl;
 
  assert(UJL.size() == SIZE_TOTAL);
  assert(WJL.size() == SIZE_TOTAL);
  assert(LJL.size() == SIZE_TOTAL);
  assert(SIZE <= SIZE_TOTAL);
 
  error.resize(SIZE);
 
  std::size_t parcerr = 0;
  for (size_type i = 0; i < SIZE_TOTAL; ++i) {
    if (fl(i)) {
      error(parcerr++) = (q(i) - LJL(i)) / WJL(i) * 2 - 1;
    }
  }
 
  sotDEBUGOUT(15);
  return error;
}
 
void FeatureJointLimits::display(std::ostream &os) const {
  os << "JointLimits <" << name << "> ... TODO";
}