moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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 <QSlider>
43#include <QEvent>
44#include <QMouseEvent>
45
46namespace moveit_rviz_plugin
47{
48
49JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent)
50 : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr)
51{
52 if (robot_state_.getRobotModel()->hasJointModelGroup(group_name))
53 jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name);
54}
55
56int JMGItemModel::rowCount(const QModelIndex& /*parent*/) const
57{
58 if (!jmg_)
59 {
60 return robot_state_.getVariableCount();
61 }
62 else
63 {
64 return jmg_->getVariableCount();
65 }
66}
67
68int JMGItemModel::columnCount(const QModelIndex& /*parent*/) const
69{
70 return 2;
71}
72
73Qt::ItemFlags JMGItemModel::flags(const QModelIndex& index) const
74{
75 if (!index.isValid())
76 return Qt::ItemFlags();
77
78 Qt::ItemFlags f = QAbstractTableModel::flags(index);
79
80 const moveit::core::JointModel* jm = getJointModel(index);
81 bool is_editable = !jm->isPassive() && !jm->getMimic();
82 f.setFlag(Qt::ItemIsEnabled, is_editable);
83 if (index.column() == 1)
84 f.setFlag(Qt::ItemIsEditable, is_editable);
85 return f;
86}
87
88QVariant JMGItemModel::data(const QModelIndex& index, int role) const
89{
90 if (!index.isValid())
91 return QVariant();
92
93 int idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
94 switch (index.column())
95 {
96 case 0: // joint name column
97 switch (role)
98 {
99 case Qt::DisplayRole:
100 return QString::fromStdString(robot_state_.getVariableNames()[idx]);
101 case Qt::TextAlignmentRole:
102 return Qt::AlignLeft;
103 }
104 break;
105 case 1: // joint value column
106 {
107 double value = robot_state_.getVariablePosition(idx);
108 const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(idx);
109 switch (role)
110 {
111 case Qt::DisplayRole:
112 return value;
113 case Qt::EditRole:
114 if (jm)
115 return jm->getType() == moveit::core::JointModel::REVOLUTE ? value * 180 / M_PI : value;
116 break;
118 if (jm)
119 return jm->getType();
120 break;
122 if (const moveit::core::VariableBounds* bounds = getVariableBounds(jm, index))
123 return QPointF(bounds->min_position_, bounds->max_position_);
124 break;
125 case Qt::TextAlignmentRole:
126 return Qt::AlignLeft;
127 }
128 }
129 }
130 return QVariant();
131}
132
133QVariant JMGItemModel::headerData(int section, Qt::Orientation orientation, int role) const
134{
135 if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
136 return section == 0 ? "Joint Name" : "Value";
137 return QAbstractTableModel::headerData(section, orientation, role);
138}
139
140bool JMGItemModel::setData(const QModelIndex& index, const QVariant& value, int role)
141{
142 if (index.column() != 1 || role != Qt::EditRole)
143 return false;
144
145 int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
146 const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(var_idx);
147 if (!value.canConvert<double>())
148 return false;
149
150 bool ok;
151 double v = value.toDouble(&ok);
152 if (!ok)
153 return false;
154
155 // for revolute joints, we convert degrees to radians
157 v *= M_PI / 180;
158
159 robot_state_.setVariablePosition(var_idx, v);
160 jm->enforcePositionBounds(robot_state_.getVariablePositions() + jm->getFirstVariableIndex());
161 dataChanged(index, index);
162 return true;
163}
164
165const moveit::core::JointModel* JMGItemModel::getJointModel(const QModelIndex& index) const
166{
167 if (!index.isValid())
168 return nullptr;
169 int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
170 return robot_state_.getRobotModel()->getJointOfVariable(var_idx);
171}
172
173const moveit::core::VariableBounds* JMGItemModel::getVariableBounds(const moveit::core::JointModel* jm,
174 const QModelIndex& index) const
175{
176 if (!jm)
177 return nullptr;
178 int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row();
179 const moveit::core::VariableBounds* bounds = &jm->getVariableBounds()[var_idx - jm->getFirstVariableIndex()];
180 return bounds->position_bounded_ ? bounds : nullptr;
181}
182
183// copy positions from new_state and notify about these changes
185{
186 if (robot_state_.getRobotModel() != state.getRobotModel())
187 return;
188 robot_state_.setVariablePositions(state.getVariablePositions());
189
190 dataChanged(index(0, 1), index(rowCount() - 1, 1));
191}
192
194 : QWidget(parent), ui_(new Ui::MotionPlanningFrameJointsUI()), planning_display_(display)
195{
196 ui_->setupUi(this);
197 // intercept mouse events delivered to joints_view_ to open editor on first mouse press
198 ui_->joints_view_->viewport()->installEventFilter(new JointsWidgetEventFilter(ui_->joints_view_));
199 ui_->joints_view_->setItemDelegateForColumn(1, new ProgressBarDelegate(this));
200 svd_.setThreshold(0.001);
201}
202
207
209{
210 ui_->joints_view_->setModel(nullptr);
211 start_state_handler_.reset();
212 goal_state_handler_.reset();
213 start_state_model_.reset();
214 goal_state_model_.reset();
215}
216
218 const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler,
219 const robot_interaction::InteractionHandlerPtr& goal_state_handler)
220{
221 // release previous models (if any)
223 // create new models
224 start_state_handler_ = start_state_handler;
225 goal_state_handler_ = goal_state_handler;
226 start_state_model_ = std::make_unique<JMGItemModel>(*start_state_handler_->getState(), group_name, this);
227 goal_state_model_ = std::make_unique<JMGItemModel>(*goal_state_handler_->getState(), group_name, this);
228
229 // forward model updates to the PlanningDisplay
230 connect(start_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() {
231 if (!ignore_state_changes_)
232 planning_display_->setQueryStartState(start_state_model_->getRobotState());
233 });
234 connect(goal_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() {
235 if (!ignore_state_changes_)
236 planning_display_->setQueryGoalState(goal_state_model_->getRobotState());
237 });
238
239 // show the goal state by default
240 setActiveModel(goal_state_model_.get());
242}
243
245{
246 if (!start_state_model_ || !start_state_handler_)
247 return;
248 ignore_state_changes_ = true;
249 start_state_model_->updateRobotState(*start_state_handler_->getState());
250 ignore_state_changes_ = false;
251 setActiveModel(start_state_model_.get());
253}
254
256{
257 if (!goal_state_model_ || !goal_state_handler_)
258 return;
259 ignore_state_changes_ = true;
260 goal_state_model_->updateRobotState(*goal_state_handler_->getState());
261 ignore_state_changes_ = false;
262 setActiveModel(goal_state_model_.get());
264}
265
267{
268 if (ui_->joints_view_->model() == model)
269 return;
270 ui_->joints_view_->setModel(model);
271 ui_->joints_view_label_->setText(
272 QString("Group joints of %1 state").arg(model == start_state_model_.get() ? "start" : "goal"));
273}
274
276{
277 if (model == start_state_model_.get())
278 {
279 planning_display_->setQueryStartState(model->getRobotState());
280 }
281 else
282 {
283 planning_display_->setQueryGoalState(model->getRobotState());
284 }
285}
286
287// Find matching key vector in columns of haystack and return the best-aligned column index.
288// Only consider available indexes (> 0). If no match is found, return largest available index.
289// sign becomes -1 if key has changed sign compared to the matching haystack column.
290static Eigen::Index findMatching(const Eigen::VectorXd& key, const Eigen::MatrixXd& haystack,
291 const Eigen::VectorXi& available, double& sign)
292{
293 Eigen::Index result = available.array().maxCoeff();
294 double best_match = 0.0;
295 for (unsigned int i = 0; i < available.rows(); ++i)
296 {
297 int index = available[i];
298 if (index < 0) // index already taken
299 continue;
300 if (index >= haystack.cols())
301 return result;
302 double match = haystack.col(available[i]).transpose() * key;
303 double abs_match = std::abs(match);
304 if (abs_match > 0.5 && abs_match > best_match)
305 {
306 best_match = abs_match;
307 result = index;
308 sign = match > 0 ? 1.0 : -1.0;
309 }
310 }
311 return result;
312}
313
315{
316 JMGItemModel* model = dynamic_cast<JMGItemModel*>(ui_->joints_view_->model());
317 std::size_t i = 0;
318 if (model && model->getJointModelGroup() && model->getJointModelGroup()->isChain())
319 {
321 Eigen::MatrixXd jacobian;
322 if (!model->getRobotState().getJacobian(model->getJointModelGroup(),
323 model->getJointModelGroup()->getLinkModels().back(),
324 Eigen::Vector3d::Zero(), jacobian, false))
325 goto cleanup;
326
327 svd_.compute(jacobian, Eigen::ComputeFullV);
328 Eigen::Index rank = svd_.rank();
329 std::size_t ns_dim = svd_.cols() - rank;
330 Eigen::MatrixXd ns(svd_.cols(), ns_dim);
331 Eigen::VectorXi available(ns_dim);
332 for (std::size_t j = 0; j < ns_dim; ++j)
333 available[j] = j;
334
335 ns_sliders_.reserve(ns_dim);
336 // create/unhide sliders
337 for (; i < ns_dim; ++i)
338 {
339 if (i >= ns_sliders_.size())
340 ns_sliders_.push_back(createNSSlider(i + 1));
341 ns_sliders_[i]->show();
342
343 // Find matching null-space basis vector in previous nullspace_
344 double sign = 1.0;
345 const Eigen::VectorXd& current = svd_.matrixV().col(rank + i);
346 Eigen::Index index = findMatching(current, nullspace_, available, sign);
347 ns.col(index).noalias() = sign * current;
348 available[index] = -1; // mark index as taken
349 }
350 nullspace_ = ns;
351 }
352
353cleanup:
354 if (i == 0)
355 nullspace_.resize(0, 0);
356
357 // show/hide dummy slider
358 ui_->dummy_ns_slider_->setVisible(i == 0);
359
360 // hide remaining sliders
361 for (; i < ns_sliders_.size(); ++i)
362 ns_sliders_[i]->hide();
363}
364
366{
367 JogSlider* slider = new JogSlider(this);
368 slider->setOrientation(Qt::Horizontal);
369 slider->setMaximum(0.1);
370 slider->setToolTip(QString("Nullspace dim #%1").arg(i));
371 ui_->nullspace_layout_->addWidget(slider);
372 connect(slider, SIGNAL(triggered(double)), this, SLOT(jogNullspace(double)));
373 return slider;
374}
375
377{
378 if (value == 0)
379 return;
380
381 std::size_t index = std::find(ns_sliders_.begin(), ns_sliders_.end(), sender()) - ns_sliders_.begin();
382 if (static_cast<int>(index) >= nullspace_.cols())
383 return;
384
385 JMGItemModel* model = dynamic_cast<JMGItemModel*>(ui_->joints_view_->model());
386 if (!model)
387 return;
388
389 Eigen::VectorXd values;
391 values += value * nullspace_.col(index);
394 triggerUpdate(model);
395}
396
397void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
398{
399 // copied from QStyledItemDelegate::paint
400 QStyle* style = option.widget ? option.widget->style() : QApplication::style();
401 QStyleOptionViewItem style_option = option;
402 initStyleOption(&style_option, index);
403
404 if (index.column() == 1)
405 {
406 QVariant joint_type = index.data(JointTypeRole);
407 double value = index.data().toDouble();
408 if (joint_type.isValid())
409 {
410 switch (joint_type.toInt())
411 {
413 style_option.text = option.locale.toString(value * 180 / M_PI, 'f', 0).append("°");
414 break;
416 style_option.text = option.locale.toString(value, 'f', 3).append("m");
417 break;
418 default:
419 break;
420 }
421 }
422
423 QVariant vbounds = index.data(VariableBoundsRole);
424 if (vbounds.isValid())
425 {
426 QPointF bounds = vbounds.toPointF();
427 const double min = bounds.x();
428 const double max = bounds.y();
429
430 QStyleOptionProgressBar opt;
431 opt.rect = option.rect;
432 opt.minimum = 0;
433 opt.maximum = 1000;
434 opt.progress = 1000. * (value - min) / (max - min);
435 opt.text = style_option.text;
436 opt.textAlignment = style_option.displayAlignment;
437 opt.textVisible = true;
438 style->drawControl(QStyle::CE_ProgressBar, &opt, painter);
439 return;
440 }
441 }
442
443 style->drawControl(QStyle::CE_ItemViewItem, &style_option, painter, option.widget);
444}
445
446QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& option,
447 const QModelIndex& index) const
448
449{
450 if (index.column() == 1)
451 {
452 QVariant vbounds = index.data(VariableBoundsRole);
453 if (vbounds.isValid())
454 {
455 QPointF bounds = vbounds.toPointF();
456 double min = bounds.x();
457 double max = bounds.y();
458 bool is_revolute = (index.data(JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE);
459 if (is_revolute)
460 {
461 min *= 180. / M_PI;
462 max *= 180. / M_PI;
463 }
464 auto* editor = new ProgressBarEditor(parent, min, max, is_revolute ? 0 : 3);
465 connect(editor, &ProgressBarEditor::editingFinished, this, &ProgressBarDelegate::commitAndCloseEditor);
466 connect(editor, &ProgressBarEditor::valueChanged, this, [=](double value) {
467 const_cast<QAbstractItemModel*>(index.model())->setData(index, value, Qt::EditRole);
468 });
469 return editor;
470 }
471 }
472 return QStyledItemDelegate::createEditor(parent, option, index);
473}
474
475void ProgressBarDelegate::commitAndCloseEditor()
476{
477 ProgressBarEditor* editor = qobject_cast<ProgressBarEditor*>(sender());
478 commitData(editor);
479 closeEditor(editor);
480}
481
482JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObject(view)
483{
484}
485
486bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event)
487{
488 if (event->type() == QEvent::MouseButtonPress)
489 {
490 QAbstractItemView* view = qobject_cast<QAbstractItemView*>(parent());
491#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
492 QModelIndex index = view->indexAt(static_cast<QMouseEvent*>(event)->position().toPoint());
493#else
494 QModelIndex index = view->indexAt(static_cast<QMouseEvent*>(event)->pos());
495#endif
496 if (index.flags() & Qt::ItemIsEditable) // mouse event on any editable slider?
497 {
498 view->setCurrentIndex(index);
499 view->edit(index);
500 return true; // event handled
501 }
502 }
503 return false;
504}
505
506ProgressBarEditor::ProgressBarEditor(QWidget* parent, double min, double max, int digits)
507 : QWidget(parent), min_(min), max_(max), digits_(digits)
508{
509 // if left mouse button is pressed, grab all future mouse events until button(s) released
510 if (QApplication::mouseButtons() & Qt::LeftButton)
511 grabMouse();
512}
513
514void ProgressBarEditor::paintEvent(QPaintEvent* /*event*/)
515{
516 QPainter painter(this);
517
518 QStyleOptionProgressBar opt;
519 opt.rect = rect();
520 opt.palette = palette();
521 opt.minimum = 0;
522 opt.maximum = 1000;
523 opt.progress = 1000. * (value_ - min_) / (max_ - min_);
524 opt.text = QLocale().toString(value_, 'f', digits_);
525 opt.textAlignment = Qt::AlignRight;
526 opt.textVisible = true;
527 style()->drawControl(QStyle::CE_ProgressBar, &opt, &painter);
528}
529
531{
532 if (event->button() == Qt::LeftButton)
533 mouseMoveEvent(event);
534}
535
536void ProgressBarEditor::mouseMoveEvent(QMouseEvent* event)
537{
538#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
539 double v = std::min(max_, std::max(min_, min_ + event->position().x() * (max_ - min_) / width()));
540#else
541 double v = std::min(max_, std::max(min_, min_ + event->x() * (max_ - min_) / width()));
542#endif
543 if (value_ != v)
544 {
545 value_ = v;
546 valueChanged(v);
547 update();
548 }
549 event->accept();
550}
551
553{
554 if (event->button() == Qt::LeftButton)
555 {
556 event->accept();
558 }
559}
560
561JogSlider::JogSlider(QWidget* parent) : QSlider(parent)
562{
564 setResolution(1000);
565 setMaximum(1.0);
566}
567
569{
570 timer_interval_ = ms;
571}
572
573void JogSlider::setResolution(unsigned int resolution)
574{
575 QSlider::setRange(-resolution, +resolution);
576}
577
579{
580 maximum_ = value;
581}
582
583void JogSlider::timerEvent(QTimerEvent* event)
584{
585 QSlider::timerEvent(event);
586 if (event->timerId() == timer_id_)
587 triggered(value());
588}
589
590void JogSlider::mousePressEvent(QMouseEvent* event)
591{
592 QSlider::mousePressEvent(event);
593 timer_id_ = startTimer(timer_interval_);
594}
595
596void JogSlider::mouseReleaseEvent(QMouseEvent* event)
597{
598 killTimer(timer_id_);
599 QSlider::mouseReleaseEvent(event);
600 setValue(0);
601}
602
603} // namespace moveit_rviz_plugin
bool isChain() const
Check if this group is a linear chain.
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
size_t getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
const JointModel * getMimic() const
Get the joint this one is mimicking.
bool isPassive() const
Check if this joint is passive.
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
JointType getType() const
Get the type of joint.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
int columnCount(const QModelIndex &parent=QModelIndex()) const override
const moveit::core::JointModelGroup * getJointModelGroup() const
JMGItemModel(const moveit::core::RobotState &robot_state, const std::string &group_name, QObject *parent=nullptr)
Qt::ItemFlags flags(const QModelIndex &index) const override
QVariant data(const QModelIndex &index, int role) const override
QVariant headerData(int section, Qt::Orientation orientation, int role) const override
void updateRobotState(const moveit::core::RobotState &state)
call this on any external change of the RobotState
int rowCount(const QModelIndex &parent=QModelIndex()) const override
bool setData(const QModelIndex &index, const QVariant &value, int role) override
void triggered(double value)
void mousePressEvent(QMouseEvent *event) override
void mouseReleaseEvent(QMouseEvent *event) override
bool eventFilter(QObject *target, QEvent *event) override
void changePlanningGroup(const std::string &group_name, const robot_interaction::InteractionHandlerPtr &start_state_handler, const robot_interaction::InteractionHandlerPtr &goal_state_handler)
MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget &)=delete
Delegate to show the joint value as with a progress bar indicator between min and max.
void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override
QWidget * createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override
ProgressBarEditor(QWidget *parent=nullptr, double min=-1.0, double max=0.0, int digits=0)
Create a progressbar-like slider for editing values in range mix..max.