1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2009 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: StepComputerJoystick.cpp |
6 |
|
|
* Project: SOT |
7 |
|
|
* Author: Olivier Stasse |
8 |
|
|
* |
9 |
|
|
* Version control |
10 |
|
|
* =============== |
11 |
|
|
* |
12 |
|
|
* $Id$ |
13 |
|
|
* |
14 |
|
|
* Description |
15 |
|
|
* ============ |
16 |
|
|
* |
17 |
|
|
* |
18 |
|
|
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
19 |
|
|
|
20 |
|
|
#include <time.h> |
21 |
|
|
|
22 |
|
|
#include <cmath> |
23 |
|
|
#ifndef WIN32 |
24 |
|
|
#include <sys/time.h> |
25 |
|
|
|
26 |
|
|
#include <iostream> |
27 |
|
|
#else |
28 |
|
|
#include <Winsock2.h> |
29 |
|
|
|
30 |
|
|
#include <sot/core/utils-windows.hh> |
31 |
|
|
#endif /*WIN32*/ |
32 |
|
|
|
33 |
|
|
#include <dynamic-graph/factory.h> |
34 |
|
|
#include <sot/pattern-generator/exception-pg.h> |
35 |
|
|
#include <sot/pattern-generator/step-checker.h> |
36 |
|
|
#include <sot/pattern-generator/step-computer-joystick.h> |
37 |
|
|
#include <sot/pattern-generator/step-queue.h> |
38 |
|
|
|
39 |
|
|
#include <sot/core/debug.hh> |
40 |
|
|
#include <sot/core/macros-signal.hh> |
41 |
|
|
|
42 |
|
|
namespace dynamicgraph { |
43 |
|
|
namespace sot { |
44 |
|
|
|
45 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepComputerJoystick, |
46 |
|
|
"StepComputerJoystick"); |
47 |
|
|
|
48 |
|
|
StepComputerJoystick::StepComputerJoystick(const std::string &name) |
49 |
|
|
: Entity(name), |
50 |
|
|
joystickSIN(NULL, "StepComputerJoystick(" + name + |
51 |
|
|
")::input(vector)::joystickin"), |
52 |
|
|
contactFootSIN( |
53 |
|
|
NULL, "StepComputerJoystick(" + name + ")::input(uint)::contactfoot"), |
54 |
|
|
laststepSOUT( |
55 |
|
|
boost::bind(&StepComputerJoystick::getlaststep, this, _1, _2), |
56 |
|
|
joystickSIN, |
57 |
|
|
"StepComputerJoystick(" + name + ")::output(vector)::laststep"), |
58 |
|
|
checker(), |
59 |
|
|
logChanges("/tmp/stepcomp_changes.dat"), |
60 |
|
|
logPreview("/tmp/stepcomp_preview.dat") { |
61 |
|
|
sotDEBUGIN(5); |
62 |
|
|
|
63 |
|
|
signalRegistration(joystickSIN); |
64 |
|
|
signalRegistration(laststepSOUT << contactFootSIN); |
65 |
|
|
|
66 |
|
|
sotDEBUGOUT(5); |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
void StepComputerJoystick::nextStep(StepQueue &queue, int timeCurr) { |
70 |
|
|
// Introduce new step at the end of the preview window. |
71 |
|
|
if (queue.getLastStep().contact == CONTACT_LEFT_FOOT) { |
72 |
|
|
queue.pushStep(0., -queue.getZeroStepPosition(), 0.); |
73 |
|
|
logPreview << timeCurr << " " << 0 << " " << -queue.getZeroStepPosition() |
74 |
|
|
<< " " << 0 << std::endl; |
75 |
|
|
} else { |
76 |
|
|
queue.pushStep(0., queue.getZeroStepPosition(), 0.); |
77 |
|
|
logPreview << timeCurr << " " << 0 << " " << queue.getZeroStepPosition() |
78 |
|
|
<< " " << 0 << std::endl; |
79 |
|
|
} |
80 |
|
|
} |
81 |
|
|
|
82 |
|
|
void StepComputerJoystick::changeFirstStep(StepQueue &queue, int timeCurr) { |
83 |
|
|
logChanges << timeCurr << " changeFirstStep" << std::endl; |
84 |
|
|
|
85 |
|
|
const FootPrint &step = queue.getFirstStep(); |
86 |
|
|
|
87 |
|
|
Vector joyin; |
88 |
|
|
joyin.resize(3); |
89 |
|
|
try { |
90 |
|
|
joyin = joystickSIN(timeCurr); |
91 |
|
|
|
92 |
|
|
} catch (...) { |
93 |
|
|
joyin(0) = 0; |
94 |
|
|
joyin(1) = 0; |
95 |
|
|
joyin(2) = 0; |
96 |
|
|
std::cerr << "No joystick input stay on the spot" << std::endl; |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
double x = step.x + joyin(0), y = step.y + joyin(1); |
100 |
|
|
double theta = step.theta + joyin(2); |
101 |
|
|
|
102 |
|
|
std::cout << "stopStepComputedJoystick::changeFirstStep: " << x << " " << y |
103 |
|
|
<< " " << theta << std::endl; |
104 |
|
|
const double THETA_MAX = 9.; |
105 |
|
|
if (theta < -THETA_MAX) { |
106 |
|
|
theta = -THETA_MAX; |
107 |
|
|
} |
108 |
|
|
if (theta > THETA_MAX) { |
109 |
|
|
theta = THETA_MAX; |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
// double nx = 0, ny = 0; |
113 |
|
|
|
114 |
|
|
// checker.clipStep(x, y, nx, ny); |
115 |
|
|
|
116 |
|
|
// Log x-y values before and after clipping |
117 |
|
|
|
118 |
|
|
// logChanges << timeCurr << " " << x << " " << y |
119 |
|
|
// << " " << nx << " " << ny << " "; |
120 |
|
|
|
121 |
|
|
// The coordinates must be expressed in the destination foot frame. |
122 |
|
|
// See the technical report of Olivier Stasse for more details, |
123 |
|
|
// on top of page 79. |
124 |
|
|
|
125 |
|
|
double theta_rad = 3.14159265 * theta / 180.; |
126 |
|
|
double ctheta = cos(theta_rad); |
127 |
|
|
double stheta = sin(theta_rad); |
128 |
|
|
|
129 |
|
|
x = x * ctheta + y * stheta; |
130 |
|
|
y = -x * stheta + y * ctheta; |
131 |
|
|
|
132 |
|
|
queue.changeFirstStep(x, y, theta); |
133 |
|
|
|
134 |
|
|
// Log the step |
135 |
|
|
m_laststep[0] = x; |
136 |
|
|
m_laststep[1] = y; |
137 |
|
|
m_laststep[2] = theta; |
138 |
|
|
|
139 |
|
|
// logChanges << x << " " << y << " " << theta << std::endl; |
140 |
|
|
} |
141 |
|
|
|
142 |
|
|
void StepComputerJoystick::display(std::ostream &os) const { |
143 |
|
|
os << "StepComputer <" << getName() << ">:" << std::endl; |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
void StepComputerJoystick::commandLine(const std::string &cmdLine, |
147 |
|
|
std::istringstream &cmdArgs, |
148 |
|
|
std::ostream &os) { |
149 |
|
|
if (cmdLine == "help") { |
150 |
|
|
os << "NextStep: " << std::endl |
151 |
|
|
<< " - verbose [OFF]" << std::endl |
152 |
|
|
<< " - state [{start|stop}] \t get/set the stepper state. " << std::endl |
153 |
|
|
<< " - yZeroStep [<value>] \t get/set the Y default position." |
154 |
|
|
<< std::endl |
155 |
|
|
<< std::endl; |
156 |
|
|
} else if (cmdLine == "thisIsZero") { |
157 |
|
|
os << "Not supported" << std::endl; |
158 |
|
|
} else { |
159 |
|
|
} |
160 |
|
|
} |
161 |
|
|
|
162 |
|
|
Vector &StepComputerJoystick::getlaststep(Vector &res, int time) { |
163 |
|
|
if (res.size() != 4) res.resize(4); |
164 |
|
|
|
165 |
|
|
res(0) = m_laststep[0]; |
166 |
|
|
res(1) = m_laststep[1]; |
167 |
|
|
res(2) = m_laststep[2]; |
168 |
|
|
res(3) = (double)contactFootSIN(time); |
169 |
|
|
return res; |
170 |
|
|
} |
171 |
|
|
|
172 |
|
|
} // namespace sot |
173 |
|
|
} // namespace dynamicgraph |