task-pd.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* SOT */
#include <dynamic-graph/all-commands.h>
 
#include <sot/core/debug.hh>
#include <sot/core/task-pd.hh>
 
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
 
#include <sot/core/factory.hh>
 
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskPD, "TaskPD");
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
TaskPD::TaskPD(const std::string &n)
    : Task(n),
      previousError(),
      beta(1),
      errorDotSOUT(boost::bind(&TaskPD::computeErrorDot, this, _1, _2),
                   errorSOUT,
                   "sotTaskPD(" + n + ")::output(vector)::errorDotOUT"),
      errorDotSIN(NULL, "sotTaskPD(" + n + ")::input(vector)::errorDot") {
  taskSOUT.setFunction(boost::bind(&TaskPD::computeTaskModif, this, _1, _2));
  taskSOUT.addDependency(errorDotSOUT);
 
  signalRegistration(errorDotSOUT << errorDotSIN);
  initCommand();
  errorDotSIN.plug(&errorDotSOUT);
}
 
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
 
dynamicgraph::Vector &TaskPD::computeErrorDot(dynamicgraph::Vector &errorDot,
                                              sigtime_t time) {
  sotDEBUG(15) << "# In {" << endl;
 
  const dynamicgraph::Vector &errCur = errorSOUT(time);
  if (previousError.size() == errCur.size()) {
    errorDot = errCur;
    errorDot -= previousError;
    previousError = errCur;
  } else {
    errorDot.resize(errCur.size());
    errorDot.setZero();
    previousError = errCur;
  }
  sotDEBUG(15) << "# Out }" << endl;
  return errorDot;
}
 
VectorMultiBound &TaskPD::computeTaskModif(VectorMultiBound &task,
                                           sigtime_t time) {
  sotDEBUG(15) << "# In {" << endl;
 
  const dynamicgraph::Vector &errorDot = errorDotSIN(time);
  Task::computeTaskExponentialDecrease(task, time);
 
  sotDEBUG(25) << " Task = " << task;
  sotDEBUG(25) << " edot = " << errorDot;
 
  for (std::size_t i = 0; i < task.size(); ++i) {
    task[i] = task[i].getSingleBound() - (beta * errorDot(i));
  }
 
  sotDEBUG(15) << "# Out }" << endl;
  return task;
}
 
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
#include <sot/core/pool.hh>
 
void TaskPD::initCommand(void) {
  using namespace command;
  addCommand("setBeta",
             makeDirectSetter(*this, &beta, docDirectSetter("beta", "double")));
}