1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2007 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: WhichFootUpper.h |
6 |
|
|
* Project: SOT |
7 |
|
|
* Author: Nicolas Mansard |
8 |
|
|
* |
9 |
|
|
* Version control |
10 |
|
|
* =============== |
11 |
|
|
* |
12 |
|
|
* $Id$ |
13 |
|
|
* |
14 |
|
|
* Description |
15 |
|
|
* ============ |
16 |
|
|
* |
17 |
|
|
* |
18 |
|
|
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
19 |
|
|
|
20 |
|
|
#include <dynamic-graph/factory.h> |
21 |
|
|
#include <sot/pattern-generator/which-foot-upper.h> |
22 |
|
|
|
23 |
|
|
#include <sot/core/debug.hh> |
24 |
|
|
#include <sot/core/macros-signal.hh> |
25 |
|
|
// #include <sot/pattern-generator/exception-pg.h> |
26 |
|
|
|
27 |
|
|
namespace dynamicgraph { |
28 |
|
|
namespace sot { |
29 |
|
|
|
30 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WhichFootUpper, "WhichFootUpper"); |
31 |
|
|
|
32 |
|
|
const unsigned int WhichFootUpper::INDEX_LEFT_FOOT_DEFAULT = 0; |
33 |
|
|
const unsigned int WhichFootUpper::INDEX_RIGHT_FOOT_DEFAULT = 1; |
34 |
|
|
|
35 |
|
|
const double WhichFootUpper::TRIGGER_THRESHOLD_DEFAULT = 5e-4; |
36 |
|
|
typedef Eigen::Matrix<double, 4, 4> &(MatrixHomogeneous::*ExtractMemberType)( |
37 |
|
|
void) const; |
38 |
|
|
|
39 |
|
|
WhichFootUpper::WhichFootUpper(const std::string &name) |
40 |
|
|
: Entity(name), |
41 |
|
|
indexLeftFoot(INDEX_LEFT_FOOT_DEFAULT), |
42 |
|
|
indexRightFoot(INDEX_RIGHT_FOOT_DEFAULT), |
43 |
|
|
triggerThreshold(TRIGGER_THRESHOLD_DEFAULT), |
44 |
|
|
lastFoot(indexLeftFoot) |
45 |
|
|
|
46 |
|
|
, |
47 |
|
|
waistRsensorSIN(NULL, "WhichFootUpper(" + name + |
48 |
|
|
")::input(matrixRotation)::waistRsensor"), |
49 |
|
|
worldRsensorSIN(NULL, "WhichFootUpper(" + name + |
50 |
|
|
")::input(matrixRotation)::worldRsensor"), |
51 |
|
|
waistMlfootSIN(NULL, "WhichFootUpper(" + name + |
52 |
|
|
")::input(matrixhomogeneous)::waistMlfoot"), |
53 |
|
|
waistMrfootSIN(NULL, "WhichFootUpper(" + name + |
54 |
|
|
")::input(matrixhomogeneous)::waistMrfoot") |
55 |
|
|
|
56 |
|
|
, |
57 |
|
|
worldMlfootSOUT( |
58 |
|
|
SOT_INIT_SIGNAL_3(WhichFootUpper::computeFootPosition, waistMlfootSIN, |
59 |
|
|
MatrixHomogeneous, waistRsensorSIN, MatrixRotation, |
60 |
|
|
worldRsensorSIN, MatrixRotation), |
61 |
|
|
"WhichFootUpper(" + name + |
62 |
|
|
")::output(MatrixHomogeneous)::worldMlfoot"), |
63 |
|
|
worldMrfootSOUT( |
64 |
|
|
SOT_INIT_SIGNAL_3(WhichFootUpper::computeFootPosition, waistMrfootSIN, |
65 |
|
|
MatrixHomogeneous, waistRsensorSIN, MatrixRotation, |
66 |
|
|
worldRsensorSIN, MatrixRotation), |
67 |
|
|
"WhichFootUpper(" + name + |
68 |
|
|
")::output(MatrixHomogeneous)::worldMrfoot"), |
69 |
|
|
whichFootSOUT(SOT_MEMBER_SIGNAL_2(WhichFootUpper::whichFoot, |
70 |
|
|
waistMlfootSIN, MatrixHomogeneous, |
71 |
|
|
waistMrfootSIN, MatrixHomogeneous), |
72 |
|
|
"WhichFootUpper(" + name + ")::output(uint)::whichFoot") |
73 |
|
|
|
74 |
|
|
, |
75 |
|
|
waistMsensorSIN(NULL, "WhichFootUpper(" + name + |
76 |
|
|
")::input(matrixRotation)::waistMsensor"), |
77 |
|
|
waistRsensorSOUT( |
78 |
|
|
boost::bind(&WhichFootUpper::computeRotationMatrix, this, _1, _2), |
79 |
|
|
waistMsensorSIN, |
80 |
|
|
"WhichFootUpper(" + name + |
81 |
|
|
")::output(MatrixHomogeneous)::waistRsensorOUT") { |
82 |
|
|
sotDEBUGIN(5); |
83 |
|
|
signalRegistration(whichFootSOUT << waistRsensorSIN << worldRsensorSIN |
84 |
|
|
<< waistMlfootSIN << waistMrfootSIN |
85 |
|
|
<< worldMlfootSOUT << worldMrfootSOUT |
86 |
|
|
<< waistMsensorSIN << waistRsensorSOUT); |
87 |
|
|
waistRsensorSIN.plug(&waistRsensorSOUT); |
88 |
|
|
sotDEBUGOUT(5); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
WhichFootUpper::~WhichFootUpper(void) { |
92 |
|
|
sotDEBUGINOUT(5); |
93 |
|
|
return; |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
/* --- SIGNALS ------------------------------------------------------ */ |
97 |
|
|
/* --- SIGNALS ------------------------------------------------------ */ |
98 |
|
|
/* --- SIGNALS ------------------------------------------------------ */ |
99 |
|
|
MatrixRotation &WhichFootUpper::computeRotationMatrix(MatrixRotation &rotMat, |
100 |
|
|
int time) { |
101 |
|
|
MatrixHomogeneous mh = waistMsensorSIN(time); |
102 |
|
|
sotDEBUGIN(15); |
103 |
|
|
rotMat.resize(3, 3); |
104 |
|
|
rotMat = mh.linear(); |
105 |
|
|
return rotMat; |
106 |
|
|
} |
107 |
|
|
|
108 |
|
|
MatrixHomogeneous &WhichFootUpper::computeFootPosition( |
109 |
|
|
const MatrixHomogeneous &waistMfoot, const MatrixRotation &waistRsensor, |
110 |
|
|
const MatrixRotation &worldRsensor, MatrixHomogeneous &worldMfoot) { |
111 |
|
|
sotDEBUGIN(15); |
112 |
|
|
|
113 |
|
|
MatrixRotation worldRwaist; |
114 |
|
|
worldRwaist = worldRsensor * waistRsensor.transpose(); |
115 |
|
|
|
116 |
|
|
MatrixHomogeneous worldMwaist; |
117 |
|
|
worldMwaist.translation().setZero(); |
118 |
|
|
worldMwaist.linear() = worldRwaist; |
119 |
|
|
|
120 |
|
|
worldMfoot = worldMwaist * waistMfoot; |
121 |
|
|
|
122 |
|
|
sotDEBUGOUT(15); |
123 |
|
|
return worldMfoot; |
124 |
|
|
} |
125 |
|
|
|
126 |
|
|
unsigned int &WhichFootUpper::whichFoot(const MatrixHomogeneous &waistMlfoot, |
127 |
|
|
const MatrixHomogeneous &waistMrfoot, |
128 |
|
|
unsigned int &res) { |
129 |
|
|
sotDEBUGIN(15); |
130 |
|
|
|
131 |
|
|
const double &leftAltitude = waistMlfoot(2, 3); |
132 |
|
|
const double &rightAltitude = waistMrfoot(2, 3); |
133 |
|
|
|
134 |
|
|
if (lastFoot == indexRightFoot) { |
135 |
|
|
if (rightAltitude - triggerThreshold < leftAltitude) { |
136 |
|
|
res = lastFoot; |
137 |
|
|
} else { |
138 |
|
|
res = lastFoot = indexLeftFoot; |
139 |
|
|
} |
140 |
|
|
} else { |
141 |
|
|
if (leftAltitude - triggerThreshold < rightAltitude) { |
142 |
|
|
res = lastFoot = indexLeftFoot; |
143 |
|
|
} else { |
144 |
|
|
res = lastFoot = indexRightFoot; |
145 |
|
|
} |
146 |
|
|
} |
147 |
|
|
|
148 |
|
|
sotDEBUGOUT(15); |
149 |
|
|
return res; |
150 |
|
|
} |
151 |
|
|
|
152 |
|
|
/* --- PARAMS -------------------------------------------------- */ |
153 |
|
|
/* --- PARAMS -------------------------------------------------- */ |
154 |
|
|
/* --- PARAMS -------------------------------------------------- */ |
155 |
|
|
|
156 |
|
|
void WhichFootUpper::commandLine(const std::string &cmdLine, |
157 |
|
|
std::istringstream &cmdArgs, |
158 |
|
|
std::ostream &os) { |
159 |
|
|
if (cmdLine == "help") { |
160 |
|
|
os << "WhichFootUpper: " << std::endl |
161 |
|
|
<< " - index {left|right} [<value>]: get/set the foot indeces." |
162 |
|
|
<< std::endl |
163 |
|
|
<< " - trigger [<value>]: get/set the trigger threshold. " << std::endl; |
164 |
|
|
} else if (cmdLine == "index") { |
165 |
|
|
std::string foot; |
166 |
|
|
cmdArgs >> foot >> std::ws; |
167 |
|
|
unsigned int *classIndex = NULL; |
168 |
|
|
if (foot == "left") { |
169 |
|
|
classIndex = &indexLeftFoot; |
170 |
|
|
} else if (foot == "right") { |
171 |
|
|
classIndex = &indexRightFoot; |
172 |
|
|
} else { |
173 |
|
|
os << "Error. Usage is: index {left|right} [<value>]" << std::endl; |
174 |
|
|
return; |
175 |
|
|
} |
176 |
|
|
|
177 |
|
|
if (cmdArgs.good()) { |
178 |
|
|
cmdArgs >> (*classIndex); |
179 |
|
|
} else { |
180 |
|
|
os << "index = " << (*classIndex) << std::endl; |
181 |
|
|
} |
182 |
|
|
|
183 |
|
|
} else if (cmdLine == "trigger") { |
184 |
|
|
cmdArgs >> std::ws; |
185 |
|
|
if (cmdArgs.good()) { |
186 |
|
|
cmdArgs >> triggerThreshold; |
187 |
|
|
} else { |
188 |
|
|
os << "trigger = " << triggerThreshold << std::endl; |
189 |
|
|
} |
190 |
|
|
} else { |
191 |
|
|
} |
192 |
|
|
} |
193 |
|
|
} // namespace sot |
194 |
|
|
} // namespace dynamicgraph |