1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
/* --------------------------------------------------------------------- */ |
11 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
12 |
|
|
/* --------------------------------------------------------------------- */ |
13 |
|
|
|
14 |
|
|
/* --- SOT --- */ |
15 |
|
|
#include <sot/core/debug.hh> |
16 |
|
|
#include <sot/core/exception-feature.hh> |
17 |
|
|
#include <sot/core/feature-joint-limits.hh> |
18 |
|
|
using namespace std; |
19 |
|
|
|
20 |
|
|
#include <../src/feature/feature-joint-limits-command.h> |
21 |
|
|
|
22 |
|
|
#include <sot/core/factory.hh> |
23 |
|
|
|
24 |
|
|
/* --------------------------------------------------------------------- */ |
25 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
26 |
|
|
/* --------------------------------------------------------------------- */ |
27 |
|
|
|
28 |
|
|
using namespace dynamicgraph::sot; |
29 |
|
|
using namespace dynamicgraph; |
30 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits, "FeatureJointLimits"); |
31 |
|
|
|
32 |
|
|
const double FeatureJointLimits::THRESHOLD_DEFAULT = .9; |
33 |
|
|
|
34 |
|
|
FeatureJointLimits::FeatureJointLimits(const string &fName) |
35 |
|
|
: FeatureAbstract(fName), |
36 |
|
|
threshold(THRESHOLD_DEFAULT) |
37 |
|
|
|
38 |
|
|
, |
39 |
|
|
jointSIN(NULL, |
40 |
|
|
"sotFeatureJointLimits(" + name + ")::input(vector)::joint"), |
41 |
|
|
upperJlSIN(NULL, |
42 |
|
|
"sotFeatureJointLimits(" + name + ")::input(vector)::upperJl"), |
43 |
|
|
lowerJlSIN(NULL, |
44 |
|
|
"sotFeatureJointLimits(" + name + ")::input(vector)::lowerJl"), |
45 |
|
|
widthJlSINTERN( |
46 |
|
|
boost::bind(&FeatureJointLimits::computeWidthJl, this, _1, _2), |
47 |
|
|
upperJlSIN << lowerJlSIN, |
48 |
|
|
"sotFeatureJointLimits(" + name + ")::input(vector)::widthJl") { |
49 |
|
|
errorSOUT.addDependency(jointSIN); |
50 |
|
|
errorSOUT.addDependency(upperJlSIN); |
51 |
|
|
errorSOUT.addDependency(lowerJlSIN); |
52 |
|
|
|
53 |
|
|
signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << widthJlSINTERN); |
54 |
|
|
|
55 |
|
|
// Commands |
56 |
|
|
// |
57 |
|
|
std::string docstring; |
58 |
|
|
// Actuate |
59 |
|
|
docstring = |
60 |
|
|
" \n" |
61 |
|
|
" Actuate\n" |
62 |
|
|
" \n"; |
63 |
|
|
addCommand("actuate", |
64 |
|
|
new command::featureJointLimits::Actuate(*this, docstring)); |
65 |
|
|
} |
66 |
|
|
|
67 |
|
|
/* --------------------------------------------------------------------- */ |
68 |
|
|
|
69 |
|
|
void FeatureJointLimits::addDependenciesFromReference(void) {} |
70 |
|
|
void FeatureJointLimits::removeDependenciesFromReference(void) {} |
71 |
|
|
|
72 |
|
|
/* --------------------------------------------------------------------- */ |
73 |
|
|
/* --------------------------------------------------------------------- */ |
74 |
|
|
/* --------------------------------------------------------------------- */ |
75 |
|
|
|
76 |
|
|
unsigned int &FeatureJointLimits::getDimension(unsigned int &dim, int time) { |
77 |
|
|
sotDEBUG(25) << "# In {" << endl; |
78 |
|
|
|
79 |
|
|
const Flags &fl = selectionSIN.access(time); |
80 |
|
|
const Matrix::Index NBJL = upperJlSIN.access(time).size(); |
81 |
|
|
|
82 |
|
|
dim = 0; |
83 |
|
|
for (Matrix::Index i = 0; i < NBJL; ++i) |
84 |
|
|
if (fl(static_cast<int>(i))) dim++; |
85 |
|
|
|
86 |
|
|
sotDEBUG(25) << "# Out }" << endl; |
87 |
|
|
return dim; |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
Vector &FeatureJointLimits::computeWidthJl(Vector &res, const int &time) { |
91 |
|
|
sotDEBUGIN(15); |
92 |
|
|
|
93 |
|
|
const Vector UJL = upperJlSIN.access(time); |
94 |
|
|
const Vector LJL = lowerJlSIN.access(time); |
95 |
|
|
const Vector::Index SIZE = UJL.size(); |
96 |
|
|
res.resize(SIZE); |
97 |
|
|
|
98 |
|
|
for (Vector::Index i = 0; i < SIZE; ++i) { |
99 |
|
|
res(i) = UJL(i) - LJL(i); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
sotDEBUGOUT(15); |
103 |
|
|
return res; |
104 |
|
|
} |
105 |
|
|
|
106 |
|
|
/** Compute the interaction matrix from a subset of |
107 |
|
|
* the possible features. |
108 |
|
|
*/ |
109 |
|
|
Matrix &FeatureJointLimits::computeJacobian(Matrix &J, int time) { |
110 |
|
|
sotDEBUG(15) << "# In {" << endl; |
111 |
|
|
|
112 |
|
|
const unsigned int SIZE = dimensionSOUT.access(time); |
113 |
|
|
const Vector q = jointSIN.access(time); |
114 |
|
|
const Flags &fl = selectionSIN(time); |
115 |
|
|
// const unsigned int SIZE_FF=SIZE+freeFloatingSize; |
116 |
|
|
const Vector::Index SIZE_TOTAL = q.size(); |
117 |
|
|
const Vector WJL = widthJlSINTERN.access(time); |
118 |
|
|
J.resize(SIZE, SIZE_TOTAL); |
119 |
|
|
J.setZero(); |
120 |
|
|
|
121 |
|
|
unsigned int idx = 0; |
122 |
|
|
for (unsigned int i = 0; i < SIZE_TOTAL; ++i) { |
123 |
|
|
if (fl(i)) { |
124 |
|
|
if (fabs(WJL(i)) > 1e-3) |
125 |
|
|
J(idx, i) = 1 / WJL(i); |
126 |
|
|
else |
127 |
|
|
J(idx, i) = 1.; |
128 |
|
|
idx++; |
129 |
|
|
} |
130 |
|
|
} |
131 |
|
|
// if( 0!=freeFloatingIndex ) |
132 |
|
|
// for( unsigned int i=0;i<freeFloatingIndex;++i ) |
133 |
|
|
// { |
134 |
|
|
// if( fabs(WJL(i))>1e-3 ) J(i,i)=1/WJL(i); else J(i,i)=1.; |
135 |
|
|
// } |
136 |
|
|
|
137 |
|
|
// if( SIZE!=freeFloatingIndex ) |
138 |
|
|
// for( unsigned int i=freeFloatingIndex;i<SIZE;++i ) |
139 |
|
|
// { |
140 |
|
|
// if( fabs(WJL(i))>1e-3 ) J(i,i+freeFloatingSIZE)=1/WJL(i); |
141 |
|
|
// else J(i,i)=1.; |
142 |
|
|
// } |
143 |
|
|
|
144 |
|
|
sotDEBUG(15) << "# Out }" << endl; |
145 |
|
|
return J; |
146 |
|
|
} |
147 |
|
|
|
148 |
|
|
/** Compute the error between two visual features from a subset |
149 |
|
|
* a the possible features. |
150 |
|
|
*/ |
151 |
|
|
Vector &FeatureJointLimits::computeError(Vector &error, int time) { |
152 |
|
|
sotDEBUGIN(15); |
153 |
|
|
|
154 |
|
|
const Flags &fl = selectionSIN(time); |
155 |
|
|
const Vector q = jointSIN.access(time); |
156 |
|
|
const Vector UJL = upperJlSIN.access(time); |
157 |
|
|
const Vector LJL = lowerJlSIN.access(time); |
158 |
|
|
const Vector WJL = widthJlSINTERN.access(time); |
159 |
|
|
const int SIZE = dimensionSOUT.access(time); |
160 |
|
|
const Vector::Index SIZE_TOTAL = q.size(); |
161 |
|
|
|
162 |
|
|
sotDEBUG(25) << "q = " << q << endl; |
163 |
|
|
sotDEBUG(25) << "ljl = " << LJL << endl; |
164 |
|
|
sotDEBUG(25) << "Wjl = " << WJL << endl; |
165 |
|
|
sotDEBUG(25) << "dim = " << SIZE << endl; |
166 |
|
|
|
167 |
|
|
assert(UJL.size() == SIZE_TOTAL); |
168 |
|
|
assert(WJL.size() == SIZE_TOTAL); |
169 |
|
|
assert(LJL.size() == SIZE_TOTAL); |
170 |
|
|
assert(SIZE <= SIZE_TOTAL); |
171 |
|
|
|
172 |
|
|
error.resize(SIZE); |
173 |
|
|
|
174 |
|
|
unsigned int parcerr = 0; |
175 |
|
|
for (int i = 0; i < SIZE_TOTAL; ++i) { |
176 |
|
|
if (fl(i)) { |
177 |
|
|
error(parcerr++) = (q(i) - LJL(i)) / WJL(i) * 2 - 1; |
178 |
|
|
} |
179 |
|
|
} |
180 |
|
|
|
181 |
|
|
sotDEBUGOUT(15); |
182 |
|
|
return error; |
183 |
|
|
} |
184 |
|
|
|
185 |
|
|
void FeatureJointLimits::display(std::ostream &os) const { |
186 |
|
|
os << "JointLimits <" << name << "> ... TODO"; |
187 |
|
|
} |