1 |
|
|
/* |
2 |
|
|
* Copyright 2013, |
3 |
|
|
* Olivier Stasse, |
4 |
|
|
* |
5 |
|
|
* CNRS, LAAS |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
//#define VP_DEBUG |
9 |
|
|
//#define VP_DEBUG_MODE 10 |
10 |
|
|
#include <sot/core/debug.hh> |
11 |
|
|
#include <sot/core/matrix-geometry.hh> |
12 |
|
|
#ifdef VP_DEBUG |
13 |
|
|
class sotJTE__INIT { |
14 |
|
|
public: |
15 |
|
|
sotJTE__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); } |
16 |
|
|
}; |
17 |
|
|
sotJTE__INIT sotJTE_initiator; |
18 |
|
|
#endif //#ifdef VP_DEBUG |
19 |
|
|
|
20 |
|
|
#include <dynamic-graph/all-commands.h> |
21 |
|
|
#include <dynamic-graph/command-bind.h> |
22 |
|
|
#include <dynamic-graph/factory.h> |
23 |
|
|
|
24 |
|
|
#include <sot/core/joint-trajectory-entity.hh> |
25 |
|
|
|
26 |
|
|
#include "joint-trajectory-command.hh" |
27 |
|
|
|
28 |
|
|
using namespace std; |
29 |
|
|
using namespace dynamicgraph; |
30 |
|
|
using namespace dynamicgraph::sot; |
31 |
|
|
using namespace dynamicgraph::command; |
32 |
|
|
|
33 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotJointTrajectoryEntity, |
34 |
|
|
"SotJointTrajectoryEntity"); |
35 |
|
|
|
36 |
|
|
SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n) |
37 |
|
|
: Entity(n), |
38 |
|
|
refresherSINTERN("SotJointTrajectoryEntity(" + n + |
39 |
|
|
")::intern(dummy)::refresher"), |
40 |
|
|
OneStepOfUpdateS( |
41 |
|
|
boost::bind(&SotJointTrajectoryEntity::OneStepOfUpdate, this, _1, _2), |
42 |
|
|
refresherSINTERN << trajectorySIN, |
43 |
|
|
"SotJointTrajectory(" + n + ")::onestepofupdate"), |
44 |
|
|
positionSOUT( |
45 |
|
|
boost::bind(&SotJointTrajectoryEntity::getNextPosition, this, _1, _2), |
46 |
|
|
OneStepOfUpdateS, |
47 |
|
|
"SotJointTrajectory(" + n + ")::output(vector)::position"), |
48 |
|
|
comSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoM, this, _1, _2), |
49 |
|
|
OneStepOfUpdateS, |
50 |
|
|
"SotJointTrajectory(" + n + ")::output(vector)::com"), |
51 |
|
|
zmpSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoP, this, _1, _2), |
52 |
|
|
OneStepOfUpdateS, |
53 |
|
|
"SotJointTrajectory(" + n + ")::output(vector)::zmp"), |
54 |
|
|
waistSOUT( |
55 |
|
|
boost::bind(&SotJointTrajectoryEntity::getNextWaist, this, _1, _2), |
56 |
|
|
OneStepOfUpdateS, |
57 |
|
|
"SotJointTrajectory(" + n + ")::output(MatrixHomogeneous)::waist"), |
58 |
|
|
seqIdSOUT(boost::bind(&SotJointTrajectoryEntity::getSeqId, this, _1, _2), |
59 |
|
|
OneStepOfUpdateS, |
60 |
|
|
"SotJointTrajectory(" + n + ")::output(uint)::seqid"), |
61 |
|
|
trajectorySIN(NULL, "SotJointTrajectory(" + n + |
62 |
|
|
")::input(trajectory)::trajectoryIN"), |
63 |
|
|
index_(0), |
64 |
|
|
traj_timestamp_(0, 0), |
65 |
|
|
seqid_(0), |
66 |
|
|
deque_traj_(0) { |
67 |
|
|
using namespace command; |
68 |
|
|
sotDEBUGIN(5); |
69 |
|
|
|
70 |
|
|
signalRegistration(positionSOUT << comSOUT << zmpSOUT << waistSOUT |
71 |
|
|
<< seqIdSOUT << trajectorySIN); |
72 |
|
|
refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY); |
73 |
|
|
refresherSINTERN.setReady(true); |
74 |
|
|
|
75 |
|
|
std::string docstring; |
76 |
|
|
docstring = |
77 |
|
|
" \n" |
78 |
|
|
" initialize the first trajectory.\n" |
79 |
|
|
" \n" |
80 |
|
|
" Input:\n" |
81 |
|
|
" = a string : .\n" |
82 |
|
|
" \n"; |
83 |
|
|
addCommand("initTraj", |
84 |
|
|
makeCommandVoid1(*this, &SotJointTrajectoryEntity::setInitTraj, |
85 |
|
|
docCommandVoid1("Set initial trajectory", |
86 |
|
|
"string (trajectory)"))); |
87 |
|
|
sotDEBUGOUT(5); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
void SotJointTrajectoryEntity::UpdatePoint(const JointTrajectoryPoint &aJTP) { |
91 |
|
|
sotDEBUGIN(5); |
92 |
|
|
// Posture |
93 |
|
|
std::vector<JointTrajectoryPoint>::size_type possize = aJTP.positions_.size(); |
94 |
|
|
if (possize == 0) return; |
95 |
|
|
|
96 |
|
|
pose_.resize(aJTP.positions_.size()); |
97 |
|
|
for (std::vector<JointTrajectoryPoint>::size_type i = 0; i < possize - 5; |
98 |
|
|
i++) { |
99 |
|
|
pose_(i) = aJTP.positions_[i]; |
100 |
|
|
sotDEBUG(5) << pose_(i) << " " << std::endl; |
101 |
|
|
} |
102 |
|
|
|
103 |
|
|
// Center of Mass |
104 |
|
|
com_.resize(3); |
105 |
|
|
for (std::vector<JointTrajectoryPoint>::size_type i = possize - 5, j = 0; |
106 |
|
|
i < possize - 2; i++, j++) |
107 |
|
|
com_(j) = aJTP.positions_[i]; |
108 |
|
|
|
109 |
|
|
sotDEBUG(5) << "com: " << com_ << std::endl; |
110 |
|
|
|
111 |
|
|
// Add a constant height TODO: make it variable |
112 |
|
|
dynamicgraph::Vector waistXYZTheta; |
113 |
|
|
waistXYZTheta.resize(4); |
114 |
|
|
|
115 |
|
|
waistXYZTheta(0) = com_(0); |
116 |
|
|
waistXYZTheta(1) = com_(1); |
117 |
|
|
waistXYZTheta(2) = 0.65; |
118 |
|
|
waistXYZTheta(3) = com_(2); |
119 |
|
|
waist_ = XYZThetaToMatrixHomogeneous(waistXYZTheta); |
120 |
|
|
|
121 |
|
|
sotDEBUG(5) << "waist: " << waist_ << std::endl; |
122 |
|
|
// Center of Pressure |
123 |
|
|
cop_.resize(3); |
124 |
|
|
for (std::vector<JointTrajectoryPoint>::size_type i = possize - 2, j = 0; |
125 |
|
|
i < possize; i++, j++) |
126 |
|
|
cop_(j) = aJTP.positions_[i]; |
127 |
|
|
cop_(2) = -0.055; |
128 |
|
|
sotDEBUG(5) << "cop_: " << cop_ << std::endl; |
129 |
|
|
sotDEBUGOUT(5); |
130 |
|
|
} |
131 |
|
|
|
132 |
|
|
void SotJointTrajectoryEntity::UpdateTrajectory(const Trajectory &aTrajectory) { |
133 |
|
|
sotDEBUGIN(3); |
134 |
|
|
sotDEBUG(3) << "traj_timestamp: " << traj_timestamp_ |
135 |
|
|
<< " aTrajectory.header_.stamp_" << aTrajectory.header_.stamp_; |
136 |
|
|
|
137 |
|
|
// Do we have the same trajectory, or are we at the initialization phase ? |
138 |
|
|
if ((traj_timestamp_ == aTrajectory.header_.stamp_) && |
139 |
|
|
(deque_traj_.size() != 0)) |
140 |
|
|
index_++; |
141 |
|
|
else { |
142 |
|
|
// No we have a new trajectory. |
143 |
|
|
sotDEBUG(3) << "index: " << index_ << " aTrajectory.points_.size(): " |
144 |
|
|
<< aTrajectory.points_.size(); |
145 |
|
|
|
146 |
|
|
// Put the new trajectory in the queue |
147 |
|
|
|
148 |
|
|
// if there is nothing |
149 |
|
|
if (deque_traj_.size() == 0) { |
150 |
|
|
deque_traj_.push_back(aTrajectory); |
151 |
|
|
index_ = 0; |
152 |
|
|
} else { |
153 |
|
|
index_++; |
154 |
|
|
// if it is not already inside the queue. |
155 |
|
|
if (deque_traj_.back().header_.stamp_ == aTrajectory.header_.stamp_) { |
156 |
|
|
} else |
157 |
|
|
deque_traj_.push_back(aTrajectory); |
158 |
|
|
} |
159 |
|
|
} |
160 |
|
|
|
161 |
|
|
sotDEBUG(3) << "step 1 " << std::endl |
162 |
|
|
<< "header: " << std::endl |
163 |
|
|
<< " timestamp:" |
164 |
|
|
<< static_cast<double>(aTrajectory.header_.stamp_.secs_) + |
165 |
|
|
0.000000001 * |
166 |
|
|
static_cast<double>(aTrajectory.header_.stamp_.nsecs_) |
167 |
|
|
<< " seq:" << aTrajectory.header_.seq_ << " " |
168 |
|
|
<< " frame_id:" << aTrajectory.header_.frame_id_ |
169 |
|
|
<< " index_: " << index_ |
170 |
|
|
<< " aTrajectory.points_.size():" << aTrajectory.points_.size() |
171 |
|
|
<< std::endl; |
172 |
|
|
|
173 |
|
|
// Strategy at the end of the trajectory. |
174 |
|
|
if (index_ >= deque_traj_.front().points_.size()) { |
175 |
|
|
if (deque_traj_.size() > 1) { |
176 |
|
|
deque_traj_.pop_front(); |
177 |
|
|
index_ = 0; |
178 |
|
|
} |
179 |
|
|
|
180 |
|
|
// If the new trajectory has a problem |
181 |
|
|
if (deque_traj_.front().points_.size() == 0) { |
182 |
|
|
// then neutralize the entity |
183 |
|
|
index_ = 0; |
184 |
|
|
sotDEBUG(3) << "current_traj_.points_.size()=" |
185 |
|
|
<< deque_traj_.front().points_.size() << std::endl; |
186 |
|
|
return; |
187 |
|
|
} |
188 |
|
|
|
189 |
|
|
// Strategy at the end of the trajectory when no new information is |
190 |
|
|
// available: It is assumed that the last pose is balanced, and we keep |
191 |
|
|
// providing this pose to the robot. |
192 |
|
|
if ((index_ != 0) && (deque_traj_.size() == 1)) { |
193 |
|
|
index_ = deque_traj_.front().points_.size() - 1; |
194 |
|
|
} |
195 |
|
|
sotDEBUG(3) << "index_=current_traj_.points_.size()-1;" << std::endl; |
196 |
|
|
} |
197 |
|
|
|
198 |
|
|
sotDEBUG(3) << "index_:" << index_ << " current_traj_.points_.size():" |
199 |
|
|
<< deque_traj_.front().points_.size() << std::endl; |
200 |
|
|
|
201 |
|
|
seqid_ = deque_traj_.front().header_.seq_; |
202 |
|
|
UpdatePoint(deque_traj_.front().points_[index_]); |
203 |
|
|
sotDEBUGOUT(3); |
204 |
|
|
} |
205 |
|
|
|
206 |
|
|
int &SotJointTrajectoryEntity::OneStepOfUpdate(int &dummy, const int &time) { |
207 |
|
|
sotDEBUGIN(4); |
208 |
|
|
const Trajectory &atraj = trajectorySIN(time); |
209 |
|
|
if ((atraj.header_.stamp_.secs_ != |
210 |
|
|
deque_traj_.front().header_.stamp_.secs_) || |
211 |
|
|
(atraj.header_.stamp_.nsecs_ != |
212 |
|
|
deque_traj_.front().header_.stamp_.nsecs_)) { |
213 |
|
|
if (index_ < deque_traj_.front().points_.size() - 1) { |
214 |
|
|
sotDEBUG(4) << "Overwrite trajectory without completion." << index_ << " " |
215 |
|
|
<< deque_traj_.front().points_.size() << std::endl; |
216 |
|
|
} |
217 |
|
|
} |
218 |
|
|
sotDEBUG(4) << "Finished to read trajectorySIN" << std::endl; |
219 |
|
|
UpdateTrajectory(atraj); |
220 |
|
|
|
221 |
|
|
sotDEBUG(4) << "Finished to update trajectory" << std::endl; |
222 |
|
|
|
223 |
|
|
sotDEBUGOUT(4); |
224 |
|
|
return dummy; |
225 |
|
|
} |
226 |
|
|
|
227 |
|
|
sot::MatrixHomogeneous SotJointTrajectoryEntity::XYZThetaToMatrixHomogeneous( |
228 |
|
|
const dynamicgraph::Vector &xyztheta) { |
229 |
|
|
assert(xyztheta.size() == 4); |
230 |
|
|
dynamicgraph::Vector t(3); |
231 |
|
|
t(0) = xyztheta(0); |
232 |
|
|
t(1) = xyztheta(1); |
233 |
|
|
t(2) = xyztheta(2); |
234 |
|
|
Eigen::Affine3d trans; |
235 |
|
|
trans = Eigen::Translation3d(t); |
236 |
|
|
Eigen::Affine3d _Rd(Eigen::AngleAxisd(xyztheta(3), Eigen::Vector3d::UnitZ())); |
237 |
|
|
sot::MatrixHomogeneous res; |
238 |
|
|
res = _Rd * trans; |
239 |
|
|
return res; |
240 |
|
|
} |
241 |
|
|
|
242 |
|
|
dynamicgraph::Vector &SotJointTrajectoryEntity::getNextPosition( |
243 |
|
|
dynamicgraph::Vector &pos, const int &time) { |
244 |
|
|
sotDEBUGIN(5); |
245 |
|
|
OneStepOfUpdateS(time); |
246 |
|
|
pos = pose_; |
247 |
|
|
sotDEBUG(5) << pos; |
248 |
|
|
sotDEBUGOUT(5); |
249 |
|
|
return pos; |
250 |
|
|
} |
251 |
|
|
|
252 |
|
|
dynamicgraph::Vector &SotJointTrajectoryEntity::getNextCoM( |
253 |
|
|
dynamicgraph::Vector &com, const int &time) { |
254 |
|
|
sotDEBUGIN(5); |
255 |
|
|
OneStepOfUpdateS(time); |
256 |
|
|
com = com_; |
257 |
|
|
sotDEBUGOUT(5); |
258 |
|
|
return com; |
259 |
|
|
} |
260 |
|
|
|
261 |
|
|
dynamicgraph::Vector &SotJointTrajectoryEntity::getNextCoP( |
262 |
|
|
dynamicgraph::Vector &cop, const int &time) { |
263 |
|
|
sotDEBUGIN(5); |
264 |
|
|
OneStepOfUpdateS(time); |
265 |
|
|
cop = cop_; |
266 |
|
|
sotDEBUGOUT(5); |
267 |
|
|
return cop; |
268 |
|
|
} |
269 |
|
|
|
270 |
|
|
sot::MatrixHomogeneous &SotJointTrajectoryEntity::getNextWaist( |
271 |
|
|
sot::MatrixHomogeneous &waist, const int &time) { |
272 |
|
|
sotDEBUGIN(5); |
273 |
|
|
OneStepOfUpdateS(time); |
274 |
|
|
waist = waist_; |
275 |
|
|
sotDEBUGOUT(5); |
276 |
|
|
return waist_; |
277 |
|
|
} |
278 |
|
|
|
279 |
|
|
unsigned int &SotJointTrajectoryEntity::getSeqId(unsigned int &seqid, |
280 |
|
|
const int &time) { |
281 |
|
|
sotDEBUGIN(5); |
282 |
|
|
OneStepOfUpdateS(time); |
283 |
|
|
seqid = seqid_; |
284 |
|
|
sotDEBUGOUT(5); |
285 |
|
|
return seqid; |
286 |
|
|
} |
287 |
|
|
|
288 |
|
|
void SotJointTrajectoryEntity::loadFile(const std::string &) { |
289 |
|
|
sotDEBUGIN(5); |
290 |
|
|
// TODO |
291 |
|
|
sotDEBUGOUT(5); |
292 |
|
|
} |
293 |
|
|
|
294 |
|
|
void SotJointTrajectoryEntity::display(std::ostream &os) const { |
295 |
|
|
sotDEBUGIN(5); |
296 |
|
|
os << this; |
297 |
|
|
sotDEBUGOUT(5); |
298 |
|
|
} |
299 |
|
|
|
300 |
|
|
void SotJointTrajectoryEntity::setInitTraj(const std::string &as) { |
301 |
|
|
sotDEBUGIN(5); |
302 |
|
|
std::istringstream is(as); |
303 |
|
|
init_traj_.deserialize(is); |
304 |
|
|
UpdateTrajectory(init_traj_); |
305 |
|
|
|
306 |
|
|
sotDEBUGOUT(5); |
307 |
|
|
} |