moveit2
The MoveIt Motion Planning Framework for ROS 2.
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
70  moveit::core::RobotState robot_state_;
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;
104  const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm,
105  const QModelIndex& index) const;
106 };
107 
108 class JointsWidgetEventFilter : public QObject
109 {
110  Q_OBJECT
111 
112 public:
113  JointsWidgetEventFilter(QAbstractItemView* view);
114 
115 protected:
116  bool eventFilter(QObject* target, QEvent* event) override;
117 };
118 
120 class MotionPlanningFrameJointsWidget : public QWidget
121 {
122  Q_OBJECT
123 
124 public:
126  MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr);
128 
129  void clearRobotModel();
130  void changePlanningGroup(const std::string& group_name,
131  const robot_interaction::InteractionHandlerPtr& start_state_handler,
132  const robot_interaction::InteractionHandlerPtr& goal_state_handler);
133 
134 public Q_SLOTS:
135  void queryStartStateChanged();
136  void queryGoalStateChanged();
137  void jogNullspace(double value);
138 
139 protected:
140  void setActiveModel(JMGItemModel* model);
141  void triggerUpdate(JMGItemModel* model);
142  void updateNullspaceSliders();
143  QSlider* createNSSlider(int i);
144 
145 private:
146  Ui::MotionPlanningFrameJointsUI* ui_;
147  MotionPlanningDisplay* planning_display_;
148  robot_interaction::InteractionHandlerPtr start_state_handler_;
149  robot_interaction::InteractionHandlerPtr goal_state_handler_;
150  std::unique_ptr<JMGItemModel> start_state_model_;
151  std::unique_ptr<JMGItemModel> goal_state_model_;
152  // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State()
153  bool ignore_state_changes_ = false;
154 
155  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
156  Eigen::MatrixXd nullspace_;
157  std::vector<QSlider*> ns_sliders_;
158 };
159 
161 class ProgressBarDelegate : public QStyledItemDelegate
162 {
163  Q_OBJECT
164 
165 public:
167  {
168  JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming)
169  VariableBoundsRole // NOLINT(readability-identifier-naming)
170  };
171 
172  ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent)
173  {
174  }
175 
176  void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
177  QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
178 
179 private Q_SLOTS:
180  void commitAndCloseEditor();
181 };
182 
184 class ProgressBarEditor : public QWidget
185 {
186  Q_OBJECT
187  Q_PROPERTY(float value READ getValue WRITE setValue NOTIFY valueChanged USER true)
188 
189 public:
191  ProgressBarEditor(QWidget* parent = nullptr, float min = -1.0, float max = 0.0, int digits = 0);
192 
193  void setValue(float value)
194  {
195  value_ = value;
196  }
197  float getValue() const
198  {
199  return value_;
200  }
201 
202 Q_SIGNALS:
203  void valueChanged(float value);
205 
206 protected:
207  void paintEvent(QPaintEvent* event) override;
208  void mousePressEvent(QMouseEvent* event) override;
209  void mouseMoveEvent(QMouseEvent* event) override;
210  void mouseReleaseEvent(QMouseEvent* event) override;
211 
212 private:
213  float value_;
214  float min_;
215  float max_;
216  int digits_;
217 };
218 
220 class JogSlider : public QSlider
221 {
222  Q_OBJECT
223  int timer_id_;
224  int timer_interval_; // ms
225  double maximum_;
226 
227 public:
228  JogSlider(QWidget* parent = nullptr);
229 
230  int timerInterval() const
231  {
232  return timer_interval_;
233  }
234  void setTimerInterval(int ms);
235  void setResolution(unsigned int resolution);
236  void setMaximum(double value);
237  double value() const
238  {
239  return QSlider::value() * maximum_ / QSlider::maximum();
240  }
241 
242 protected:
243  void timerEvent(QTimerEvent* event) override;
244  void mousePressEvent(QMouseEvent* event) override;
245  void mouseReleaseEvent(QMouseEvent* event) override;
246 
247 private:
248  using QSlider::setMaximum;
249  using QSlider::setMinimum;
250  using QSlider::setRange;
251 
252 Q_SIGNALS:
253  void triggered(double value);
254 };
255 } // namespace moveit_rviz_plugin
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
int columnCount(const QModelIndex &parent=QModelIndex()) const override
JMGItemModel(const moveit::core::RobotState &robot_state, const std::string &group_name, QObject *parent=nullptr)
const moveit::core::RobotState & getRobotState() const
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
const moveit::core::JointModelGroup * getJointModelGroup() const
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
void timerEvent(QTimerEvent *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
Number editor via progress bar dragging.
ProgressBarEditor(QWidget *parent=nullptr, float min=-1.0, float max=0.0, int digits=0)
Create a progressbar-like slider for editing values in range mix..max.
MOVEIT_CLASS_FORWARD(InteractionHandler)