Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tools/smooth-reach.cpp	Lines:	16	55	29.1 %
Date:	2023-03-13 12:09:37	Branches:	28	78	35.9 %


/*
 * 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)",
                                              "int (duration)")));
  addCommand("param",
             makeCommandVoid2(*this, &SmoothReach::setSmoothing,
                              docCommandVoid2("Set the parameter.",
                                              "int (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 int &mode, const double &param) {
  smoothMode = mode;
  smoothParam = param;
}

dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
                                                     const int &time) {
  if (isParam) {
    start = startSIN(time);
    startTime = time;

    assert(start.size() == goal.size());
    isParam = false;
    isStarted = true;
  }

  if (isStarted) {
    double x = double(time - startTime) / 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 int &lengthDes) {
  goal = goalDes;
  lengthTime = lengthDes;
  isParam = true;
}

const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }

const int &SmoothReach::getLength(void) { return lengthTime; }

const int &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;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			#include <dynamic-graph/all-commands.h>
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
19	✓✗	1	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SmoothReach, "SmoothReach");
20
21		1	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	✓✗	2	startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
38			goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	3	sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
40
41			{
42			sotDEBUGIN(5);
43
44	✓✗✓✗	1	signalRegistration(startSIN << goalSOUT);
45	✓✗	1	initCommands();
46		1	goalSOUT.setNeedUpdateFromAllChildren(true);
47			sotDEBUGOUT(5);
48		1	}
49
50		1	void SmoothReach::initCommands(void) {
51			using namespace command;
52	✓✗✓✗	1	addCommand("set",
53	✓✗	1	makeCommandVoid2(*this, &SmoothReach::set,
54	✓✗✓✗ ✓✗✓✗	2	docCommandVoid2("Set the curve.", "vector (goal)",
55			"int (duration)")));
56	✓✗✓✗	1	addCommand("param",
57	✓✗	1	makeCommandVoid2(*this, &SmoothReach::setSmoothing,
58	✓✗✓✗ ✓✗✓✗	2	docCommandVoid2("Set the parameter.",
59			"int (mode)", "double (beta)")));
60		1	}
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
84			dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
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
106			void SmoothReach::set(const dynamicgraph::Vector &goalDes,
107			const int &lengthDes) {
108			goal = goalDes;
109			lengthTime = lengthDes;
110			isParam = true;
111			}
112
113			const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
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			}