GCC Code Coverage Report

Directory: ./
File: src/tools/joint-trajectory-entity.cpp
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 0 121 0.0%
Branches: 0 238 0.0%
Line Branch Exec Source
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 }
308