feature-generic.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/factory.hh>
#include <sot/core/feature-generic.hh>
 
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
FeatureGeneric::FeatureGeneric(const string &pointName)
    : FeatureAbstract(pointName),
      dimensionDefault(0),
      errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
      jacobianSIN(NULL,
                  "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
 
{
  jacobianSOUT.addDependency(jacobianSIN);
  errorSOUT.addDependency(errorSIN);
 
  signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT);
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void FeatureGeneric::addDependenciesFromReference(void) {
  assert(SP::isReferenceSet());
  errorSOUT.addDependency(getReference()->errorSIN);
  errordotSOUT.addDependency(getReference()->errordotSIN);
}
 
void FeatureGeneric::removeDependenciesFromReference(void) {
  assert(SP::isReferenceSet());
  errorSOUT.removeDependency(getReference()->errorSIN);
  errordotSOUT.removeDependency(getReference()->errordotSIN);
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
size_type &FeatureGeneric::getDimension(size_type &dim, sigtime_t time) {
  sotDEBUG(25) << "# In {" << endl;
 
  const Flags &fl = selectionSIN.access(time);
 
  if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();
 
  dim = 0;
  for (size_type i = 0; i < dimensionDefault; ++i)
    if (fl(i)) dim++;
 
  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}
 
Vector &FeatureGeneric::computeError(Vector &res, sigtime_t time) {
  const Vector &err = errorSIN.access(time);
  const Flags &fl = selectionSIN.access(time);
  const size_type &dim = dimensionSOUT(time);
 
  std::size_t curr = 0;
  res.resize(dim);
  if (err.size() < dim) {
    SOT_THROW ExceptionFeature(
        ExceptionFeature::UNCOMPATIBLE_SIZE,
        "Error: dimension uncompatible with des->errorIN size."
        " (while considering feature <%s>).",
        getName().c_str());
  }
 
  sotDEBUG(15) << "Err = " << err;
  sotDEBUG(25) << "Dim = " << dim << endl;
 
  if (isReferenceSet()) {
    const Vector &errDes = getReference()->errorSIN(time);
    sotDEBUG(15) << "Err* = " << errDes;
    if (errDes.size() < dim) {
      SOT_THROW ExceptionFeature(
          ExceptionFeature::UNCOMPATIBLE_SIZE,
          "Error: dimension uncompatible with des->errorIN size."
          " (while considering feature <%s>).",
          getName().c_str());
    }
 
    for (size_type i = 0; i < err.size(); ++i)
      if (fl(i))
        if (fl(i)) res(curr++) = err(i) - errDes(i);
  } else {
    for (size_type i = 0; i < err.size(); ++i)
      if (fl(i)) res(curr++) = err(i);
  }
 
  return res;
}
 
Matrix &FeatureGeneric::computeJacobian(Matrix &res, sigtime_t time) {
  sotDEBUGIN(15);
 
  const Matrix &Jac = jacobianSIN.access(time);
  const Flags &fl = selectionSIN.access(time);
  const std::size_t &dim = dimensionSOUT(time);
 
  std::size_t curr = 0;
  res.resize(dim, Jac.cols());
 
  for (std::size_t i = 0; curr < dim; ++i)
    if (fl(i)) {
      for (size_type j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j);
      curr++;
    }
 
  sotDEBUGOUT(15);
  return res;
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void FeatureGeneric::display(std::ostream &os) const {
  os << "Generic <" << name << ">: " << std::endl;
 
  try {
    os << "  error= " << errorSIN.accessCopy() << endl
       << "  J    = " << jacobianSIN.accessCopy() << endl;
  } catch (ExceptionSignal &e) {
    os << e.what();
  }
}