Directory: | ./ |
---|---|
File: | src/tools/smooth-reach.cpp |
Date: | 2024-08-13 12:13:25 |
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 ¶m) { | |
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 |