GCC Code Coverage Report

Directory: ./
File: src/tools/smooth-reach.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 24 62 38.7%
Branches: 28 78 35.9%
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/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 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
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 goal(0u),
27 1 startTime(-1),
28 1 lengthTime(-1),
29 1 isStarted(false),
30 1 isParam(true)
31
32 ,
33 1 smoothMode(2),
34 1 smoothParam(1.2)
35
36 ,
37
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
38
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
39
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 3 sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
40
41 {
42 sotDEBUGIN(5);
43
44
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 signalRegistration(startSIN << goalSOUT);
45
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 initCommands();
46 1 goalSOUT.setNeedUpdateFromAllChildren(true);
47 sotDEBUGOUT(5);
48 1 }
49
50 1 void SmoothReach::initCommands(void) {
51 using namespace command;
52
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("set",
53
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(*this, &SmoothReach::set,
54
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set the curve.", "vector (goal)",
55 "int (duration)")));
56
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("param",
57
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(*this, &SmoothReach::setSmoothing,
58
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 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 }
124