feature-1d.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-1d.hh>
using namespace std;
 
#include <sot/core/factory.hh>
 
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Feature1D, "Feature1D");
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
Feature1D::Feature1D(const string &pointName)
    : FeatureAbstract(pointName),
      errorSIN(NULL, "sotFeature1D(" + name + ")::input(vector)::errorIN"),
      jacobianSIN(NULL,
                  "sotFeature1D(" + name + ")::input(matrix)::jacobianIN") {
  jacobianSOUT.addDependency(jacobianSIN);
  errorSOUT.addDependency(errorSIN);
 
  signalRegistration(errorSIN << jacobianSIN);
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void Feature1D::addDependenciesFromReference(void) {}
void Feature1D::removeDependenciesFromReference(void) {}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
size_type &Feature1D::getDimension(size_type &dim, sigtime_t /*time*/) {
  sotDEBUG(25) << "# In {" << endl;
 
  dim = 1;
 
  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}
 
dynamicgraph::Vector &Feature1D::computeError(dynamicgraph::Vector &res,
                                              sigtime_t time) {
  const dynamicgraph::Vector &err = errorSIN.access(time);
  res.resize(1);
  res(0) = err.dot(err) * .5;
 
  return res;
}
 
dynamicgraph::Matrix &Feature1D::computeJacobian(dynamicgraph::Matrix &res,
                                                 sigtime_t time) {
  sotDEBUGIN(15);
 
  const dynamicgraph::Matrix &Jac = jacobianSIN.access(time);
  const dynamicgraph::Vector &err = errorSIN.access(time);
 
  res.resize(1, Jac.cols());
  res.fill(0);
  for (size_type j = 0; j < Jac.cols(); ++j)
    for (size_type i = 0; i < Jac.rows(); ++i) res(0, j) += err(i) * Jac(i, j);
 
  sotDEBUGOUT(15);
  return res;
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void Feature1D::display(std::ostream &os) const {
  os << "1D <" << name << ">: " << std::endl;
 
  try {
    os << "  error= " << errorSIN.accessCopy() << endl
       << "  J    = " << jacobianSIN.accessCopy() << endl;
  } catch (const ExceptionAbstract &e) {
    os << " All SIN not set.";
  }
}