smooth-reach.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
 
#include <sot/core/debug.hh>
#include <sot/core/smooth-reach.hh>
 
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
 
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SmoothReach, "SmoothReach");
 
SmoothReach::SmoothReach(const std::string &name)
    : Entity(name)
 
      ,
      start(0u),
      goal(0u),
      startTime(-1),
      lengthTime(-1),
      isStarted(false),
      isParam(true)
 
      ,
      smoothMode(2),
      smoothParam(1.2)
 
      ,
      startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
      goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
               sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
 
{
  sotDEBUGIN(5);
 
  signalRegistration(startSIN << goalSOUT);
  initCommands();
  goalSOUT.setNeedUpdateFromAllChildren(true);
  sotDEBUGOUT(5);
}
 
void SmoothReach::initCommands(void) {
  using namespace command;
  addCommand("set",
             makeCommandVoid2(*this, &SmoothReach::set,
                              docCommandVoid2("Set the curve.", "vector (goal)",
                                              "size_type (duration)")));
  addCommand("param", makeCommandVoid2(*this, &SmoothReach::setSmoothing,
                                       docCommandVoid2("Set the parameter.",
                                                       "size_type (mode)",
                                                       "double (beta)")));
}
 
double SmoothReach::smoothFunction(double x) {
  switch (smoothMode) {
    case 0:
      return x;
 
    case 1: {
      // const double smoothParam = 0.45;
      return tanh(-smoothParam / x + smoothParam / (1 - x)) / 2 + 0.5;
    }
    case 2: {
      // const double smoothParam = 1.5;
      return atan(-smoothParam / x + smoothParam / (1 - x)) / M_PI + 0.5;
    }
  }
  return 0;
}
 
void SmoothReach::setSmoothing(const size_type &mode, const double &param) {
  smoothMode = mode;
  smoothParam = param;
}
 
dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
                                                     const sigtime_t &time) {
  if (isParam) {
    start = startSIN(time);
    startTime = time;
 
    assert(start.size() == goal.size());
    isParam = false;
    isStarted = true;
  }
 
  if (isStarted) {
    double x = double(time - startTime) / (double)lengthTime;
    if (x > 1) x = 1;
    double x1 = smoothFunction(x);
    double x0 = 1 - x1;
    res = start * x0 + goal * x1;
  }
 
  return res;
}
 
void SmoothReach::set(const dynamicgraph::Vector &goalDes,
                      const size_type &lengthDes) {
  goal = goalDes;
  lengthTime = lengthDes;
  isParam = true;
}
 
const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
 
const size_type &SmoothReach::getLength(void) { return lengthTime; }
 
const size_type &SmoothReach::getStart(void) { return startTime; }
 
void SmoothReach::display(std::ostream &os) const {
  os << "Status: " << isStarted << isParam << std::endl
     << "Goal: " << goal << "start: " << start << "Times: " << startTime << " "
     << lengthTime << std::endl;
}