Directory: | ./ |
---|---|
File: | src/feature/feature-abstract.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 40 | 59 | 67.8% |
Branches: | 65 | 140 | 46.4% |
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 | |||
12 | #include <iostream> | ||
13 | #include <sot/core/feature-abstract.hh> | ||
14 | #include <sot/core/pool.hh> | ||
15 | |||
16 | #include "sot/core/debug.hh" | ||
17 | #include "sot/core/exception-feature.hh" | ||
18 | |||
19 | using namespace dynamicgraph::sot; | ||
20 | using dynamicgraph::sot::ExceptionFeature; | ||
21 | |||
22 | const std::string FeatureAbstract::CLASS_NAME = "FeatureAbstract"; | ||
23 | |||
24 | 12 | FeatureAbstract::FeatureAbstract(const std::string &name) | |
25 | : Entity(name), | ||
26 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | selectionSIN(NULL, |
27 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | "sotFeatureAbstract(" + name + ")::input(flag)::selec"), |
28 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | errordotSIN( |
29 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | NULL, "sotFeatureAbstract(" + name + ")::input(vector)::errordotIN"), |
30 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
12 | errorSOUT(boost::bind(&FeatureAbstract::computeError, this, _1, _2), |
31 | selectionSIN, | ||
32 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | "sotFeatureAbstract(" + name + ")::output(vector)::error"), |
33 |
3/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
|
12 | errordotSOUT(boost::bind(&FeatureAbstract::computeErrorDot, this, _1, _2), |
34 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | selectionSIN << errordotSIN, |
35 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | "sotFeatureAbstract(" + name + ")::output(vector)::errordot") |
36 | |||
37 | , | ||
38 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
12 | jacobianSOUT( |
39 | boost::bind(&FeatureAbstract::computeJacobian, this, _1, _2), | ||
40 | selectionSIN, | ||
41 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | "sotFeatureAbstract(" + name + ")::output(matrix)::jacobian"), |
42 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
12 | dimensionSOUT(boost::bind(&FeatureAbstract::getDimension, this, _1, _2), |
43 | selectionSIN, | ||
44 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
|
36 | "sotFeatureAbstract(" + name + ")::output(uint)::dim") { |
45 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | selectionSIN = true; |
46 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | signalRegistration(selectionSIN << errorSOUT << jacobianSOUT |
47 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | << dimensionSOUT); |
48 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | featureRegistration(); |
49 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | initCommands(); |
50 | 12 | } | |
51 | |||
52 | 12 | void FeatureAbstract::initCommands(void) { | |
53 | using namespace command; | ||
54 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
24 | addCommand("setReference", |
55 | new dynamicgraph::command::Setter<FeatureAbstract, std::string>( | ||
56 | *this, &FeatureAbstract::setReferenceByName, | ||
57 | "Give the name of the reference feature.\nInput: a string " | ||
58 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | "(feature name).")); |
59 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
24 | addCommand("getReference", |
60 | new dynamicgraph::command::Getter<FeatureAbstract, std::string>( | ||
61 | *this, &FeatureAbstract::getReferenceByName, | ||
62 | "Get the name of the reference feature.\nOutput: a string " | ||
63 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | "(feature name).")); |
64 | 12 | } | |
65 | |||
66 | 12 | void FeatureAbstract::featureRegistration(void) { | |
67 | 12 | PoolStorage::getInstance()->registerFeature(name, this); | |
68 | 12 | } | |
69 | |||
70 | ✗ | std::ostream &FeatureAbstract::writeGraph(std::ostream &os) const { | |
71 | ✗ | Entity::writeGraph(os); | |
72 | |||
73 | ✗ | if (isReferenceSet()) { | |
74 | ✗ | const FeatureAbstract *asotFA = getReferenceAbstract(); | |
75 | ✗ | os << "\t\"" << asotFA->getName() << "\" -> \"" << getName() << "\"" | |
76 | ✗ | << "[ color=darkseagreen4 ]" << std::endl; | |
77 | } else | ||
78 | ✗ | std::cout << "asotFAT : 0" << std::endl; | |
79 | |||
80 | ✗ | return os; | |
81 | } | ||
82 | |||
83 | ✗ | void FeatureAbstract::setReferenceByName(const std::string &name) { | |
84 | ✗ | setReference( | |
85 | ✗ | &dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name)); | |
86 | } | ||
87 | |||
88 | ✗ | std::string FeatureAbstract::getReferenceByName() const { | |
89 | ✗ | if (isReferenceSet()) | |
90 | ✗ | return getReferenceAbstract()->getName(); | |
91 | else | ||
92 | ✗ | return "none"; | |
93 | } | ||
94 | |||
95 | 10 | dynamicgraph::Vector &FeatureAbstract::computeErrorDot( | |
96 | dynamicgraph::Vector &res, int time) { | ||
97 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | const Flags &fl = selectionSIN.access(time); |
98 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | const int &dim = dimensionSOUT(time); |
99 | |||
100 | 10 | unsigned int curr = 0; | |
101 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | res.resize(dim); |
102 | |||
103 | sotDEBUG(25) << "Dim = " << dim << std::endl; | ||
104 | |||
105 |
5/10✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
|
10 | if (isReferenceSet() && getReferenceAbstract()->errordotSIN.isPlugged()) { |
106 | const dynamicgraph::Vector &errdotDes = | ||
107 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | getReferenceAbstract()->errordotSIN(time); |
108 | sotDEBUG(15) << "Err* = " << errdotDes; | ||
109 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
|
10 | if (errdotDes.size() < dim) { |
110 | ✗ | SOT_THROW ExceptionFeature( | |
111 | ✗ | ExceptionFeature::UNCOMPATIBLE_SIZE, | |
112 | "Error: dimension uncompatible with des->errorIN size." | ||
113 | " (while considering feature <%s>).", | ||
114 | ✗ | getName().c_str()); | |
115 | } | ||
116 | |||
117 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 10 times.
|
70 | for (int i = 0; i < errdotDes.size(); ++i) |
118 |
4/8✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 60 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 60 times.
✗ Branch 10 not taken.
|
60 | if (fl(i)) res(curr++) = -errdotDes(i); |
119 | } else | ||
120 | ✗ | res.setZero(); | |
121 | |||
122 | 10 | return res; | |
123 | } | ||
124 |