1 |
|
|
#include <dynamic-graph/factory.h> |
2 |
|
|
#include <sot/pattern-generator/step-observer.h> |
3 |
|
|
|
4 |
|
|
#include <cmath> |
5 |
|
|
#include <sot/core/debug.hh> |
6 |
|
|
#include <sot/core/matrix-geometry.hh> |
7 |
|
|
|
8 |
|
|
namespace dynamicgraph { |
9 |
|
|
namespace sot { |
10 |
|
|
|
11 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepObserver, "StepObserver"); |
12 |
|
|
|
13 |
|
|
StepObserver::StepObserver(const std::string &name) |
14 |
|
|
: Entity(name) |
15 |
|
|
|
16 |
|
|
, |
17 |
|
|
leftHandPositionSIN(NULL, |
18 |
|
|
"StepObserver(" + name + ")::input(vector)::lefthand") |
19 |
|
|
|
20 |
|
|
, |
21 |
|
|
rightHandPositionSIN( |
22 |
|
|
NULL, "StepObserver(" + name + ")::input(vector)::righthand") |
23 |
|
|
|
24 |
|
|
, |
25 |
|
|
leftFootPositionSIN( |
26 |
|
|
NULL, "StepObserver(" + name + ")::input(matrixhomo)::leftfoot") |
27 |
|
|
|
28 |
|
|
, |
29 |
|
|
rightFootPositionSIN( |
30 |
|
|
NULL, "StepObserver(" + name + ")::input(matrixhomo)::rightfoot") |
31 |
|
|
|
32 |
|
|
, |
33 |
|
|
waistPositionSIN(NULL, |
34 |
|
|
"StepObserver(" + name + ")::input(matrixhomo)::waist") |
35 |
|
|
|
36 |
|
|
, |
37 |
|
|
referencePositionLeftSOUT( |
38 |
|
|
boost::bind(&StepObserver::computeReferencePositionLeft, this, _1, |
39 |
|
|
_2), |
40 |
|
|
leftFootPositionSIN << leftHandPositionSIN << rightHandPositionSIN, |
41 |
|
|
"StepObserver(" + name + ")::output(vector)::position2handLeft") |
42 |
|
|
|
43 |
|
|
, |
44 |
|
|
referencePositionRightSOUT( |
45 |
|
|
boost::bind(&StepObserver::computeReferencePositionRight, this, _1, |
46 |
|
|
_2), |
47 |
|
|
rightFootPositionSIN << rightHandPositionSIN << leftHandPositionSIN, |
48 |
|
|
"StepObserver(" + name + ")::output(vector)::position2handRight") |
49 |
|
|
|
50 |
|
|
, |
51 |
|
|
referencePositionWaistSOUT( |
52 |
|
|
boost::bind(&StepObserver::computeReferencePositionWaist, this, _1, |
53 |
|
|
_2), |
54 |
|
|
waistPositionSIN << rightHandPositionSIN << leftHandPositionSIN, |
55 |
|
|
"StepObserver(" + name + ")::output(vector)::position2handWaist") { |
56 |
|
|
sotDEBUGIN(25); |
57 |
|
|
signalRegistration(getSignals()); |
58 |
|
|
sotDEBUGOUT(25); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
SignalArray<int> StepObserver::getSignals(void) { |
62 |
|
|
return (leftHandPositionSIN |
63 |
|
|
<< leftFootPositionSIN << waistPositionSIN << rightHandPositionSIN |
64 |
|
|
<< rightFootPositionSIN << referencePositionLeftSOUT |
65 |
|
|
<< referencePositionRightSOUT << referencePositionWaistSOUT); |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
StepObserver::operator SignalArray<int>() { return getSignals(); } |
69 |
|
|
|
70 |
|
|
MatrixHomogeneous &StepObserver::computeRefPos(MatrixHomogeneous &res, |
71 |
|
|
int timeCurr, |
72 |
|
|
const MatrixHomogeneous &wMref) { |
73 |
|
|
sotDEBUGIN(15); |
74 |
|
|
|
75 |
|
|
// Set to 0 to compute a reference frame using both hands. Set to non zero |
76 |
|
|
// value to use the right hand frame as a reference frame (for debug). |
77 |
|
|
#define RIGHT_HAND_REFERENCE 0 |
78 |
|
|
|
79 |
|
|
#if RIGHT_HAND_REFERENCE |
80 |
|
|
|
81 |
|
|
const MatrixHomogeneous &wMrh = rightHandPositionSIN(timeCurr); |
82 |
|
|
MatrixHomogeneous refMw; |
83 |
|
|
refMw = wMref.inverse(); |
84 |
|
|
res = refMw * wMrh; |
85 |
|
|
|
86 |
|
|
#else |
87 |
|
|
|
88 |
|
|
const MatrixHomogeneous &wMlh = leftHandPositionSIN(timeCurr); |
89 |
|
|
const MatrixHomogeneous &wMrh = rightHandPositionSIN(timeCurr); |
90 |
|
|
|
91 |
|
|
MatrixHomogeneous refMw; |
92 |
|
|
refMw = wMref.inverse(); |
93 |
|
|
MatrixHomogeneous sfMlh; |
94 |
|
|
sfMlh = refMw * wMlh; |
95 |
|
|
MatrixHomogeneous sfMrh; |
96 |
|
|
sfMrh = refMw * wMrh; |
97 |
|
|
|
98 |
|
|
VectorRollPitchYaw rpy; |
99 |
|
|
|
100 |
|
|
Vector prh(3); |
101 |
|
|
prh = sfMrh.translation(); |
102 |
|
|
|
103 |
|
|
Vector plh(3); |
104 |
|
|
plh = sfMlh.translation(); |
105 |
|
|
|
106 |
|
|
rpy.setZero(); |
107 |
|
|
rpy(2) = std::atan2(prh(0) - plh(0), plh(1) - prh(1)); |
108 |
|
|
Vector p(3); |
109 |
|
|
p = .5 * (plh + prh); |
110 |
|
|
|
111 |
|
|
MatrixRotation R; |
112 |
|
|
|
113 |
|
|
R = (Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * |
114 |
|
|
Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * |
115 |
|
|
Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX())) |
116 |
|
|
.toRotationMatrix(); |
117 |
|
|
|
118 |
|
|
res.translation() = p; |
119 |
|
|
res.linear() = R; |
120 |
|
|
|
121 |
|
|
#endif |
122 |
|
|
|
123 |
|
|
sotDEBUGOUT(15); |
124 |
|
|
return res; |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
MatrixHomogeneous &StepObserver::computeReferencePositionLeft( |
128 |
|
|
MatrixHomogeneous &res, int timeCurr) { |
129 |
|
|
sotDEBUGIN(15); |
130 |
|
|
|
131 |
|
|
const MatrixHomogeneous &wMref = leftFootPositionSIN(timeCurr); |
132 |
|
|
|
133 |
|
|
sotDEBUGOUT(15); |
134 |
|
|
return computeRefPos(res, timeCurr, wMref); |
135 |
|
|
} |
136 |
|
|
|
137 |
|
|
MatrixHomogeneous &StepObserver::computeReferencePositionRight( |
138 |
|
|
MatrixHomogeneous &res, int timeCurr) { |
139 |
|
|
sotDEBUGIN(15); |
140 |
|
|
|
141 |
|
|
const MatrixHomogeneous &wMref = rightFootPositionSIN(timeCurr); |
142 |
|
|
|
143 |
|
|
sotDEBUGOUT(15); |
144 |
|
|
return computeRefPos(res, timeCurr, wMref); |
145 |
|
|
} |
146 |
|
|
|
147 |
|
|
MatrixHomogeneous &StepObserver::computeReferencePositionWaist( |
148 |
|
|
MatrixHomogeneous &res, int timeCurr) { |
149 |
|
|
sotDEBUGIN(15); |
150 |
|
|
|
151 |
|
|
const MatrixHomogeneous &wMref = waistPositionSIN(timeCurr); |
152 |
|
|
|
153 |
|
|
sotDEBUGOUT(15); |
154 |
|
|
return computeRefPos(res, timeCurr, wMref); |
155 |
|
|
} |
156 |
|
|
|
157 |
|
|
void StepObserver::display(std::ostream &os) const { |
158 |
|
|
os << "StepObserver <" << getName() << ">:" << std::endl; |
159 |
|
|
} |
160 |
|
|
|
161 |
|
|
void StepObserver::commandLine(const std::string &cmdLine, |
162 |
|
|
std::istringstream &cmdArgs, std::ostream &os) { |
163 |
|
|
if (cmdLine == "help") { |
164 |
|
|
os << "StepObserver: " << std::endl << std::endl; |
165 |
|
|
} else { |
166 |
|
|
} |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
} // namespace sot |
170 |
|
|
} // namespace dynamicgraph |