motion_planning_frame_joints_widget.h
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 
37 #pragma once
38 
41 #include <Eigen/SVD>
42 #include <QAbstractItemModel>
43 #include <QWidget>
44 #include <QStyledItemDelegate>
45 #include <vector>
46 #include <memory>
47 
48 class QSlider;
49 
50 namespace Ui
51 {
52 class MotionPlanningFrameJointsUI;
53 }
54 namespace robot_interaction
55 {
56 MOVEIT_CLASS_FORWARD(InteractionHandler);
57 }
58 namespace moveit_rviz_plugin
59 {
67 class JMGItemModel : public QAbstractTableModel
68 {
69  Q_OBJECT
72 
73 public:
74  JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr);
75 
76  // QAbstractItemModel interface
77  int rowCount(const QModelIndex& parent = QModelIndex()) const override;
78  int columnCount(const QModelIndex& parent = QModelIndex()) const override;
79  Qt::ItemFlags flags(const QModelIndex& index) const override;
80  QVariant data(const QModelIndex& index, int role) const override;
81  QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
82  bool setData(const QModelIndex& index, const QVariant& value, int role) override;
83 
85  void updateRobotState(const moveit::core::RobotState& state);
86 
88  {
89  return robot_state_;
90  }
92  {
93  return robot_state_;
94  }
96  {
97  return jmg_;
98  }
99 
100 private:
102  const moveit::core::JointModel* getJointModel(const QModelIndex& index) const;
105  const QModelIndex& index) const;
106 };
107 
108 class JointsWidgetEventFilter : public QObject
109 {
110  Q_OBJECT
111  QModelIndex active_; // joint index being operated on
112  int pmin_, pmax_; // pixel min/max values
113  float delta_ = 0.0f; // speed of joint value changes from keyboard interaction
114 
115 public:
116  JointsWidgetEventFilter(QAbstractItemView* view);
117 
118 protected:
119  bool eventFilter(QObject* target, QEvent* event) override;
120 };
121 
123 class MotionPlanningFrameJointsWidget : public QWidget
124 {
125  Q_OBJECT
126 
127 public:
129  MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr);
131 
132  void clearRobotModel();
133  void changePlanningGroup(const std::string& group_name,
134  const robot_interaction::InteractionHandlerPtr& start_state_handler,
135  const robot_interaction::InteractionHandlerPtr& goal_state_handler);
136 
137  bool useRadians() const;
138  void setUseRadians(bool use_radians);
139 
140 Q_SIGNALS:
141  void configChanged();
142 
143 public Q_SLOTS:
144  void queryStartStateChanged();
145  void queryGoalStateChanged();
146  void jogNullspace(double value);
147 
148 protected:
149  void setActiveModel(JMGItemModel* model);
150  void triggerUpdate(JMGItemModel* model);
151  void updateNullspaceSliders();
152  QSlider* createNSSlider(int i);
153 
154 private:
155  Ui::MotionPlanningFrameJointsUI* ui_;
157  robot_interaction::InteractionHandlerPtr start_state_handler_;
158  robot_interaction::InteractionHandlerPtr goal_state_handler_;
159  std::unique_ptr<JMGItemModel> start_state_model_;
160  std::unique_ptr<JMGItemModel> goal_state_model_;
161  // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State()
162  bool ignore_state_changes_ = false;
163 
164  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
165  Eigen::MatrixXd nullspace_;
166  std::vector<QSlider*> ns_sliders_;
167 };
168 
170 class ProgressBarDelegate : public QStyledItemDelegate
171 {
172  Q_OBJECT
173 
174 public:
176  {
177  JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming)
178  VariableBoundsRole, // NOLINT(readability-identifier-naming)
179  JointRangeFractionRole, // NOLINT(readability-identifier-naming)
180  };
182  {
183  DEGREES = 0,
184  RADIANS = 1,
185  };
186 
187  ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent), unit_(DEGREES)
188  {
189  }
190 
192  {
193  unit_ = unit;
194  }
195  void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
196  QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
197  bool isEditing() const;
198  void setEditorData(QWidget* editor, const QModelIndex& index) const override;
199  void setModelData(QWidget* editor, QAbstractItemModel* model, const QModelIndex& index) const override;
200 
202 
203 protected Q_SLOTS:
204  void onEditorDestroyed(QObject* /* editor */) const;
205 
206 private:
207  mutable int editor_open_count_ = 0;
208 };
209 
211 class JogSlider : public QSlider
212 {
213  Q_OBJECT
215  int timer_interval_; // ms
216  double maximum_;
217 
218 public:
219  JogSlider(QWidget* parent = nullptr);
220 
221  int timerInterval() const
222  {
223  return timer_interval_;
224  }
225  void setTimerInterval(int ms);
226  void setResolution(unsigned int resolution);
227  void setMaximum(double value);
228  double value() const
229  {
230  return QSlider::value() * maximum_ / QSlider::maximum();
231  }
232 
233 protected:
234  void timerEvent(QTimerEvent* event) override;
235  void mousePressEvent(QMouseEvent* event) override;
236  void mouseReleaseEvent(QMouseEvent* event) override;
237 
238 private:
239  using QSlider::setMaximum;
240  using QSlider::setMinimum;
241  using QSlider::setRange;
242 
243 Q_SIGNALS:
244  void triggered(double value);
245 };
246 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::JogSlider::JogSlider
JogSlider(QWidget *parent=nullptr)
Definition: motion_planning_frame_joints_widget.cpp:685
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::start_state_handler_
robot_interaction::InteractionHandlerPtr start_state_handler_
Definition: motion_planning_frame_joints_widget.h:157
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_rviz_plugin::JMGItemModel::rowCount
int rowCount(const QModelIndex &parent=QModelIndex()) const override
Definition: motion_planning_frame_joints_widget.cpp:87
moveit_rviz_plugin::ProgressBarDelegate::setModelData
void setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const override
Definition: motion_planning_frame_joints_widget.cpp:584
moveit::core::VariableBounds
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_rviz_plugin::MotionPlanningFrameJointsWidget::createNSSlider
QSlider * createNSSlider(int i)
Definition: motion_planning_frame_joints_widget.cpp:431
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::ns_sliders_
std::vector< QSlider * > ns_sliders_
Definition: motion_planning_frame_joints_widget.h:166
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::updateNullspaceSliders
void updateNullspaceSliders()
Definition: motion_planning_frame_joints_widget.cpp:377
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_rviz_plugin::MotionPlanningFrameJointsWidget::goal_state_model_
std::unique_ptr< JMGItemModel > goal_state_model_
Definition: motion_planning_frame_joints_widget.h:160
moveit_rviz_plugin::ProgressBarDelegate::setUnit
void setUnit(RevoluteUnit unit)
Definition: motion_planning_frame_joints_widget.h:191
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
robot_state.h
moveit_rviz_plugin::ProgressBarDelegate::DEGREES
@ DEGREES
Definition: motion_planning_frame_joints_widget.h:183
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::ignore_state_changes_
bool ignore_state_changes_
Definition: motion_planning_frame_joints_widget.h:162
moveit_rviz_plugin::JMGItemModel::robot_state_
moveit::core::RobotState robot_state_
Definition: motion_planning_frame_joints_widget.h:70
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::queryGoalStateChanged
void queryGoalStateChanged()
Definition: motion_planning_frame_joints_widget.cpp:322
moveit_rviz_plugin::JogSlider::setResolution
void setResolution(unsigned int resolution)
Definition: motion_planning_frame_joints_widget.cpp:697
moveit_rviz_plugin::MotionPlanningDisplay
Definition: motion_planning_display.h:82
moveit_rviz_plugin::JogSlider::timer_interval_
int timer_interval_
Definition: motion_planning_frame_joints_widget.h:215
moveit_rviz_plugin::JogSlider::maximum_
double maximum_
Definition: motion_planning_frame_joints_widget.h:216
moveit_rviz_plugin::JMGItemModel::getRobotState
moveit::core::RobotState & getRobotState()
Definition: motion_planning_frame_joints_widget.h:87
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::setUseRadians
void setUseRadians(bool use_radians)
Definition: motion_planning_frame_joints_widget.cpp:270
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget
~MotionPlanningFrameJointsWidget() override
Definition: motion_planning_frame_joints_widget.cpp:261
moveit_rviz_plugin::JMGItemModel::columnCount
int columnCount(const QModelIndex &parent=QModelIndex()) const override
Definition: motion_planning_frame_joints_widget.cpp:95
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::nullspace_
Eigen::MatrixXd nullspace_
Definition: motion_planning_frame_joints_widget.h:165
moveit_rviz_plugin::MotionPlanningFrameJointsWidget
Definition: motion_planning_frame_joints_widget.h:123
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::triggerUpdate
void triggerUpdate(JMGItemModel *model)
Definition: motion_planning_frame_joints_widget.cpp:342
Ui
Definition: motion_planning_frame.h:69
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::useRadians
bool useRadians() const
Definition: motion_planning_frame_joints_widget.cpp:266
moveit_rviz_plugin::ProgressBarDelegate::onEditorDestroyed
void onEditorDestroyed(QObject *) const
Definition: motion_planning_frame_joints_widget.cpp:555
moveit_rviz_plugin::JMGItemModel
Definition: motion_planning_frame_joints_widget.h:67
moveit_rviz_plugin::JogSlider::mouseReleaseEvent
void mouseReleaseEvent(QMouseEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:720
moveit_rviz_plugin::ProgressBarDelegate::ProgressBarDelegate
ProgressBarDelegate(QWidget *parent=nullptr)
Definition: motion_planning_frame_joints_widget.h:187
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::planning_display_
MotionPlanningDisplay * planning_display_
Definition: motion_planning_frame_joints_widget.h:156
robot_interaction::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(InteractionHandler)
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::MotionPlanningFrameJointsWidget::configChanged
void configChanged()
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
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::MotionPlanningFrameJointsWidget::svd_
Eigen::JacobiSVD< Eigen::MatrixXd > svd_
Definition: motion_planning_frame_joints_widget.h:164
moveit_rviz_plugin::JointsWidgetEventFilter::pmax_
int pmax_
Definition: motion_planning_frame_joints_widget.h:112
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::goal_state_handler_
robot_interaction::InteractionHandlerPtr goal_state_handler_
Definition: motion_planning_frame_joints_widget.h:158
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
moveit_rviz_plugin::ProgressBarDelegate::paint
void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override
Definition: motion_planning_frame_joints_widget.cpp:479
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::ui_
Ui::MotionPlanningFrameJointsUI * ui_
Definition: motion_planning_frame_joints_widget.h:155
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::changePlanningGroup
void changePlanningGroup(const std::string &group_name, const robot_interaction::InteractionHandlerPtr &start_state_handler, const robot_interaction::InteractionHandlerPtr &goal_state_handler)
Definition: motion_planning_frame_joints_widget.cpp:284
moveit_rviz_plugin::ProgressBarDelegate::createEditor
QWidget * createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override
Definition: motion_planning_frame_joints_widget.cpp:520
moveit_rviz_plugin::ProgressBarDelegate::JointRangeFractionRole
@ JointRangeFractionRole
Definition: motion_planning_frame_joints_widget.h:179
moveit_rviz_plugin::JogSlider::timerInterval
int timerInterval() const
Definition: motion_planning_frame_joints_widget.h:221
class_forward.h
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
moveit_rviz_plugin::JogSlider
Slider that jumps back to zero.
Definition: motion_planning_frame_joints_widget.h:211
moveit_rviz_plugin::ProgressBarDelegate::unit_
RevoluteUnit unit_
Definition: motion_planning_frame_joints_widget.h:201
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_rviz_plugin::MotionPlanningFrameJointsWidget::clearRobotModel
void clearRobotModel()
Definition: motion_planning_frame_joints_widget.cpp:275
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::JointsWidgetEventFilter::JointsWidgetEventFilter
JointsWidgetEventFilter(QAbstractItemView *view)
Definition: motion_planning_frame_joints_widget.cpp:602
moveit::core::JointModelGroup
moveit_rviz_plugin::JogSlider::mousePressEvent
void mousePressEvent(QMouseEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:714
robot_interaction
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::jogNullspace
void jogNullspace(double value)
Definition: motion_planning_frame_joints_widget.cpp:442
moveit_rviz_plugin::JogSlider::timerEvent
void timerEvent(QTimerEvent *event) override
Definition: motion_planning_frame_joints_widget.cpp:707
moveit_rviz_plugin::JMGItemModel::getRobotState
const moveit::core::RobotState & getRobotState() const
Definition: motion_planning_frame_joints_widget.h:91
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::queryStartStateChanged
void queryStartStateChanged()
Definition: motion_planning_frame_joints_widget.cpp:311
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_rviz_plugin::ProgressBarDelegate::RevoluteUnit
RevoluteUnit
Definition: motion_planning_frame_joints_widget.h:181
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::setActiveModel
void setActiveModel(JMGItemModel *model)
Definition: motion_planning_frame_joints_widget.cpp:333
moveit_rviz_plugin::ProgressBarDelegate::editor_open_count_
int editor_open_count_
Definition: motion_planning_frame_joints_widget.h:207
moveit_rviz_plugin::ProgressBarDelegate::setEditorData
void setEditorData(QWidget *editor, const QModelIndex &index) const override
Definition: motion_planning_frame_joints_widget.cpp:566
moveit_rviz_plugin::ProgressBarDelegate::VariableBoundsRole
@ VariableBoundsRole
Definition: motion_planning_frame_joints_widget.h:178
moveit_rviz_plugin::ProgressBarDelegate::CustomRole
CustomRole
Definition: motion_planning_frame_joints_widget.h:175
moveit::core::JointModel
moveit_rviz_plugin::JointsWidgetEventFilter::active_
QModelIndex active_
Definition: motion_planning_frame_joints_widget.h:111
moveit_rviz_plugin::MotionPlanningFrameJointsWidget::start_state_model_
std::unique_ptr< JMGItemModel > start_state_model_
Definition: motion_planning_frame_joints_widget.h:159


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