40 #include "ui_motion_planning_rviz_plugin_frame_joints.h"
42 #include <QDoubleSpinBox>
44 #include <QMouseEvent>
49 : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr)
51 if (robot_state_.getRobotModel()->hasJointModelGroup(group_name))
52 jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name);
71 return Qt::ItemFlags();
73 Qt::ItemFlags
f = QAbstractTableModel::flags(index);
77 f.setFlag(Qt::ItemIsEnabled, is_editable);
78 if (
index.column() == 1)
79 f.setFlag(Qt::ItemIsEditable, is_editable);
89 switch (
index.column())
96 case Qt::TextAlignmentRole:
106 case Qt::DisplayRole:
128 return QPointF(bounds->min_position_, bounds->max_position_);
130 case Qt::TextAlignmentRole:
131 return Qt::AlignLeft;
140 if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
141 return section == 0 ?
"Joint Name" :
"Value";
142 return QAbstractTableModel::headerData(section, orientation, role);
152 if (!
value.canConvert<
double>())
156 double v =
value.toDouble(&ok);
170 dataChanged(index, index);
176 if (!
index.isValid())
183 const QModelIndex& index)
const
202 : QWidget(parent), ui_(new
Ui::MotionPlanningFrameJointsUI()), planning_display_(display)
213 connect(ui_->button_group_units_, QOverload<QAbstractButton*, bool>::of(&QButtonGroup::buttonToggled),
214 ui_->joints_view_, [delegate,
this](QAbstractButton* button,
bool checked) {
217 delegate->setUnit(static_cast<ProgressBarDelegate::RevoluteUnit>(ui_->button_group_units_->id(button)));
219 auto model = ui_->joints_view_->model();
221 ui_->joints_view_->dataChanged(model->index(0, 1), model->index(model->rowCount() - 1, 1));
222 Q_EMIT configChanged();
225 ui_->joints_view_->setItemDelegateForColumn(1, delegate);
226 svd_.setThreshold(0.001);
229 MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget()
234 bool MotionPlanningFrameJointsWidget::useRadians()
const
236 return ui_->radio_radian_->isChecked();
238 void MotionPlanningFrameJointsWidget::setUseRadians(
bool use_radians)
240 ui_->radio_radian_->setChecked(use_radians);
243 void MotionPlanningFrameJointsWidget::clearRobotModel()
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();
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)
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);
265 connect(start_state_model_.get(), &JMGItemModel::dataChanged,
this, [
this]() {
266 if (!ignore_state_changes_)
267 planning_display_->setQueryStartState(start_state_model_->getRobotState());
269 connect(goal_state_model_.get(), &JMGItemModel::dataChanged,
this, [
this]() {
270 if (!ignore_state_changes_)
271 planning_display_->setQueryGoalState(goal_state_model_->getRobotState());
275 setActiveModel(goal_state_model_.get());
276 updateNullspaceSliders();
279 void MotionPlanningFrameJointsWidget::queryStartStateChanged()
281 if (!start_state_model_ || !start_state_handler_)
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();
290 void MotionPlanningFrameJointsWidget::queryGoalStateChanged()
292 if (!goal_state_model_ || !goal_state_handler_)
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();
301 void MotionPlanningFrameJointsWidget::setActiveModel(JMGItemModel* model)
303 if (ui_->joints_view_->model() == model)
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"));
310 void MotionPlanningFrameJointsWidget::triggerUpdate(JMGItemModel* model)
312 if (model == start_state_model_.get())
313 planning_display_->setQueryStartState(model->getRobotState());
315 planning_display_->setQueryGoalState(model->getRobotState());
321 static Eigen::Index
findMatching(
const Eigen::VectorXd& key,
const Eigen::MatrixXd& haystack,
322 const Eigen::VectorXi& available,
double& sign)
324 Eigen::Index result = available.array().maxCoeff();
325 double best_match = 0.0;
326 for (
unsigned int i = 0; i < available.rows(); ++i)
328 int index = available[i];
331 if (
index >= haystack.cols())
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)
337 best_match = abs_match;
339 sign = match > 0 ? 1.0 : -1.0;
345 void MotionPlanningFrameJointsWidget::updateNullspaceSliders()
352 Eigen::MatrixXd jacobian;
355 Eigen::Vector3d::Zero(), jacobian,
false))
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)
366 ns_sliders_.reserve(ns_dim);
368 for (; i < ns_dim; ++i)
370 if (i >= ns_sliders_.size())
371 ns_sliders_.push_back(createNSSlider(i + 1));
372 ns_sliders_[i]->show();
376 const Eigen::VectorXd& current = svd_.matrixV().col(rank + i);
378 ns.col(index).noalias() = sign * current;
379 available[
index] = -1;
386 nullspace_.resize(0, 0);
389 ui_->dummy_ns_slider_->setVisible(i == 0);
392 for (; i < ns_sliders_.size(); ++i)
394 ns_sliders_[i]->setValue(0);
395 ns_sliders_[i]->hide();
399 QSlider* MotionPlanningFrameJointsWidget::createNSSlider(
int i)
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)));
410 void MotionPlanningFrameJointsWidget::jogNullspace(
double value)
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())
419 JMGItemModel* model =
dynamic_cast<JMGItemModel*
>(ui_->joints_view_->model());
424 model->getRobotState().copyJointGroupPositions(model->getJointModelGroup(),
values);
426 model->getRobotState().setJointGroupPositions(model->getJointModelGroup(),
values);
427 model->getRobotState().harmonizePositions(model->getJointModelGroup());
428 triggerUpdate(model);
433 void paintProgressBar(QPainter* painter, QStyle* style,
const QString&
text,
float value,
const QRect& rect)
435 QStyleOptionProgressBar opt;
439 opt.progress = 1000. *
value;
441 opt.textAlignment = Qt::AlignCenter;
442 opt.textVisible =
true;
443 style->drawControl(QStyle::CE_ProgressBar, &opt, painter);
447 void ProgressBarDelegate::paint(QPainter* painter,
const QStyleOptionViewItem& option,
const QModelIndex& index)
const
450 QStyle* style = option.widget ? option.widget->style() : QApplication::style();
451 QStyleOptionViewItem style_option = option;
452 initStyleOption(&style_option,
index);
454 if (
index.column() == 1)
456 QVariant joint_type =
index.data(JointTypeRole);
458 if (joint_type.isValid())
460 switch (joint_type.toInt())
463 if (unit_ == RADIANS)
464 style_option.text = option.locale.toString(
value,
'f', 3);
466 style_option.text = option.locale.toString(
value * 180 /
M_PI,
'f', 0).append(
"°");
469 style_option.text = option.locale.toString(
value,
'f', 3).append(
"m");
476 QVariant vbounds =
index.data(VariableBoundsRole);
477 if (vbounds.isValid())
479 const double progressbar_fraction{
index.data(ProgressBarDelegate::JointRangeFractionRole).toDouble() };
480 paintProgressBar(painter, style, style_option.text, progressbar_fraction, style_option.rect);
485 style->drawControl(QStyle::CE_ItemViewItem, &style_option, painter, option.widget);
488 QWidget* ProgressBarDelegate::createEditor(QWidget* parent,
const QStyleOptionViewItem& option,
489 const QModelIndex& index)
const
492 auto editor = QStyledItemDelegate::createEditor(parent, option, index);
493 if (
auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
498 if (unit_ == RADIANS)
500 spinbox->setSuffix(
"");
501 spinbox->setDecimals(3);
502 spinbox->setSingleStep(0.01);
506 spinbox->setSuffix(
"°");
507 spinbox->setDecimals(0);
508 spinbox->setSingleStep(1);
513 spinbox->setSuffix(
"m");
514 spinbox->setDecimals(3);
515 spinbox->setSingleStep(0.001);
518 connect(editor, &QObject::destroyed,
this, &ProgressBarDelegate::onEditorDestroyed);
519 ++editor_open_count_;
523 void ProgressBarDelegate::onEditorDestroyed(QObject* )
const
525 if (editor_open_count_ > 0)
526 --editor_open_count_;
529 bool ProgressBarDelegate::isEditing()
const
531 return editor_open_count_ > 0;
534 void ProgressBarDelegate::setEditorData(QWidget* editor,
const QModelIndex& index)
const
536 if (
auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
540 if (unit_ == RADIANS)
541 spinbox->setValue(
index.data().toDouble());
543 spinbox->setValue(
index.data().toDouble() * 180 / M_PI);
546 spinbox->setValue(
index.data().toDouble());
549 QStyledItemDelegate::setEditorData(editor, index);
552 void ProgressBarDelegate::setModelData(QWidget* editor, QAbstractItemModel* model,
const QModelIndex& index)
const
554 if (
auto spinbox = qobject_cast<QDoubleSpinBox*>(editor))
558 if (unit_ == RADIANS)
559 model->setData(
index, spinbox->value(), Qt::EditRole);
561 model->setData(index, spinbox->value() * M_PI / 180, Qt::EditRole);
564 model->setData(index, spinbox->value(), Qt::EditRole);
567 QStyledItemDelegate::setModelData(editor, model, index);
570 JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObject(view)
576 if ((event->type() == QEvent::MouseButtonPress || event->type() == QEvent::MouseMove) &&
577 static_cast<QMouseEvent*
>(event)->buttons() & Qt::LeftButton)
579 QAbstractItemView* view = qobject_cast<QAbstractItemView*>(parent());
580 if (event->type() == QEvent::MouseButtonPress)
582 QModelIndex
index = view->indexAt(
static_cast<QMouseEvent*
>(event)->pos());
584 view->setCurrentIndex(index);
585 if (!
index.isValid() || !(
index.flags() & Qt::ItemIsEditable))
587 if (!vbounds.isValid())
589 ProgressBarDelegate* delegate = qobject_cast<ProgressBarDelegate*>(view->itemDelegateForColumn(1));
596 const QRect& rect = view->visualRect(
active_);
598 pmax_ = rect.x() + rect.width();
602 #if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
603 auto x =
static_cast<QMouseEvent*
>(event)->pos().x();
605 auto x =
static_cast<QMouseEvent*
>(event)->position().x();
611 else if (event->type() == QEvent::MouseButtonRelease &&
static_cast<QMouseEvent*
>(event)->button() == Qt::LeftButton)
614 else if (event->type() == QEvent::KeyPress)
616 QKeyEvent* key_event =
static_cast<QKeyEvent*
>(event);
618 if (key_event->key() != Qt::Key_Left && key_event->key() != Qt::Key_Right && key_event->key() != Qt::Key_Return)
621 QAbstractItemView* view = qobject_cast<QAbstractItemView*>(parent());
622 QModelIndex
index = view->currentIndex();
625 if (key_event->key() == Qt::Key_Return)
627 ProgressBarDelegate* delegate = qobject_cast<ProgressBarDelegate*>(view->itemDelegateForColumn(1));
633 if (key_event->type() == QEvent::KeyPress && key_event->modifiers() == Qt::NoModifier &&
634 index.flags() & Qt::ItemIsEditable)
636 if (!key_event->isAutoRepeat())
638 delta_ = key_event->key() == Qt::Key_Left ? -0.002f : 0.002f;
655 setTimerInterval(50);
667 QSlider::setRange(-resolution, +resolution);
677 QSlider::timerEvent(event);
684 QSlider::mousePressEvent(event);
691 QSlider::mouseReleaseEvent(event);