smooth-reach.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
11 #include <dynamic-graph/factory.h>
12 
13 #include <sot/core/debug.hh>
14 #include <sot/core/smooth-reach.hh>
15 
16 using namespace dynamicgraph;
17 using namespace dynamicgraph::sot;
18 
20 
21 SmoothReach::SmoothReach(const std::string &name)
22  : Entity(name)
23 
24  ,
25  start(0u),
26  goal(0u),
27  startTime(-1),
28  lengthTime(-1),
29  isStarted(false),
30  isParam(true)
31 
32  ,
33  smoothMode(2),
34  smoothParam(1.2)
35 
36  ,
37  startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
38  goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
39  sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
40 
41 {
42  sotDEBUGIN(5);
43 
45  initCommands();
47  sotDEBUGOUT(5);
48 }
49 
51  using namespace command;
52  addCommand("set",
54  docCommandVoid2("Set the curve.", "vector (goal)",
55  "int (duration)")));
56  addCommand("param",
58  docCommandVoid2("Set the parameter.",
59  "int (mode)", "double (beta)")));
60 }
61 
62 double SmoothReach::smoothFunction(double x) {
63  switch (smoothMode) {
64  case 0:
65  return x;
66 
67  case 1: {
68  // const double smoothParam = 0.45;
69  return tanh(-smoothParam / x + smoothParam / (1 - x)) / 2 + 0.5;
70  }
71  case 2: {
72  // const double smoothParam = 1.5;
73  return atan(-smoothParam / x + smoothParam / (1 - x)) / M_PI + 0.5;
74  }
75  }
76  return 0;
77 }
78 
79 void SmoothReach::setSmoothing(const int &mode, const double &param) {
80  smoothMode = mode;
81  smoothParam = param;
82 }
83 
85  const int &time) {
86  if (isParam) {
87  start = startSIN(time);
88  startTime = time;
89 
90  assert(start.size() == goal.size());
91  isParam = false;
92  isStarted = true;
93  }
94 
95  if (isStarted) {
96  double x = double(time - startTime) / lengthTime;
97  if (x > 1) x = 1;
98  double x1 = smoothFunction(x);
99  double x0 = 1 - x1;
100  res = start * x0 + goal * x1;
101  }
102 
103  return res;
104 }
105 
107  const int &lengthDes) {
108  goal = goalDes;
109  lengthTime = lengthDes;
110  isParam = true;
111 }
112 
114 
115 const int &SmoothReach::getLength(void) { return lengthTime; }
116 
117 const int &SmoothReach::getStart(void) { return startTime; }
118 
119 void SmoothReach::display(std::ostream &os) const {
120  os << "Status: " << isStarted << isParam << std::endl
121  << "Goal: " << goal << "start: " << start << "Times: " << startTime << " "
122  << lengthTime << std::endl;
123 }
void set(const dynamicgraph::Vector &goal, const int &length)
Eigen::VectorXd Vector
SignalArray< int > sotNOSIGNAL(0)
SmoothReach(const std::string &name)
void signalRegistration(const SignalArray< int > &signals)
#define sotDEBUGOUT(level)
Definition: debug.hh:212
void setNeedUpdateFromAllChildren(const bool b=true)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
#define M_PI
#define sotDEBUGIN(level)
Definition: debug.hh:211
void setSmoothing(const int &mode, const double &param)
x
dynamicgraph::Vector goal
Definition: smooth-reach.hh:51
dynamicgraph::Vector & goalSOUT_function(dynamicgraph::Vector &goal, const int &time)
dynamicgraph::Vector start
Definition: smooth-reach.hh:51
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > startSIN
Definition: smooth-reach.hh:61
std::string docCommandVoid2(const std::string &doc, const std::string &type1, const std::string &type2)
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > goalSOUT
Definition: smooth-reach.hh:65
u
void addCommand(const std::string &name, command::Command *command)
const dynamicgraph::Vector & getGoal(void)
virtual void display(std::ostream &os) const
x0
CommandVoid2< E, T1, T2 > * makeCommandVoid2(E &entity, boost::function< void(const T1 &, const T2 &)> function, const std::string &docString)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26