Commit 358877f1 authored by Maximilien Naveau's avatar Maximilien Naveau Committed by Olivier Stasse
Browse files

[dg plugins] re-package the smooth-reach plugins.

parent fd002b2d
Pipeline #12619 passed with stage
in 35 minutes and 7 seconds
......@@ -61,6 +61,7 @@ SET(plugins
tools/exp-moving-avg
tools/gradient-ascent
tools/parameter-server
tools/smooth-reach
control/control-gr
control/control-pd
......
#include <sot/core/smooth-reach.hh>
typedef boost::mpl::vector<dynamicgraph::sot::SmoothReach> entities_t;
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#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(0), goal(0), startTime(-1), lengthTime(-1), isStarted(false),
isParam(true)
,
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);
sotDEBUGOUT(5);
}
double smoothFunction(double x) { return x; }
dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &goal,
const int &time) {
if (isParam) {
start = startSIN(time);
startTime = time;
assert(start.size() == goal.size());
isParam = false;
isStarted = true;
}
if (isReady) {
double x = double(time - start) / length;
if (x > 1)
x = 1;
double x1 = smoothFunction(x);
double x0 = 1 - x1;
goal = start * x0 + goal * x1;
}
return goal;
}
void SmoothReach::gset(const dynamicgraph::Vector &goalDes,
const int &lengthDes) {
goal = goalDes;
length = lengthDes;
isParam = true;
}
const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
const int &SmoothReach::getLength(void) { return length; }
const int &SmoothReach::getStart(void) { return start; }
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment