motion_planning_frame_joints_widget.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, CITEC, Bielefeld University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CITEC / Bielefeld University nor the names of
18  * its contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Robert Haschke */
36 
39 
40 #include "ui_motion_planning_rviz_plugin_frame_joints.h"
41 #include <QPainter>
42 #include <QDoubleSpinBox>
43 #include <QSlider>
44 #include <QMouseEvent>
45 
46 namespace moveit_rviz_plugin
47 {
48 JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent)
49  : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr)
50 {
51  if (robot_state_.getRobotModel()->hasJointModelGroup(group_name))
52  jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name);
53 }
54 
55 int JMGItemModel::rowCount(const QModelIndex& /*parent*/) const
56 {
57  if (!jmg_)
59  else
60  return jmg_->getVariableCount();
61 }
62 
63 int JMGItemModel::columnCount(const QModelIndex& /*parent*/) const
64 {
65  return 2;
66 }
67 
68 Qt::ItemFlags JMGItemModel::flags(const QModelIndex& index) const
69 {
70  if (!index.isValid())
71  return Qt::ItemFlags();
72 
73  Qt::ItemFlags f = QAbstractTableModel::flags(index);
74 
75  const moveit::core::JointModel* jm = getJointModel(index);
76  bool is_editable = !jm->isPassive() && !jm->getMimic();
77  f.setFlag(Qt::ItemIsEnabled, is_editable);
78  if (index.column() == 1)
79  f.setFlag(Qt::ItemIsEditable, is_editable);
80  return f;
81 }
82 
83 QVariant JMGItemModel::data(const QModelIndex& index, int role) const
84 {
85  if (!index.isValid())
86  return QVariant();
87 
88  int idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
89  switch (index.column())
90  {
91  case 0: // joint name column
92  switch (role)
93  {
94  case Qt::DisplayRole:
95  return QString::fromStdString(robot_state_.getVariableNames()[idx]);
96  case Qt::TextAlignmentRole:
97  return Qt::AlignLeft;
98  }
99  break;
100  case 1: // joint value column
101  {
102  double value = robot_state_.getVariablePosition(idx);
103  const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(idx);
104  switch (role)
105  {
106  case Qt::DisplayRole:
107  return value;
109  if (jm)
110  {
112  if (bounds)
113  {
114  return (value - bounds->min_position_) / (bounds->max_position_ - bounds->min_position_);
115  }
116  }
117  break;
118  case Qt::EditRole:
119  if (jm)
120  return value;
121  break;
123  if (jm)
124  return jm->getType();
125  break;
127  if (const moveit::core::VariableBounds* bounds = getVariableBounds(jm, index))
128  return QPointF(bounds->min_position_, bounds->max_position_);
129  break;
130  case Qt::TextAlignmentRole:
131  return Qt::AlignLeft;
132  }
133  }
134  }
135  return QVariant();
136 }
137 
138 QVariant JMGItemModel::headerData(int section, Qt::Orientation orientation, int role) const
139 {
140  if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
141  return section == 0 ? "Joint Name" : "Value";
142  return QAbstractTableModel::headerData(section, orientation, role);
143 }
144 
145 bool JMGItemModel::setData(const QModelIndex& index, const QVariant& value, int role)
146 {
147  if (index.column() != 1 || (role != Qt::EditRole && role != ProgressBarDelegate::JointRangeFractionRole))
148  return false;
149 
150  int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
151  const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(var_idx);
152  if (!value.canConvert<double>())
153  return false;
154 
155  bool ok;
156  double v = value.toDouble(&ok);
157  if (!ok)
158  return false;
159 
161  {
162  const moveit::core::VariableBounds* bounds = getVariableBounds(jm, index);
163  if (!bounds)
164  return false;
165  v = bounds->min_position_ + v * (bounds->max_position_ - bounds->min_position_);
166  }
167 
168  robot_state_.setVariablePosition(var_idx, v);
170  dataChanged(index, index);
171  return true;
172 }
173 
174 const moveit::core::JointModel* JMGItemModel::getJointModel(const QModelIndex& index) const
175 {
176  if (!index.isValid())
177  return nullptr;
178  int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
179  return robot_state_.getRobotModel()->getJointOfVariable(var_idx);
180 }
181 
183  const QModelIndex& index) const
184 {
185  if (!jm)
186  return nullptr;
187  int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
188  const moveit::core::VariableBounds* bounds = &jm->getVariableBounds()[var_idx - jm->getFirstVariableIndex()];
189  return bounds->position_bounded_ ? bounds : nullptr;
190 }
191 
192 // copy positions from new_state and notify about these changes
194 {
195  if (robot_state_.getRobotModel() != state.getRobotModel())
196  return;
197  robot_state_ = state;
198  dataChanged(index(0, 1), index(rowCount() - 1, 1));
199 }
200 
201 MotionPlanningFrameJointsWidget::MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent)
202  : QWidget(parent), ui_(new Ui::MotionPlanningFrameJointsUI()), planning_display_(display)
203 {
204  ui_->setupUi(this);
205  // intercept mouse events delivered to joints_view_ to operate "sliders"
206  ui_->joints_view_->viewport()->installEventFilter(new JointsWidgetEventFilter(ui_->joints_view_));
207  // intercept keyboard events delivered to joints_view_ to operate joints directly
208  ui_->joints_view_->installEventFilter(new JointsWidgetEventFilter(ui_->joints_view_));
209 
210  auto delegate = new ProgressBarDelegate(this);
211  ui_->button_group_units_->setId(ui_->radio_degree_, ProgressBarDelegate::DEGREES);
212  ui_->button_group_units_->setId(ui_->radio_radian_, ProgressBarDelegate::RADIANS);
213  connect(ui_->button_group_units_, QOverload<QAbstractButton*, bool>::of(&QButtonGroup::buttonToggled),
214  ui_->joints_view_, [delegate, this](QAbstractButton* button, bool checked) {
215  if (checked)
216  {
217  delegate->setUnit(static_cast<ProgressBarDelegate::RevoluteUnit>(ui_->button_group_units_->id(button)));
218  // trigger repaint of joint values
219  auto model = ui_->joints_view_->model();
220  if (model) // during initial loading, the model is not yet set
221  ui_->joints_view_->dataChanged(model->index(0, 1), model->index(model->rowCount() - 1, 1));
222  Q_EMIT configChanged();
223  }
224  });
225  ui_->joints_view_->setItemDelegateForColumn(1, delegate);
226  svd_.setThreshold(0.001);
227 }
228 
229 MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget()
230 {
231  delete ui_;
232 }
233 
234 bool MotionPlanningFrameJointsWidget::useRadians() const
235 {
236  return ui_->radio_radian_->isChecked();
237 }
238 void MotionPlanningFrameJointsWidget::setUseRadians(bool use_radians)
239 {
240  ui_->radio_radian_->setChecked(use_radians);
241 }
242 
243 void MotionPlanningFrameJointsWidget::clearRobotModel()
244 {
245  ui_->joints_view_->setModel(nullptr);
246  start_state_handler_.reset();
247  goal_state_handler_.reset();
248  start_state_model_.reset();
249  goal_state_model_.reset();
250 }
251 
252 void MotionPlanningFrameJointsWidget::changePlanningGroup(
253  const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler,
254  const robot_interaction::InteractionHandlerPtr& goal_state_handler)
255 {
256  // release previous models (if any)
257  clearRobotModel();
258  // create new models
259  start_state_handler_ = start_state_handler;
260  goal_state_handler_ = goal_state_handler;
261  start_state_model_ = std::make_unique<JMGItemModel>(*start_state_handler_->getState(), group_name, this);
262  goal_state_model_ = std::make_unique<JMGItemModel>(*goal_state_handler_->getState(), group_name, this);
263 
264  // forward model updates to the PlanningDisplay
265  connect(start_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() {
266  if (!ignore_state_changes_)
267  planning_display_->setQueryStartState(start_state_model_->getRobotState());
268  });
269  connect(goal_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() {
270  if (!ignore_state_changes_)
271  planning_display_->setQueryGoalState(goal_state_model_->getRobotState());
272  });
273 
274  // show the goal state by default
275  setActiveModel(goal_state_model_.get());
276  updateNullspaceSliders();
277 }
278 
279 void MotionPlanningFrameJointsWidget::queryStartStateChanged()
280 {
281  if (!start_state_model_ || !start_state_handler_)
282  return;
283  ignore_state_changes_ = true;
284  start_state_model_->updateRobotState(*start_state_handler_->getState());
285  ignore_state_changes_ = false;
286  setActiveModel(start_state_model_.get());
287  updateNullspaceSliders();
288 }
289 
290 void MotionPlanningFrameJointsWidget::queryGoalStateChanged()
291 {
292  if (!goal_state_model_ || !goal_state_handler_)
293  return;
294  ignore_state_changes_ = true;
295  goal_state_model_->updateRobotState(*goal_state_handler_->getState());
296  ignore_state_changes_ = false;
297  setActiveModel(goal_state_model_.get());
298  updateNullspaceSliders();
299 }
300 
301 void MotionPlanningFrameJointsWidget::setActiveModel(JMGItemModel* model)
302 {
303  if (ui_->joints_view_->model() == model)
304  return;
305  ui_->joints_view_->setModel(model);
306  ui_->joints_view_label_->setText(
307  QString("Group joints of %1 state").arg(model == start_state_model_.get() ? "start" : "goal"));
308 }
309 
310 void MotionPlanningFrameJointsWidget::triggerUpdate(JMGItemModel* model)
311 {
312  if (model == start_state_model_.get())
313  planning_display_->setQueryStartState(model->getRobotState());
314  else
315  planning_display_->setQueryGoalState(model->getRobotState());
316 }
317 
318 // Find matching key vector in columns of haystack and return the best-aligned column index.
319 // Only consider available indexes (> 0). If no match is found, return largest available index.
320 // sign becomes -1 if key has changed sign compared to the matching haystack column.
321 static Eigen::Index findMatching(const Eigen::VectorXd& key, const Eigen::MatrixXd& haystack,
322  const Eigen::VectorXi& available, double& sign)
323 {
324  Eigen::Index result = available.array().maxCoeff();
325  double best_match = 0.0;
326  for (unsigned int i = 0; i < available.rows(); ++i)
327  {
328  int index = available[i];
329  if (index < 0) // index already taken
330  continue;
331  if (index >= haystack.cols())
332  return result;
333  double match = haystack.col(available[i]).transpose() * key;
334  double abs_match = std::abs(match);
335  if (abs_match > 0.5 && abs_match > best_match)
336  {
337  best_match = abs_match;
338  result = index;
339  sign = match > 0 ? 1.0 : -1.0;
340  }
341  }
342  return result;
343 }
344 
345 void MotionPlanningFrameJointsWidget::updateNullspaceSliders()
346 {
347  JMGItemModel* model = dynamic_cast<JMGItemModel*>(ui_->joints_view_->model());
348  std::size_t i = 0;
349  if (model && model->getJointModelGroup() && model->getJointModelGroup()->isChain())
350  {
352  Eigen::MatrixXd jacobian;
354  model->getJointModelGroup()->getLinkModels().back(),
355  Eigen::Vector3d::Zero(), jacobian, false))
356  goto cleanup;
357 
358  svd_.compute(jacobian, Eigen::ComputeFullV);
359  Eigen::Index rank = svd_.rank();
360  std::size_t ns_dim = svd_.cols() - rank;
361  Eigen::MatrixXd ns(svd_.cols(), ns_dim);
362  Eigen::VectorXi available(ns_dim);
363  for (std::size_t j = 0; j < ns_dim; ++j)
364  available[j] = j;
365 
366  ns_sliders_.reserve(ns_dim);
367  // create/unhide sliders
368  for (; i < ns_dim; ++i)
369  {
370  if (i >= ns_sliders_.size())
371  ns_sliders_.push_back(createNSSlider(i + 1));
372  ns_sliders_[i]->show();
373 
374  // Find matching null-space basis vector in previous nullspace_
375  double sign = 1.0;
376  const Eigen::VectorXd& current = svd_.matrixV().col(rank + i);
377  Eigen::Index index = findMatching(current, nullspace_, available, sign);
378  ns.col(index).noalias() = sign * current;
379  available[index] = -1; // mark index as taken
380  }
381  nullspace_ = ns;
382  }
383 
384 cleanup:
385  if (i == 0)
386  nullspace_.resize(0, 0);
387 
388  // show/hide dummy slider
389  ui_->dummy_ns_slider_->setVisible(i == 0);
390 
391  // hide remaining sliders
392  for (; i < ns_sliders_.size(); ++i)
393  {
394  ns_sliders_[i]->setValue(0);
395  ns_sliders_[i]->hide();
396  }
397 }
398 
399 QSlider* MotionPlanningFrameJointsWidget::createNSSlider(int i)
400 {
401  JogSlider* slider = new JogSlider(this);
402  slider->setOrientation(Qt::Horizontal);
403  slider->setMaximum(0.1);
404  slider->setToolTip(QString("Nullspace dim #%1").arg(i));
405  ui_->nullspace_layout_->addWidget(slider);
406  connect(slider, SIGNAL(triggered(double)), this, SLOT(jogNullspace(double)));
407  return slider;
408 }
409 
410 void MotionPlanningFrameJointsWidget::jogNullspace(double value)
411 {
412  if (value == 0)
413  return;
414 
415  std::size_t index = std::find(ns_sliders_.begin(), ns_sliders_.end(), sender()) - ns_sliders_.begin();
416  if (static_cast<int>(index) >= nullspace_.cols())
417  return;
418 
419  JMGItemModel* model = dynamic_cast<JMGItemModel*>(ui_->joints_view_->model());
420  if (!model)
421  return;
422 
423  Eigen::VectorXd values;
424  model->getRobotState().copyJointGroupPositions(model->getJointModelGroup(), values);
425  values += value * nullspace_.col(index);
426  model->getRobotState().setJointGroupPositions(model->getJointModelGroup(), values);
427  model->getRobotState().harmonizePositions(model->getJointModelGroup());
428  triggerUpdate(model);
429 }
430 
431 namespace
432 {
433 void paintProgressBar(QPainter* painter, QStyle* style, const QString& text, float value, const QRect& rect)
434 {
435  QStyleOptionProgressBar opt;
436  opt.rect = rect;
437  opt.minimum = 0;
438  opt.maximum = 1000;
439  opt.progress = 1000. * value;
440  opt.text = text;
441  opt.textAlignment = Qt::AlignCenter;
442  opt.textVisible = true;
443  style->drawControl(QStyle::CE_ProgressBar, &opt, painter);
444 }
445 } // namespace
446 
447 void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
448 {
449  // copied from QStyledItemDelegate::paint
450  QStyle* style = option.widget ? option.widget->style() : QApplication::style();
451  QStyleOptionViewItem style_option = option;
452  initStyleOption(&style_option, index);
453 
454  if (index.column() == 1)
455  {
456  QVariant joint_type = index.data(JointTypeRole);
457  double value = index.data().toDouble();
458  if (joint_type.isValid())
459  {
460  switch (joint_type.toInt())
461  {
463  if (unit_ == RADIANS)
464  style_option.text = option.locale.toString(value, 'f', 3);
465  else
466  style_option.text = option.locale.toString(value * 180 / M_PI, 'f', 0).append("°");
467  break;
469  style_option.text = option.locale.toString(value, 'f', 3).append("m");
470  break;
471  default:
472  break;
473  }
474  }
475 
476  QVariant vbounds = index.data(VariableBoundsRole);
477  if (vbounds.isValid())
478  {
479  const double progressbar_fraction{ index.data(ProgressBarDelegate::JointRangeFractionRole).toDouble() };
480  paintProgressBar(painter, style, style_option.text, progressbar_fraction, style_option.rect);
481  return;
482  }
483  }
484 
485  style->drawControl(QStyle::CE_ItemViewItem, &style_option, painter, option.widget);
486 }
487 
488 QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& option,
489  const QModelIndex& index) const
490 
491 {
492  auto editor = QStyledItemDelegate::createEditor(parent, option, index);
493  if (auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
494  {
495  bool is_revolute = (index.data(ProgressBarDelegate::JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE);
496  if (is_revolute)
497  {
498  if (unit_ == RADIANS)
499  {
500  spinbox->setSuffix("");
501  spinbox->setDecimals(3);
502  spinbox->setSingleStep(0.01);
503  }
504  else
505  {
506  spinbox->setSuffix("°");
507  spinbox->setDecimals(0);
508  spinbox->setSingleStep(1);
509  }
510  }
511  else
512  {
513  spinbox->setSuffix("m");
514  spinbox->setDecimals(3);
515  spinbox->setSingleStep(0.001);
516  }
517  }
518  connect(editor, &QObject::destroyed, this, &ProgressBarDelegate::onEditorDestroyed);
519  ++editor_open_count_;
520  return editor;
521 }
522 
523 void ProgressBarDelegate::onEditorDestroyed(QObject* /* editor */) const
524 {
525  if (editor_open_count_ > 0)
526  --editor_open_count_;
527 }
528 
529 bool ProgressBarDelegate::isEditing() const
530 {
531  return editor_open_count_ > 0;
532 }
533 
534 void ProgressBarDelegate::setEditorData(QWidget* editor, const QModelIndex& index) const
535 {
536  if (auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
537  {
538  if (index.data(ProgressBarDelegate::JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE)
539  {
540  if (unit_ == RADIANS)
541  spinbox->setValue(index.data().toDouble());
542  else
543  spinbox->setValue(index.data().toDouble() * 180 / M_PI);
544  }
545  else
546  spinbox->setValue(index.data().toDouble());
547  }
548  else
549  QStyledItemDelegate::setEditorData(editor, index);
550 }
551 
552 void ProgressBarDelegate::setModelData(QWidget* editor, QAbstractItemModel* model, const QModelIndex& index) const
553 {
554  if (auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
555  {
556  if (index.data(ProgressBarDelegate::JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE)
557  {
558  if (unit_ == RADIANS)
559  model->setData(index, spinbox->value(), Qt::EditRole);
560  else
561  model->setData(index, spinbox->value() * M_PI / 180, Qt::EditRole);
562  }
563  else
564  model->setData(index, spinbox->value(), Qt::EditRole);
565  }
566  else
567  QStyledItemDelegate::setModelData(editor, model, index);
568 }
569 
570 JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObject(view)
571 {
572 }
573 
574 bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event)
575 {
576  if ((event->type() == QEvent::MouseButtonPress || event->type() == QEvent::MouseMove) &&
577  static_cast<QMouseEvent*>(event)->buttons() & Qt::LeftButton)
578  {
579  QAbstractItemView* view = qobject_cast<QAbstractItemView*>(parent());
580  if (event->type() == QEvent::MouseButtonPress)
581  { // start dragging the "slider"
582  QModelIndex index = view->indexAt(static_cast<QMouseEvent*>(event)->pos());
583  QVariant vbounds = index.data(ProgressBarDelegate::VariableBoundsRole);
584  view->setCurrentIndex(index);
585  if (!index.isValid() || !(index.flags() & Qt::ItemIsEditable))
586  return false;
587  if (!vbounds.isValid())
588  {
589  ProgressBarDelegate* delegate = qobject_cast<ProgressBarDelegate*>(view->itemDelegateForColumn(1));
590  if (delegate && !delegate->isEditing())
591  view->edit(index);
592  return false;
593  }
594 
595  active_ = index;
596  const QRect& rect = view->visualRect(active_);
597  pmin_ = rect.x();
598  pmax_ = rect.x() + rect.width();
599  }
600  else if (!active_.isValid())
601  return false;
602 #if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
603  auto x = static_cast<QMouseEvent*>(event)->pos().x();
604 #else
605  auto x = static_cast<QMouseEvent*>(event)->position().x();
606 #endif
607  float v = static_cast<float>(x - pmin_) / (pmax_ - pmin_);
608  view->model()->setData(active_, v, ProgressBarDelegate::JointRangeFractionRole);
609  return true; // event handled
610  }
611  else if (event->type() == QEvent::MouseButtonRelease && static_cast<QMouseEvent*>(event)->button() == Qt::LeftButton)
612  active_ = QModelIndex();
613 
614  else if (event->type() == QEvent::KeyPress)
615  {
616  QKeyEvent* key_event = static_cast<QKeyEvent*>(event);
617 
618  if (key_event->key() != Qt::Key_Left && key_event->key() != Qt::Key_Right && key_event->key() != Qt::Key_Return)
619  return false; // only react to these events
620 
621  QAbstractItemView* view = qobject_cast<QAbstractItemView*>(parent());
622  QModelIndex index = view->currentIndex();
623  index = index.sibling(index.row(), 1);
624 
625  if (key_event->key() == Qt::Key_Return)
626  {
627  ProgressBarDelegate* delegate = qobject_cast<ProgressBarDelegate*>(view->itemDelegateForColumn(1));
628  if (delegate && !delegate->isEditing())
629  view->edit(index);
630  return false;
631  }
632 
633  if (key_event->type() == QEvent::KeyPress && key_event->modifiers() == Qt::NoModifier &&
634  index.flags() & Qt::ItemIsEditable)
635  {
636  if (!key_event->isAutoRepeat()) // first key press: initialize delta_ from joint type and direction
637  {
638  delta_ = key_event->key() == Qt::Key_Left ? -0.002f : 0.002f;
639  }
640  else // increase delta in a multiplicative fashion when holding down the key
641  {
642  delta_ *= 1.1;
643  }
644  }
645 
646  float current = index.data(ProgressBarDelegate::JointRangeFractionRole).toFloat();
647  view->model()->setData(index, current + delta_, ProgressBarDelegate::JointRangeFractionRole);
648  return true;
649  }
650  return false;
651 }
652 
653 JogSlider::JogSlider(QWidget* parent) : QSlider(parent)
654 {
655  setTimerInterval(50);
656  setResolution(1000);
657  setMaximum(1.0);
658 }
659 
660 void JogSlider::setTimerInterval(int ms)
661 {
663 }
664 
665 void JogSlider::setResolution(unsigned int resolution)
666 {
667  QSlider::setRange(-resolution, +resolution);
668 }
669 
670 void JogSlider::setMaximum(double value)
671 {
672  maximum_ = value;
673 }
674 
675 void JogSlider::timerEvent(QTimerEvent* event)
676 {
677  QSlider::timerEvent(event);
678  if (event->timerId() == timer_id_)
679  triggered(value());
680 }
681 
682 void JogSlider::mousePressEvent(QMouseEvent* event)
683 {
684  QSlider::mousePressEvent(event);
685  timer_id_ = startTimer(timer_interval_);
686 }
687 
688 void JogSlider::mouseReleaseEvent(QMouseEvent* event)
689 {
690  killTimer(timer_id_);
691  QSlider::mouseReleaseEvent(event);
692  setValue(0);
693 }
694 
695 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::JogSlider::JogSlider
JogSlider(QWidget *parent=nullptr)
Definition: motion_planning_frame_joints_widget.cpp:685
moveit_rviz_plugin::JogSlider::triggered
void triggered(double value)
moveit_rviz_plugin::JointsWidgetEventFilter::delta_
float delta_
Definition: motion_planning_frame_joints_widget.h:113
moveit::core::JointModel::enforcePositionBounds
bool enforcePositionBounds(double *values) const
moveit_rviz_plugin::JMGItemModel::rowCount
int rowCount(const QModelIndex &parent=QModelIndex()) const override
Definition: motion_planning_frame_joints_widget.cpp:87
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
moveit::core::JointModelGroup::getVariableIndexList
const std::vector< int > & getVariableIndexList() const
moveit::core::VariableBounds
motion_planning_display.h
moveit::core::VariableBounds::max_position_
double max_position_
moveit_rviz_plugin::JointsWidgetEventFilter::pmin_
int pmin_
Definition: motion_planning_frame_joints_widget.h:112
moveit_rviz_plugin::JMGItemModel::flags
Qt::ItemFlags flags(const QModelIndex &index) const override
Definition: motion_planning_frame_joints_widget.cpp:100
moveit::core::RobotState::getJacobian
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
moveit_rviz_plugin::ProgressBarDelegate::JointTypeRole
@ JointTypeRole
Definition: motion_planning_frame_joints_widget.h:177
moveit_rviz_plugin::ProgressBarDelegate::RADIANS
@ RADIANS
Definition: motion_planning_frame_joints_widget.h:184
moveit_rviz_plugin::JMGItemModel::getJointModelGroup
const moveit::core::JointModelGroup * getJointModelGroup() const
Definition: motion_planning_frame_joints_widget.h:95
moveit_rviz_plugin::JMGItemModel::jmg_
const moveit::core::JointModelGroup * jmg_
Definition: motion_planning_frame_joints_widget.h:71
moveit::core::RobotState
moveit_rviz_plugin::JogSlider::timer_id_
int timer_id_
Definition: motion_planning_frame_joints_widget.h:214
moveit_rviz_plugin::JogSlider::setTimerInterval
void setTimerInterval(int ms)
Definition: motion_planning_frame_joints_widget.cpp:692
moveit::core::VariableBounds::min_position_
double min_position_
moveit::core::JointModel::PRISMATIC
PRISMATIC
moveit_rviz_plugin::ProgressBarDelegate::DEGREES
@ DEGREES
Definition: motion_planning_frame_joints_widget.h:183
moveit_rviz_plugin::JMGItemModel::robot_state_
moveit::core::RobotState robot_state_
Definition: motion_planning_frame_joints_widget.h:70
ok
ROSCPP_DECL bool ok()
moveit_rviz_plugin::JogSlider::setResolution
void setResolution(unsigned int resolution)
Definition: motion_planning_frame_joints_widget.cpp:697
moveit_rviz_plugin::JogSlider::timer_interval_
int timer_interval_
Definition: motion_planning_frame_joints_widget.h:215
ms
ms
moveit_rviz_plugin::JogSlider::maximum_
double maximum_
Definition: motion_planning_frame_joints_widget.h:216
moveit::core::RobotState::getVariableNames
const std::vector< std::string > & getVariableNames() const
moveit::core::JointModelGroup::getLinkModels
const std::vector< const LinkModel * > & getLinkModels() const
moveit::core::JointModel::getType
JointType getType() const
moveit::core::JointModel::REVOLUTE
REVOLUTE
moveit_rviz_plugin::JMGItemModel::getRobotState
moveit::core::RobotState & getRobotState()
Definition: motion_planning_frame_joints_widget.h:87
moveit_rviz_plugin::JMGItemModel::columnCount
int columnCount(const QModelIndex &parent=QModelIndex()) const override
Definition: motion_planning_frame_joints_widget.cpp:95
moveit::core::JointModel::getVariableBounds
const VariableBounds & getVariableBounds(const std::string &variable) const
moveit::core::RobotState::setVariablePosition
void setVariablePosition(const std::string &variable, double value)
Ui
Definition: motion_planning_frame.h:69
moveit_rviz_plugin::JMGItemModel
Definition: motion_planning_frame_joints_widget.h:67
moveit::core::JointModel::getFirstVariableIndex
int getFirstVariableIndex() const
moveit_rviz_plugin::JogSlider::mouseReleaseEvent
void mouseReleaseEvent(QMouseEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:720
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
moveit::core::RobotState::getVariablePosition
double getVariablePosition(const std::string &variable) const
moveit_rviz_plugin::ProgressBarDelegate::isEditing
bool isEditing() const
Definition: motion_planning_frame_joints_widget.cpp:561
moveit_rviz_plugin::JointsWidgetEventFilter
Definition: motion_planning_frame_joints_widget.h:108
value
float value
moveit_rviz_plugin::JMGItemModel::getVariableBounds
const moveit::core::VariableBounds * getVariableBounds(const moveit::core::JointModel *jm, const QModelIndex &index) const
retrieve the variable bounds referenced by variable index
Definition: motion_planning_frame_joints_widget.cpp:214
x
x
moveit_rviz_plugin::JogSlider::value
double value() const
Definition: motion_planning_frame_joints_widget.h:228
moveit_rviz_plugin::JMGItemModel::data
QVariant data(const QModelIndex &index, int role) const override
Definition: motion_planning_frame_joints_widget.cpp:115
moveit_rviz_plugin::JointsWidgetEventFilter::pmax_
int pmax_
Definition: motion_planning_frame_joints_widget.h:112
moveit_rviz_plugin::JMGItemModel::updateRobotState
void updateRobotState(const moveit::core::RobotState &state)
call this on any external change of the RobotState
Definition: motion_planning_frame_joints_widget.cpp:225
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::MotionPlanningFrameJointsWidget
MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget &)=delete
text
text
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit::core::RobotState::getVariablePositions
double * getVariablePositions()
f
f
moveit_rviz_plugin::ProgressBarDelegate::JointRangeFractionRole
@ JointRangeFractionRole
Definition: motion_planning_frame_joints_widget.h:179
values
std::vector< double > values
moveit::core::JointModel::getMimic
const JointModel * getMimic() const
moveit_rviz_plugin::JogSlider::setMaximum
void setMaximum(double value)
Definition: motion_planning_frame_joints_widget.cpp:702
moveit_rviz_plugin::JMGItemModel::setData
bool setData(const QModelIndex &index, const QVariant &value, int role) override
Definition: motion_planning_frame_joints_widget.cpp:177
M_PI
#define M_PI
moveit_rviz_plugin::JMGItemModel::JMGItemModel
JMGItemModel(const moveit::core::RobotState &robot_state, const std::string &group_name, QObject *parent=nullptr)
Definition: motion_planning_frame_joints_widget.cpp:80
moveit::core::JointModelGroup::isChain
bool isChain() const
moveit_rviz_plugin::ProgressBarDelegate
Delegate to show the joint value as with a progress bar indicator between min and max.
Definition: motion_planning_frame_joints_widget.h:170
moveit_rviz_plugin::JogSlider::mousePressEvent
void mousePressEvent(QMouseEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:714
index
unsigned int index
v
v
moveit_rviz_plugin::JogSlider::timerEvent
void timerEvent(QTimerEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:707
motion_planning_frame_joints_widget.h
moveit_rviz_plugin::JMGItemModel::headerData
QVariant headerData(int section, Qt::Orientation orientation, int role) const override
Definition: motion_planning_frame_joints_widget.cpp:170
moveit_rviz_plugin::JMGItemModel::getJointModel
const moveit::core::JointModel * getJointModel(const QModelIndex &index) const
retrieve the JointModel corresponding to the variable referenced by index
Definition: motion_planning_frame_joints_widget.cpp:206
moveit_rviz_plugin::JointsWidgetEventFilter::eventFilter
bool eventFilter(QObject *target, QEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:606
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
moveit::core::VariableBounds::position_bounded_
bool position_bounded_
cleanup
moveit_rviz_plugin::ProgressBarDelegate::VariableBoundsRole
@ VariableBoundsRole
Definition: motion_planning_frame_joints_widget.h:178
moveit::core::JointModel::isPassive
bool isPassive() const
moveit_rviz_plugin::findMatching
static Eigen::Index findMatching(const Eigen::VectorXd &key, const Eigen::MatrixXd &haystack, const Eigen::VectorXi &available, double &sign)
Definition: motion_planning_frame_joints_widget.cpp:353
moveit::core::JointModel
moveit_rviz_plugin::JointsWidgetEventFilter::active_
QModelIndex active_
Definition: motion_planning_frame_joints_widget.h:111


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:25