1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#include <dynamic-graph/command-getter.h> |
11 |
|
|
#include <dynamic-graph/command-setter.h> |
12 |
|
|
#include <dynamic-graph/command.h> |
13 |
|
|
#include <dynamic-graph/factory.h> |
14 |
|
|
#include <sot/dynamic-pinocchio/waist-attitude-from-sensor.h> |
15 |
|
|
|
16 |
|
|
#include <sot/core/debug.hh> |
17 |
|
|
|
18 |
|
|
using namespace dynamicgraph::sot; |
19 |
|
|
using namespace dynamicgraph; |
20 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistAttitudeFromSensor, |
21 |
|
|
"WaistAttitudeFromSensor"); |
22 |
|
|
|
23 |
|
|
WaistAttitudeFromSensor::WaistAttitudeFromSensor(const std::string& name) |
24 |
|
|
: Entity(name), |
25 |
|
|
attitudeSensorSIN(NULL, "sotWaistAttitudeFromSensor(" + name + |
26 |
|
|
")::input(MatrixRotation)::attitudeIN"), |
27 |
|
|
positionSensorSIN(NULL, "sotWaistAttitudeFromSensor(" + name + |
28 |
|
|
")::input(matrixHomo)::position"), |
29 |
|
|
attitudeWaistSOUT( |
30 |
|
|
boost::bind(&WaistAttitudeFromSensor::computeAttitudeWaist, this, _1, |
31 |
|
|
_2), |
32 |
|
|
attitudeSensorSIN << positionSensorSIN, |
33 |
|
|
"sotWaistAttitudeFromSensor(" + name + ")::output(RPY)::attitude") { |
34 |
|
|
sotDEBUGIN(5); |
35 |
|
|
|
36 |
|
|
signalRegistration(attitudeSensorSIN); |
37 |
|
|
signalRegistration(positionSensorSIN); |
38 |
|
|
signalRegistration(attitudeWaistSOUT); |
39 |
|
|
|
40 |
|
|
sotDEBUGOUT(5); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
WaistAttitudeFromSensor::~WaistAttitudeFromSensor(void) { |
44 |
|
|
sotDEBUGIN(5); |
45 |
|
|
|
46 |
|
|
sotDEBUGOUT(5); |
47 |
|
|
return; |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
51 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
52 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
53 |
|
|
VectorRollPitchYaw& WaistAttitudeFromSensor::computeAttitudeWaist( |
54 |
|
|
VectorRollPitchYaw& res, const int& time) { |
55 |
|
|
sotDEBUGIN(15); |
56 |
|
|
|
57 |
|
|
const MatrixHomogeneous& waistMchest = positionSensorSIN(time); |
58 |
|
|
const MatrixRotation& worldRchest = attitudeSensorSIN(time); |
59 |
|
|
|
60 |
|
|
MatrixRotation waistRchest; |
61 |
|
|
waistRchest = waistMchest.linear(); |
62 |
|
|
MatrixRotation chestRwaist; |
63 |
|
|
chestRwaist = waistRchest.transpose(); |
64 |
|
|
|
65 |
|
|
MatrixRotation worldrchest; |
66 |
|
|
worldrchest = worldRchest * chestRwaist; |
67 |
|
|
res = (worldrchest.eulerAngles(2, 1, 0)).reverse(); |
68 |
|
|
sotDEBUGOUT(15); |
69 |
|
|
return res; |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
/* === WaistPoseFromSensorAndContact ===================================== */ |
73 |
|
|
/* === WaistPoseFromSensorAndContact ===================================== */ |
74 |
|
|
/* === WaistPoseFromSensorAndContact ===================================== */ |
75 |
|
|
|
76 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistPoseFromSensorAndContact, |
77 |
|
|
"WaistPoseFromSensorAndContact"); |
78 |
|
|
|
79 |
|
|
WaistPoseFromSensorAndContact::WaistPoseFromSensorAndContact( |
80 |
|
|
const std::string& name) |
81 |
|
|
: WaistAttitudeFromSensor(name), |
82 |
|
|
fromSensor_(false), |
83 |
|
|
positionContactSIN(NULL, "sotWaistPoseFromSensorAndContact(" + name + |
84 |
|
|
")::input(matrixHomo)::contact"), |
85 |
|
|
positionWaistSOUT( |
86 |
|
|
boost::bind(&WaistPoseFromSensorAndContact::computePositionWaist, |
87 |
|
|
this, _1, _2), |
88 |
|
|
attitudeWaistSOUT << positionContactSIN, |
89 |
|
|
"sotWaistPoseFromSensorAndContact(" + name + |
90 |
|
|
")::output(RPY+T)::positionWaist") { |
91 |
|
|
sotDEBUGIN(5); |
92 |
|
|
|
93 |
|
|
signalRegistration(positionContactSIN); |
94 |
|
|
signalRegistration(positionWaistSOUT); |
95 |
|
|
|
96 |
|
|
// Commands |
97 |
|
|
std::string docstring; |
98 |
|
|
docstring = |
99 |
|
|
" \n" |
100 |
|
|
" Set flag specifying whether angles are measured from sensors or " |
101 |
|
|
"simulated.\n" |
102 |
|
|
" \n" |
103 |
|
|
" Input:\n" |
104 |
|
|
" - a boolean value.\n" |
105 |
|
|
" \n"; |
106 |
|
|
addCommand( |
107 |
|
|
"setFromSensor", |
108 |
|
|
new ::dynamicgraph::command::Setter<WaistPoseFromSensorAndContact, bool>( |
109 |
|
|
*this, &WaistPoseFromSensorAndContact::fromSensor, docstring)); |
110 |
|
|
|
111 |
|
|
docstring = |
112 |
|
|
" \n" |
113 |
|
|
" Get flag specifying whether angles are measured from sensors or " |
114 |
|
|
"simulated.\n" |
115 |
|
|
" \n" |
116 |
|
|
" No input,\n" |
117 |
|
|
" return a boolean value.\n" |
118 |
|
|
" \n"; |
119 |
|
|
addCommand( |
120 |
|
|
"getFromSensor", |
121 |
|
|
new ::dynamicgraph::command::Getter<WaistPoseFromSensorAndContact, bool>( |
122 |
|
|
*this, &WaistPoseFromSensorAndContact::fromSensor, docstring)); |
123 |
|
|
|
124 |
|
|
sotDEBUGOUT(5); |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
WaistPoseFromSensorAndContact::~WaistPoseFromSensorAndContact(void) { |
128 |
|
|
sotDEBUGIN(5); |
129 |
|
|
|
130 |
|
|
sotDEBUGOUT(5); |
131 |
|
|
return; |
132 |
|
|
} |
133 |
|
|
|
134 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
135 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
136 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
137 |
|
|
dynamicgraph::Vector& WaistPoseFromSensorAndContact::computePositionWaist( |
138 |
|
|
dynamicgraph::Vector& res, const int& time) { |
139 |
|
|
sotDEBUGIN(15); |
140 |
|
|
|
141 |
|
|
const MatrixHomogeneous& waistMcontact = positionContactSIN(time); |
142 |
|
|
MatrixHomogeneous contactMwaist; |
143 |
|
|
contactMwaist = waistMcontact.inverse(); |
144 |
|
|
|
145 |
|
|
res.resize(6); |
146 |
|
|
for (unsigned int i = 0; i < 3; ++i) { |
147 |
|
|
res(i) = contactMwaist(i, 3); |
148 |
|
|
} |
149 |
|
|
|
150 |
|
|
if (fromSensor_) { |
151 |
|
|
const VectorRollPitchYaw& worldrwaist = attitudeWaistSOUT(time); |
152 |
|
|
for (unsigned int i = 0; i < 3; ++i) { |
153 |
|
|
res(i + 3) = worldrwaist(i); |
154 |
|
|
} |
155 |
|
|
} else { |
156 |
|
|
VectorRollPitchYaw contactrwaist; |
157 |
|
|
contactrwaist = contactMwaist.linear().eulerAngles(2, 1, 0).reverse(); |
158 |
|
|
|
159 |
|
|
for (unsigned int i = 0; i < 3; ++i) { |
160 |
|
|
res(i + 3) = contactrwaist(i); |
161 |
|
|
} |
162 |
|
|
} |
163 |
|
|
|
164 |
|
|
sotDEBUGOUT(15); |
165 |
|
|
return res; |
166 |
|
|
} |