GCC Code Coverage Report


Directory: plugins/
File: plugins/hppwidgetsplugin/joint-tree-item.cc
Date: 2024-12-13 15:51:58
Exec Total Coverage
Lines: 0 248 0.0%
Branches: 0 403 0.0%

Line Branch Exec Source
1 //
2 // Copyright (c) CNRS
3 // Authors: Joseph Mirabel
4 //
5
6 #include "hppwidgetsplugin/joint-tree-item.hh"
7
8 #include <gepetto/viewer/group-node.h>
9 #include <omniORB4/CORBA.h>
10
11 #include <QDoubleSpinBox>
12 #include <QMenu>
13 #include <QPushButton>
14 #include <hpp/corbaserver/client.hh>
15 #include <iostream>
16
17 #include "gepetto/gui/mainwindow.hh"
18 #include "hppwidgetsplugin/joint-action.hh"
19 #include "hppwidgetsplugin/jointtreewidget.hh"
20 #include "hppwidgetsplugin/pathplayer.hh"
21
22 using CORBA::ULong;
23
24 namespace hpp {
25 namespace gui {
26 using CORBA::ULong;
27
28 const int JointTreeItem::IndexRole = Qt::UserRole + 1;
29 const int JointTreeItem::LowerBoundRole = Qt::UserRole + 2;
30 const int JointTreeItem::UpperBoundRole = Qt::UserRole + 3;
31 const int JointTreeItem::TypeRole = Qt::UserRole + 10;
32
33 JointTreeItem::JointTreeItem(const char *name, const ULong &idxQ,
34 const ULong &idxV, const hpp::floatSeq &q,
35 const hpp::floatSeq &b, const ULong &nbDof,
36 const NodesPtr_t &nodes)
37 : QStandardItem(QString(name)),
38 name_(name),
39 idxQ_(idxQ),
40 idxV_(idxV),
41 nq_(q.length()),
42 nv_(nbDof),
43 nodes_(nodes),
44 value_() {
45 setData((int)-1, IndexRole);
46 setData(SkipType, TypeRole);
47 for (size_t i = 0; i < nq_; ++i) {
48 QStandardItem *joint = new QStandardItem;
49 QStandardItem *upper = new QStandardItem;
50 QStandardItem *lower = new QStandardItem;
51 joint->setData(static_cast<int>(i), IndexRole);
52 QList<QStandardItem *> row;
53 row << joint << lower << upper;
54 value_.append(row);
55 appendRow(row);
56 }
57 updateConfig(q);
58 updateBounds(b);
59 }
60
61 JointTreeItem::~JointTreeItem() { qDeleteAll(actions_); }
62
63 QStandardItem *JointTreeItem::clone() const {
64 hpp::floatSeq q = hpp::floatSeq();
65 q.length(value_.size());
66 hpp::floatSeq b = hpp::floatSeq();
67 b.length(2 * value_.size());
68 for (size_t i = 0; i < q.length(); ++i) {
69 q[(ULong)i] = value_[(ULong)i][0]->data(Qt::EditRole).toFloat();
70 b[2 * (ULong)i] = value_[(ULong)i][0]->data(LowerBoundRole).toFloat();
71 b[2 * (ULong)i + 1] = value_[(ULong)i][0]->data(UpperBoundRole).toFloat();
72 }
73 return new JointTreeItem(name_.c_str(), idxQ_, idxV_, q, b, nv_, nodes_);
74 }
75
76 hpp::floatSeq JointTreeItem::config() const {
77 hpp::floatSeq q = hpp::floatSeq();
78 q.length(value_.size());
79 for (size_t i = 0; i < q.length(); ++i)
80 q[(ULong)i] = value_[(ULong)i][0]->data(Qt::EditRole).toFloat();
81 return q;
82 }
83
84 hpp::floatSeq JointTreeItem::bounds() const {
85 hpp::floatSeq b = hpp::floatSeq();
86 b.length(2 * value_.size());
87 for (int i = 0; i < value_.size(); ++i) {
88 b[2 * i] = value_[i][1]->data(Qt::EditRole).toFloat();
89 b[2 * i + 1] = value_[i][2]->data(Qt::EditRole).toFloat();
90 }
91 return b;
92 }
93
94 void JointTreeItem::updateConfig(const hpp::floatSeq &c) {
95 assert((int)c.length() == value_.size());
96 for (int i = 0; i < value_.size(); ++i)
97 value_[i][0]->setData(c[i], Qt::EditRole);
98 }
99
100 void JointTreeItem::updateFromRobotConfig(const hpp::floatSeq &rc) {
101 assert(idxQ_ + value_.size() <= rc.length());
102 for (int i = 0; i < value_.size(); ++i)
103 value_[i][0]->setData(rc[(int)idxQ_ + i], Qt::EditRole);
104 }
105
106 void JointTreeItem::updateBounds(const hpp::floatSeq &b) {
107 assert((int)b.length() == 2 * value_.size());
108 for (int i = 0; i < value_.size(); ++i) {
109 QStandardItem *lower = value_[i][1];
110 QStandardItem *upper = value_[i][2];
111 lower->setData(BoundType, TypeRole);
112 upper->setData(BoundType, TypeRole);
113 lower->setData(b[2 * i], Qt::EditRole);
114 upper->setData(b[2 * i + 1], Qt::EditRole);
115 }
116 updateTypeRole();
117 }
118
119 void JointTreeItem::updateTypeRole() {
120 // planar and freeflyer joints
121 int threshold = value_.size();
122 if (nv_ == 1 && nq_ == 2) // SO2
123 threshold = 0;
124 // TODO SO3 joint fall in that case too whereas threshold should be 0 for
125 // them.
126 else if (nv_ == 3 && nq_ == 4) // SO3 and SE2
127 threshold = 2;
128 else if (nv_ == 6 && nq_ == 7) // SE3
129 threshold = 3;
130 for (int i = 0; i < value_.size(); ++i) {
131 float lo = value_[i][1]->data(Qt::EditRole).toFloat();
132 float up = value_[i][2]->data(Qt::EditRole).toFloat();
133 if (i >= threshold)
134 value_[i][0]->setData(IntegratorType, TypeRole);
135 else if (lo < up)
136 value_[i][0]->setData(BoundedValueType, TypeRole);
137 else
138 value_[i][0]->setData(UnboundedValueType, TypeRole);
139 }
140 }
141
142 void JointTreeItem::setupActions(HppWidgetsPlugin *plugin) {
143 JointAction *a;
144
145 a = new JointAction(QObject::tr("Move &joint..."), name_, 0);
146 plugin->jointTreeWidget()->connect(a, SIGNAL(triggered(std::string)),
147 SLOT(openJointMoveDialog(std::string)));
148 actions_.append(a);
149
150 a = new JointAction(QObject::tr("Set &bounds..."), name_, 0);
151 plugin->jointTreeWidget()->connect(a, SIGNAL(triggered(std::string)),
152 SLOT(openJointBoundDialog(std::string)));
153 actions_.append(a);
154
155 a = new JointAction(QObject::tr("Add joint &frame"), name_, 0);
156 plugin->connect(a, SIGNAL(triggered(std::string)),
157 SLOT(addJointFrame(std::string)));
158 actions_.append(a);
159
160 a = new JointAction(QObject::tr("Display &roadmap"), name_, 0);
161 plugin->connect(a, SIGNAL(triggered(std::string)),
162 SLOT(displayRoadmap(std::string)));
163 actions_.append(a);
164
165 a = new JointAction(QObject::tr("Display &waypoints of selected path"), name_,
166 0);
167 plugin->pathPlayer()->connect(a, SIGNAL(triggered(std::string)),
168 SLOT(displayWaypointsOfPath(std::string)));
169 actions_.append(a);
170
171 a = new JointAction(QObject::tr("Display selected &path"), name_, 0);
172 plugin->pathPlayer()->connect(a, SIGNAL(triggered(std::string)),
173 SLOT(displayPath(std::string)));
174 actions_.append(a);
175 }
176
177 const QList<QAction *> &JointTreeItem::actions() const { return actions_; }
178
179 JointItemDelegate::JointItemDelegate(QPushButton *forceVelocity,
180 HppWidgetsPlugin *plugin,
181 gepetto::gui::MainWindow *parent)
182 : QItemDelegate(parent),
183 main_(parent),
184 plugin_(plugin),
185 forceIntegrator_(forceVelocity) {}
186
187 void JointItemDelegate::updateTypeRole(JointTreeItem::ItemType &type) const {
188 if (forceIntegrator_ && forceIntegrator_->isChecked() &&
189 (type == JointTreeItem::UnboundedValueType ||
190 type == JointTreeItem::BoundedValueType))
191 type = JointTreeItem::IntegratorType;
192 }
193
194 QWidget *JointItemDelegate::createEditor(
195 QWidget *parent, const QStyleOptionViewItem & /*option*/,
196 const QModelIndex &index) const {
197 JointTreeItem::ItemType type =
198 (JointTreeItem::ItemType)index.data(JointTreeItem::TypeRole).toInt();
199 updateTypeRole(type);
200 const QStandardItemModel *m =
201 static_cast<const QStandardItemModel *>(index.model());
202 const JointTreeItem *ji =
203 dynamic_cast<const JointTreeItem *>(m->itemFromIndex(index)->parent());
204 switch (type) {
205 case JointTreeItem::SkipType:
206 return 0;
207 case JointTreeItem::IntegratorType:
208 assert(ji);
209 return new IntegratorWheel(
210 Qt::Horizontal, plugin_, parent, main_, ji,
211 std::min((ULong)index.data(JointTreeItem::IndexRole).toInt(),
212 ji->numberDof() - 1));
213 case JointTreeItem::BoundedValueType:
214 assert(ji);
215 return new SliderBoundedJoint(
216 Qt::Horizontal, plugin_, parent, main_, ji,
217 index.data(JointTreeItem::IndexRole).toInt());
218 case JointTreeItem::UnboundedValueType:
219 case JointTreeItem::BoundType: {
220 QDoubleSpinBox *spinbox = new QDoubleSpinBox(parent);
221 spinbox->setMinimum(-DBL_MAX);
222 spinbox->setMaximum(DBL_MAX);
223 spinbox->setSingleStep(0.01);
224 spinbox->setDecimals(3);
225 return spinbox;
226 }
227 default:
228 return 0;
229 }
230 }
231
232 void JointItemDelegate::setEditorData(QWidget *editor,
233 const QModelIndex &index) const {
234 JointTreeItem::ItemType type =
235 (JointTreeItem::ItemType)index.data(JointTreeItem::TypeRole).toInt();
236 updateTypeRole(type);
237 float q = index.data(Qt::EditRole).toFloat();
238 switch (type) {
239 case JointTreeItem::SkipType:
240 return;
241 case JointTreeItem::IntegratorType:
242 return;
243 case JointTreeItem::BoundedValueType:
244 return;
245 case JointTreeItem::UnboundedValueType:
246 case JointTreeItem::BoundType: {
247 QDoubleSpinBox *spinbox = static_cast<QDoubleSpinBox *>(editor);
248 spinbox->setValue(q);
249 break;
250 }
251 default:
252 break;
253 }
254 }
255
256 void JointItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model,
257 const QModelIndex &index) const {
258 JointTreeItem::ItemType type =
259 (JointTreeItem::ItemType)index.data(JointTreeItem::TypeRole).toInt();
260 updateTypeRole(type);
261 QStandardItemModel *m = static_cast<QStandardItemModel *>(model);
262 JointTreeItem *ji =
263 dynamic_cast<JointTreeItem *>(m->itemFromIndex(index)->parent());
264 double q;
265 switch (type) {
266 case JointTreeItem::SkipType:
267 return;
268 case JointTreeItem::IntegratorType:
269 return;
270 case JointTreeItem::BoundedValueType: {
271 SliderBoundedJoint *slider = static_cast<SliderBoundedJoint *>(editor);
272 q = slider->getValue();
273 break;
274 }
275 case JointTreeItem::UnboundedValueType:
276 case JointTreeItem::BoundType: {
277 QDoubleSpinBox *spinbox = static_cast<QDoubleSpinBox *>(editor);
278 q = spinbox->value();
279 break;
280 }
281 default:
282 return;
283 }
284 model->setData(index, q, Qt::EditRole);
285 assert(ji);
286 ULong idxCfg = (ULong)index.data(JointTreeItem::IndexRole).toInt();
287 switch (type) {
288 case JointTreeItem::BoundedValueType:
289 return;
290 case JointTreeItem::UnboundedValueType:
291 plugin_->currentConfig()[ji->rankInConfig() + idxCfg] = q;
292 break;
293 case JointTreeItem::BoundType:
294 plugin_->client()->robot()->setJointBounds(ji->name().c_str(),
295 ji->bounds());
296 ji->updateTypeRole();
297 break;
298 default:
299 break;
300 }
301 main_->requestApplyCurrentConfiguration();
302 }
303
304 void JointItemDelegate::updateEditorGeometry(
305 QWidget *editor, const QStyleOptionViewItem &option,
306 const QModelIndex & /*index*/) const {
307 editor->setGeometry(option.rect);
308 }
309
310 IntegratorWheel::IntegratorWheel(Qt::Orientation o, HppWidgetsPlugin *plugin,
311 QWidget *parent,
312 gepetto::gui::MainWindow *main,
313 JointTreeItem const *item, int index)
314 : QSlider(o, parent),
315 rate_(100),
316 main_(main),
317 plugin_(plugin),
318 item_(item),
319 bound_(100),
320 maxVelocity_(0.1),
321 index_(index) {
322 setMinimum(-bound_);
323 setMaximum(bound_);
324 q_.length(item_->configSize());
325 dq_.length(item_->numberDof());
326 for (ULong i = 0; i < q_.length(); ++i)
327 q_[i] = plugin_->currentConfig()[item_->rankInConfig() + i];
328 for (ULong i = 0; i < dq_.length(); ++i) dq_[i] = 0;
329 setValue(0);
330 connect(this, SIGNAL(sliderReleased()), this, SLOT(reset()));
331 connect(this, SIGNAL(valueChanged(int)), this, SLOT(updateIntegrator(int)));
332 timerId_ = startTimer(rate_);
333 }
334
335 void IntegratorWheel::timerEvent(QTimerEvent *) {
336 killTimer(timerId_);
337 if (dq_[index_] != 0) {
338 hpp::floatSeq_var q = plugin_->client()->robot()->jointIntegrate(
339 q_, item_->name().c_str(), dq_, true);
340 q_ = q.in();
341 for (ULong i = 0; i < q_.length(); ++i)
342 plugin_->currentConfig()[item_->rankInConfig() + i] = q_[i];
343 main_->requestApplyCurrentConfiguration();
344 }
345 timerId_ = startTimer(rate_);
346 }
347
348 void IntegratorWheel::reset() {
349 dq_[index_] = 0;
350 setValue(0);
351 }
352
353 void IntegratorWheel::updateIntegrator(int value) {
354 dq_[index_] = maxVelocity_ * (double)value / (double)bound_;
355 }
356
357 SliderBoundedJoint::SliderBoundedJoint(Qt::Orientation orientation,
358 HppWidgetsPlugin *plugin,
359 QWidget *parent,
360 gepetto::gui::MainWindow *main,
361 JointTreeItem const *item, int index)
362 : QSlider(orientation, parent),
363 main_(main),
364 plugin_(plugin),
365 item_(item),
366 index_(index) {
367 value_ = item_->config()[index];
368 hpp::floatSeq bounds = item_->bounds();
369 m_ = bounds[2 * index];
370 M_ = bounds[2 * index + 1];
371
372 setMinimum(0);
373 setMaximum(100);
374 setValue((int)(100 * (value_ - m_) / (M_ - m_)));
375 connect(this, SIGNAL(valueChanged(int)), this, SLOT(updateConfig(int)));
376 }
377
378 double SliderBoundedJoint::getValue() { return value_; }
379
380 void SliderBoundedJoint::updateConfig(int value) {
381 value_ = m_ + (double)(value - 0) * (M_ - m_) / (double)100;
382 plugin_->currentConfig()[item_->rankInConfig() + index_] = value_;
383 main_->requestApplyCurrentConfiguration();
384 }
385 } // namespace gui
386 } // namespace hpp
387