1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2007 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: NextStepPgSot.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 <dynamic-graph/pool.h> |
22 |
|
|
#include <sot/pattern-generator/exception-pg.h> |
23 |
|
|
#include <sot/pattern-generator/next-step-pg-sot.h> |
24 |
|
|
|
25 |
|
|
#include <algorithm> |
26 |
|
|
#include <cmath> |
27 |
|
|
#include <sot/core/debug.hh> |
28 |
|
|
#include <sot/core/macros-signal.hh> |
29 |
|
|
|
30 |
|
|
#define PI 3.1416 |
31 |
|
|
|
32 |
|
|
namespace dg = dynamicgraph; |
33 |
|
|
namespace dynamicgraph { |
34 |
|
|
namespace sot { |
35 |
|
|
|
36 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NextStepPgSot, "NextStepPgSot"); |
37 |
|
|
|
38 |
|
|
/* --- CONSTRUCT ----------------------------------------------------- */ |
39 |
|
|
/* --- CONSTRUCT ----------------------------------------------------- */ |
40 |
|
|
/* --- CONSTRUCT ----------------------------------------------------- */ |
41 |
|
|
|
42 |
|
|
NextStepPgSot::NextStepPgSot(const std::string &name) : NextStep(name) { |
43 |
|
|
sotDEBUGIN(5); |
44 |
|
|
m_StepModificationMode = NextStepPgSot::ADDING_STEP; |
45 |
|
|
m_NextStepTime = -1.0; |
46 |
|
|
m_NbOfFirstSteps = 0; |
47 |
|
|
m_PGI = 0; |
48 |
|
|
sotDEBUGOUT(5); |
49 |
|
|
|
50 |
|
|
stepbuf.reserve(10000); |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
/* --- FUNCTIONS ------------------------------------------------------- */ |
54 |
|
|
/* --- FUNCTIONS ------------------------------------------------------- */ |
55 |
|
|
|
56 |
|
|
void positionClipper(double x, double y, double &x_result, double &y_result) { |
57 |
|
|
const double MIN_y = 0.16; |
58 |
|
|
const double MAX_y = 0.40; |
59 |
|
|
const double MAX_x = 0.25; |
60 |
|
|
const double EDGE_POINT_x = 0.20; |
61 |
|
|
const double EDGE_POINT_y = 0.30; |
62 |
|
|
|
63 |
|
|
double x0 = std::abs(x); |
64 |
|
|
double y0 = std::abs(y); |
65 |
|
|
double alpha = y0 / x0; |
66 |
|
|
double y_inter = MIN_y; |
67 |
|
|
double x_inter = MIN_y / alpha; |
68 |
|
|
|
69 |
|
|
if (alpha < MIN_y / MAX_x) { |
70 |
|
|
x_result = MAX_x; |
71 |
|
|
y_result = MIN_y; |
72 |
|
|
} else if (alpha < EDGE_POINT_y / EDGE_POINT_x) { |
73 |
|
|
double delta1 = (MIN_y - EDGE_POINT_y) / (MAX_x - EDGE_POINT_x); |
74 |
|
|
double x_inter2 = (EDGE_POINT_y - delta1 * EDGE_POINT_x) / (alpha - delta1); |
75 |
|
|
double y_inter2 = alpha * x_inter2; |
76 |
|
|
if (x0 < x_inter) { |
77 |
|
|
x_result = x_inter; |
78 |
|
|
y_result = y_inter; |
79 |
|
|
} else if (x0 > x_inter2) { |
80 |
|
|
x_result = x_inter2; |
81 |
|
|
y_result = y_inter2; |
82 |
|
|
} else { |
83 |
|
|
x_result = x0; |
84 |
|
|
y_result = y0; |
85 |
|
|
} |
86 |
|
|
} else { |
87 |
|
|
double delta2 = (EDGE_POINT_y - MAX_y) / (EDGE_POINT_x); |
88 |
|
|
double x_inter2 = (MAX_y) / (alpha - delta2); |
89 |
|
|
double y_inter2 = alpha * x_inter2; |
90 |
|
|
if (x0 < x_inter) { |
91 |
|
|
x_result = x_inter; |
92 |
|
|
y_result = y_inter; |
93 |
|
|
} else if (x0 > x_inter2) { |
94 |
|
|
x_result = x_inter2; |
95 |
|
|
y_result = y_inter2; |
96 |
|
|
} else { |
97 |
|
|
x_result = x0; |
98 |
|
|
y_result = y0; |
99 |
|
|
} |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
if (x < 0) x_result = -x_result; |
103 |
|
|
if (y < 0) y_result = -y_result; |
104 |
|
|
} |
105 |
|
|
|
106 |
|
|
/* --- FUNCTIONS -------------------------------------------------- */ |
107 |
|
|
|
108 |
|
|
void NextStepPgSot::starter(const int &timeCurr) { |
109 |
|
|
sotDEBUGIN(15); |
110 |
|
|
|
111 |
|
|
NextStep::starter(timeCurr); |
112 |
|
|
|
113 |
|
|
if (pgEntity) { |
114 |
|
|
std::ostringstream cmdstd; |
115 |
|
|
cmdstd << ":StartOnLineStepSequencing "; |
116 |
|
|
for (std::deque<FootPrint>::const_iterator iter = footPrintList.begin(); |
117 |
|
|
iter != footPrintList.end(); ++iter) { |
118 |
|
|
cmdstd << iter->x << " " << iter->y << " " << iter->theta << " "; |
119 |
|
|
} |
120 |
|
|
std::istringstream cmdArg(cmdstd.str()); |
121 |
|
|
std::istringstream emptyArg; |
122 |
|
|
m_sPG->InitState(); |
123 |
|
|
m_sPG->pgCommandLine(cmdArg.str()); |
124 |
|
|
sotDEBUG(15) << "Cmd: " << cmdstd.str() << std::endl; |
125 |
|
|
} else { |
126 |
|
|
sotERROR << "PG not set" << std::endl; |
127 |
|
|
} |
128 |
|
|
|
129 |
|
|
state = STATE_STARTED; |
130 |
|
|
|
131 |
|
|
sotDEBUGOUT(15); |
132 |
|
|
return; |
133 |
|
|
} |
134 |
|
|
|
135 |
|
|
void NextStepPgSot::stoper(const int &) { |
136 |
|
|
sotDEBUGIN(15); |
137 |
|
|
|
138 |
|
|
if (pgEntity) { |
139 |
|
|
std::ostringstream cmdstd; |
140 |
|
|
std::ostringstream os; |
141 |
|
|
cmdstd << ":StopOnLineStepSequencing"; |
142 |
|
|
std::istringstream cmdArg(cmdstd.str()); |
143 |
|
|
m_sPG->pgCommandLine(cmdArg.str()); |
144 |
|
|
} else { |
145 |
|
|
sotERROR << "PG not set" << std::endl; |
146 |
|
|
} |
147 |
|
|
|
148 |
|
|
m_NbOfFirstSteps = 0; |
149 |
|
|
state = STATE_STOPED; |
150 |
|
|
|
151 |
|
|
sotDEBUGOUT(15); |
152 |
|
|
return; |
153 |
|
|
} |
154 |
|
|
|
155 |
|
|
void NextStepPgSot::introductionCallBack(const int &timeCurr) { |
156 |
|
|
sotDEBUGIN(15); |
157 |
|
|
|
158 |
|
|
if (state == STATE_STARTED) { |
159 |
|
|
FootPrint &lastStep = footPrintList.back(); |
160 |
|
|
if (pgEntity) { |
161 |
|
|
if (m_StepModificationMode == NextStepPgSot::ADDING_STEP) { |
162 |
|
|
std::string cmdLine = "addStep"; |
163 |
|
|
std::ostringstream cmdArgIn; |
164 |
|
|
std::ostringstream os; |
165 |
|
|
cmdArgIn << cmdLine << " " << lastStep.x << " " << lastStep.y << " " |
166 |
|
|
<< lastStep.theta; |
167 |
|
|
std::istringstream cmdArg(cmdArgIn.str()); |
168 |
|
|
m_sPG->pgCommandLine(cmdArgIn.str()); |
169 |
|
|
} else if (m_StepModificationMode == NextStepPgSot::CHANGING_STEP) { |
170 |
|
|
if (m_NbOfFirstSteps < 3) |
171 |
|
|
m_NbOfFirstSteps++; |
172 |
|
|
else { |
173 |
|
|
sotDEBUG(15) << "m_NextStepTime: " << m_NextStepTime << std::endl; |
174 |
|
|
|
175 |
|
|
PatternGeneratorJRL::FootAbsolutePosition aFAP; |
176 |
|
|
|
177 |
|
|
int lSupportFoot = 0; |
178 |
|
|
|
179 |
|
|
lSupportFoot = m_sPG->SupportFootSOUT(timeCurr); |
180 |
|
|
|
181 |
|
|
double NextFAPx = lastStep.x; |
182 |
|
|
double NextFAPy = lastStep.y; |
183 |
|
|
double NextFAPtheta = lastStep.theta; |
184 |
|
|
if (1) { |
185 |
|
|
NextFAPx = 0.80 * (double)rand() / (double)RAND_MAX - 0.40; |
186 |
|
|
NextFAPy = 0.80 * (double)rand() / (double)RAND_MAX - 0.40; |
187 |
|
|
NextFAPtheta = 18.0 * (double)rand() / (double)RAND_MAX - 6.0; |
188 |
|
|
} else { |
189 |
|
|
NextFAPx = 0.2; |
190 |
|
|
NextFAPy = 0.0; |
191 |
|
|
NextFAPtheta = 45.0; |
192 |
|
|
} |
193 |
|
|
|
194 |
|
|
/* Warning : HARDCODED VALUES */ |
195 |
|
|
#if 1 |
196 |
|
|
if (0 && mode == MODE_1D) { |
197 |
|
|
const double LIMIT_X = 0.20; |
198 |
|
|
if (NextFAPx < -LIMIT_X) NextFAPx = -LIMIT_X; |
199 |
|
|
if (NextFAPx > LIMIT_X) NextFAPx = LIMIT_X; |
200 |
|
|
NextFAPtheta = 0.; |
201 |
|
|
NextFAPy = 0.; |
202 |
|
|
} else { |
203 |
|
|
// const double LIMIT_X = 0.10; |
204 |
|
|
// const double LIMIT_Y_SUP = 0.05; |
205 |
|
|
// const double LIMIT_Y_INF = 0.03; |
206 |
|
|
const double THETA_MAX = 9.; |
207 |
|
|
|
208 |
|
|
if (NextFAPtheta < -THETA_MAX) NextFAPtheta = -THETA_MAX; |
209 |
|
|
if (NextFAPtheta > THETA_MAX) NextFAPtheta = THETA_MAX; |
210 |
|
|
|
211 |
|
|
if (lSupportFoot == 1) { |
212 |
|
|
if (NextFAPy < 0) NextFAPy = -NextFAPy; |
213 |
|
|
double Next_xResult; |
214 |
|
|
double Next_yResult; |
215 |
|
|
positionClipper(NextFAPx, NextFAPy, Next_xResult, Next_yResult); |
216 |
|
|
NextFAPx = cos(NextFAPtheta * PI / 180) * Next_xResult + |
217 |
|
|
sin(NextFAPtheta * PI / 180) * Next_yResult; |
218 |
|
|
NextFAPy = -sin(NextFAPtheta * PI / 180) * Next_xResult + |
219 |
|
|
cos(NextFAPtheta * PI / 180) * Next_yResult; |
220 |
|
|
|
221 |
|
|
// This is the right foot support. |
222 |
|
|
// Make sure this is relative to the |
223 |
|
|
// next stepping value. |
224 |
|
|
std::cout << NextFAPx << " " << NextFAPy << " " << NextFAPtheta |
225 |
|
|
<< std::endl; |
226 |
|
|
NextFAPy = NextFAPy - 0.19; |
227 |
|
|
} else { |
228 |
|
|
if (NextFAPy > 0) NextFAPy = -NextFAPy; |
229 |
|
|
double Next_xResult; |
230 |
|
|
double Next_yResult; |
231 |
|
|
positionClipper(NextFAPx, NextFAPy, Next_xResult, Next_yResult); |
232 |
|
|
NextFAPx = cos(NextFAPtheta * PI / 180) * Next_xResult + |
233 |
|
|
sin(NextFAPtheta * PI / 180) * Next_yResult; |
234 |
|
|
NextFAPy = -sin(NextFAPtheta * PI / 180) * Next_xResult + |
235 |
|
|
cos(NextFAPtheta * PI / 180) * Next_yResult; |
236 |
|
|
|
237 |
|
|
// This is the left foot support. |
238 |
|
|
// Make sure this is relative to the |
239 |
|
|
// next stepping value. |
240 |
|
|
std::cout << NextFAPx << " " << NextFAPy << " " << NextFAPtheta |
241 |
|
|
<< std::endl; |
242 |
|
|
NextFAPy = NextFAPy + 0.19; |
243 |
|
|
} |
244 |
|
|
} |
245 |
|
|
#endif |
246 |
|
|
/* End of Warning : HARDCODED VALUES */ |
247 |
|
|
aFAP.x = NextFAPx; |
248 |
|
|
aFAP.y = NextFAPy; |
249 |
|
|
aFAP.theta = NextFAPtheta; |
250 |
|
|
// std::cout << lSupportFoot << std::endl; |
251 |
|
|
// std::cout << "FAP = ( "<< aFAP.x << " " << aFAP.y |
252 |
|
|
// << " " << aFAP.z << " ) " << std::endl; |
253 |
|
|
stepbuf.push_back(std::make_pair(lSupportFoot, aFAP)); |
254 |
|
|
m_NextStepTime = 0.0; |
255 |
|
|
|
256 |
|
|
m_PGI->ChangeOnLineStep(0.805, aFAP, m_NextStepTime); |
257 |
|
|
|
258 |
|
|
if (m_NextStepTime < 0.0) |
259 |
|
|
period = NextStep::PERIOD_DEFAULT; |
260 |
|
|
else |
261 |
|
|
period = (unsigned int)((m_NextStepTime + 0.025) * 200.0); |
262 |
|
|
} |
263 |
|
|
} |
264 |
|
|
} else { |
265 |
|
|
sotERROR << "Walk plugin not found. " << std::endl; |
266 |
|
|
/* TODO Error: PG not set. */ |
267 |
|
|
} |
268 |
|
|
} |
269 |
|
|
|
270 |
|
|
sotDEBUGOUT(15); |
271 |
|
|
return; |
272 |
|
|
} |
273 |
|
|
|
274 |
|
|
void NextStepPgSot::commandLine(const std::string &cmdLine, |
275 |
|
|
std::istringstream &cmdArgs, std::ostream &os) { |
276 |
|
|
if ("help" == cmdLine) { |
277 |
|
|
os << "NextStepPgSot: " << std::endl |
278 |
|
|
<< " - initPg [<pg_name>]" << std::endl |
279 |
|
|
<< " - stepmodificationmode [add|change]" << std::endl; |
280 |
|
|
NextStep::commandLine(cmdLine, cmdArgs, os); |
281 |
|
|
} else if ("initPg" == cmdLine) { |
282 |
|
|
std::string name = "pg"; |
283 |
|
|
cmdArgs >> std::ws; |
284 |
|
|
if (cmdArgs.good()) cmdArgs >> name; |
285 |
|
|
pgEntity = &(PoolStorage::getInstance()->getEntity(name)); |
286 |
|
|
m_sPG = dynamic_cast<PatternGenerator *>(pgEntity); |
287 |
|
|
if (m_sPG != 0) m_PGI = m_sPG->GetPatternGeneratorInterface(); |
288 |
|
|
} else if ("stepmodificationmode" == cmdLine) { |
289 |
|
|
std::string stgmode; |
290 |
|
|
cmdArgs >> std::ws; |
291 |
|
|
if (cmdArgs.good()) { |
292 |
|
|
cmdArgs >> stgmode; |
293 |
|
|
if (stgmode == "add") |
294 |
|
|
m_StepModificationMode = NextStepPgSot::ADDING_STEP; |
295 |
|
|
else if (stgmode == "change") |
296 |
|
|
m_StepModificationMode = NextStepPgSot::CHANGING_STEP; |
297 |
|
|
} else { |
298 |
|
|
if (m_StepModificationMode == NextStepPgSot::ADDING_STEP) |
299 |
|
|
os << "add"; |
300 |
|
|
else if (m_StepModificationMode == NextStepPgSot::CHANGING_STEP) |
301 |
|
|
os << "change"; |
302 |
|
|
} |
303 |
|
|
} else if ("savesteps" == cmdLine) { |
304 |
|
|
std::ofstream os("/tmp/steps.dat"); |
305 |
|
|
for (size_t i = 0; i < stepbuf.size(); ++i) { |
306 |
|
|
os << stepbuf[i].first << " " << stepbuf[i].second.x << " " |
307 |
|
|
<< stepbuf[i].second.y << " " << stepbuf[i].second.z << "\n"; |
308 |
|
|
} |
309 |
|
|
os << std::endl; |
310 |
|
|
stepbuf.clear(); |
311 |
|
|
} else { |
312 |
|
|
NextStep::commandLine(cmdLine, cmdArgs, os); |
313 |
|
|
} |
314 |
|
|
} |
315 |
|
|
|
316 |
|
|
} // namespace sot |
317 |
|
|
} // namespace dynamicgraph |