VertexDataWidget.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package MemoryX::ArmarXObjects::GraphImportExport
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "VertexDataWidget.h"
24 
25 #include <QComboBox>
26 #include <QDoubleSpinBox>
27 #include <QFormLayout>
28 #include <QHBoxLayout>
29 #include <QLineEdit>
30 #include <QList>
31 #include <QPushButton>
32 #include <QRadioButton>
33 #include <QSignalBlocker>
34 
35 #include <SimoxUtility/math/convert/deg_to_rad.h>
36 #include <SimoxUtility/math/convert/mat4f_to_rpy.h>
37 #include <SimoxUtility/math/convert/rad_to_deg.h>
38 #include <SimoxUtility/math/convert/rpy_to_mat4f.h>
39 #include <SimoxUtility/math/pose/pose.h>
40 #include <VirtualRobot/VirtualRobot.h>
41 #include <VirtualRobot/XML/RobotIO.h>
42 
46 
48 
49 #include "ObjectPoseClientWidget.h"
50 #include "RobotVisuWidget.h"
51 
53 {
54 
56  {
57  setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
58 
59  name = new QLineEdit(this);
60  name->setReadOnly(true);
61  locationID = new QLineEdit(this);
62  locationID->setReadOnly(true);
63 
64  agent = new QComboBox(this);
65  frame = new QComboBox(this);
66  removeInstanceButton = new QPushButton("Remove Instance");
67  removeInstanceButton->setToolTip(
68  "Remove the instance id from the agent and make this location relative to an object "
69  "class instead of a specific object instance.");
70 
71  x = new QDoubleSpinBox(this);
72  y = new QDoubleSpinBox(this);
73  z = new QDoubleSpinBox(this);
74 
75  roll = new QDoubleSpinBox(this);
76  pitch = new QDoubleSpinBox(this);
77  yaw = new QDoubleSpinBox(this);
78 
79  angleUnitDeg = new QRadioButton("Degrees");
80  angleUnitRad = new QRadioButton("Radians");
81 
82  _useCurrentRobotPose = new QPushButton("Use Current Robot Pose");
83  _useCurrentRobotPose->setEnabled(false);
84  _useCurrentRobotPose->setToolTip(
85  "Use the robot's current global pose (requires connection to robot state).");
86 
87 
88  for (QDoubleSpinBox* pos : _positionSpinBoxes())
89  {
90  pos->setSuffix(" mm");
91  pos->setMinimum(-1e6);
92  pos->setMaximum(+1e6);
93  pos->setSingleStep(50);
94  }
95  for (QDoubleSpinBox* spinBox : _allSpinBoxes())
96  {
97  spinBox->setValue(0);
98  spinBox->setAlignment(Qt::AlignRight);
99  }
100 
101  QHBoxLayout* angleUnitLayout = new QHBoxLayout();
102  for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
103  {
104  angleUnitLayout->addWidget(angleUnit);
105  }
106 
107  QFormLayout* layout = new QFormLayout();
108  this->setLayout(layout);
109 
110  layout->addRow("Name", name);
111  layout->addRow("Entity ID", locationID);
112  layout->addRow("Agent", agent);
113  layout->addRow("Frame", frame);
114  layout->addRow(removeInstanceButton);
115  layout->addRow("X", x);
116  layout->addRow("Y", y);
117  layout->addRow("Z", z);
118  layout->addRow("Roll", roll);
119  layout->addRow("Pitch", pitch);
120  layout->addRow("Yaw", yaw);
121  layout->addRow("Angle Units", angleUnitLayout);
122  layout->addRow(_useCurrentRobotPose);
123 
124 
125  // Connect
126 
127  for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
128  {
129  connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
130  }
131 
132  connect(agent,
133  QOverload<int>::of(&QComboBox::currentIndexChanged),
134  this,
135  &This::_updateVertexAgent);
136  connect(frame,
137  QOverload<int>::of(&QComboBox::currentIndexChanged),
138  this,
139  &This::_updateVertexFrame);
140  connect(removeInstanceButton, &QPushButton::pressed, this, &This::_removeInstance);
141 
142  for (QDoubleSpinBox* spinBox : _allSpinBoxes())
143  {
144  connect(spinBox,
145  QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
146  this,
147  &This::_updateVertexAttribs);
148  }
149 
150  connect(_useCurrentRobotPose, &QPushButton::pressed, this, &This::_setFromCurrentRobotPose);
151 
152 
153  angleUnitDeg->click();
154  }
155 
156  std::optional<GuiGraph::Vertex>
158  {
159  return _vertex;
160  }
161 
162  void
163  VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
164  {
165  QSignalBlocker blocker(this);
166 
167  _vertex = vertex;
168  _setFromVertex(vertex);
169  setEnabled(true);
170  }
171 
172  void
174  {
175  _vertex = std::nullopt;
176  setEnabled(false);
177  }
178 
179  Eigen::Vector3d
181  {
182  return {x->value(), y->value(), z->value()};
183  }
184 
185  Eigen::Vector3d
187  {
188  Eigen::Vector3d raw = _rpyRaw();
189  if (angleUnitRad->isChecked())
190  {
191  return {}; // return simox::math::rad_to_deg(raw);
192  }
193  else
194  {
195  return raw;
196  }
197  }
198 
199  Eigen::Vector3d
201  {
202  Eigen::Vector3d raw = _rpyRaw();
203  if (angleUnitDeg->isChecked())
204  {
205  return simox::math::deg_to_rad(raw);
206  }
207  else
208  {
209  return raw;
210  }
211  }
212 
215  {
216  return core::Pose_d(simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad())));
217  }
218 
219  void
221  {
222  this->_robotConnection = connection;
223  _useCurrentRobotPose->setEnabled(_robotConnection != nullptr);
224  }
225 
226  void
228  {
229  this->_objectPosesConnection = connection;
230  if (_vertex.has_value())
231  {
232  QSignalBlocker blocker(this);
233 
234  _loadFramedPoseFromVertex(_vertex.value());
235  }
236  }
237 
238  void
239  VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
240  {
241  const VertexData& attrib = vertex.attrib();
242 
243  name->setText(QString::fromStdString(attrib.getName()));
244  locationID->setText(
245  QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
246 
247  _loadFramedPoseFromVertex(vertex);
248 
249  _setPose(core::Pose_d(attrib.getPose().toEigen().cast<qreal>()));
250  }
251 
252  void
253  VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
254  {
255  VertexData& attrib = vertex.attrib();
256 
257  // name is read-only
258  // locationID is read-only
259 
260  FramedPose framed(pose().cast<float>().matrix(),
261  frame->currentText().toStdString(),
262  agent->currentText().toStdString());
263  attrib.setPose(framed);
264  }
265 
266  void
267  VertexDataWidget::_updateAngleUnit()
268  {
269  std::function<double(double)> convertValue;
270  QString suffix = " \u00b0";
271  double min = -360;
272  double max = +360;
273  double step = 5.;
274  int decimals = 2;
275 
276  if (angleUnitRad->isChecked())
277  {
278  convertValue = [](double deg) { return simox::math::deg_to_rad(deg); };
279  suffix = " rad";
280  min = simox::math::deg_to_rad(min);
281  max = simox::math::deg_to_rad(max);
282  step = simox::math::deg_to_rad(step);
283  decimals = 3;
284  }
285  else
286  {
287  ARMARX_CHECK(angleUnitDeg->isChecked());
288  convertValue = [](double rad) { return simox::math::rad_to_deg(rad); };
289  }
290 
291  std::vector<QSignalBlocker> blockers;
292  for (QDoubleSpinBox* angle : _angleSpinBoxes())
293  {
294  blockers.emplace_back(angle);
295 
296  angle->setSuffix(suffix);
297  angle->setMinimum(min);
298  angle->setMaximum(max);
299  angle->setSingleStep(step);
300  angle->setDecimals(decimals);
301  }
302  if (_vertex.has_value())
303  {
304  _setRpyRad(
305  simox::math::mat4f_to_rpy(_vertex->attrib().getPose().toEigen().cast<qreal>()));
306  }
307 
308  // blockers will disable blocking for all spin boxes on destruction.
309  }
310 
311  void
312  VertexDataWidget::_updateVertexAttribs()
313  {
314  if (not signalsBlocked() and _vertex.has_value())
315  {
316  _getToVertex(_vertex.value());
317  _vertex->attrib().changed = true;
318  emit vertexDataChanged();
319  }
320  }
321 
322  void
323  VertexDataWidget::_updateVertexAgent()
324  {
325  if (not signalsBlocked() and _vertex.has_value() and _objectPosesConnection != nullptr)
326  {
327  {
328  QSignalBlocker blocker(this);
329 
330  _updateFrameMenu();
331  frame->setCurrentIndex(0);
332 
333  _framedPoseCoordinateConversion();
334  }
335  _updateVertexAttribs();
336  }
337  }
338 
339  void
340  VertexDataWidget::_updateVertexFrame()
341  {
342  if (not signalsBlocked() and _vertex.has_value() and _objectPosesConnection != nullptr)
343  {
344  {
345  QSignalBlocker blocker(this);
346 
347  _framedPoseCoordinateConversion();
348  }
349  _updateVertexAttribs();
350  }
351  }
352 
353  void
354  VertexDataWidget::_removeInstance()
355  {
356  if (not signalsBlocked() and _vertex.has_value())
357  {
358  // when no agent is set, we don't have to do anything
359  if (agent->currentText().isEmpty())
360  {
361  return;
362  }
363 
364  {
365  QSignalBlocker blocker(this);
366 
367  ObjectID currentAgent(agent->currentText().toStdString());
368  QString newAgent = QString::fromStdString(currentAgent.getClassID().str());
369  if (agent->findText(newAgent) < 0)
370  {
371  agent->insertItem(0, newAgent);
372  }
373  agent->setCurrentText(newAgent);
374  }
375  _updateVertexAttribs();
376  }
377  }
378 
379  std::vector<QDoubleSpinBox*>
380  VertexDataWidget::_positionSpinBoxes()
381  {
382  return {x, y, z};
383  }
384 
385  std::vector<QDoubleSpinBox*>
386  VertexDataWidget::_angleSpinBoxes()
387  {
388  return {roll, pitch, yaw};
389  }
390 
391  std::vector<QDoubleSpinBox*>
392  VertexDataWidget::_allSpinBoxes()
393  {
394  return {x, y, z, roll, pitch, yaw};
395  }
396 
397  void
398  VertexDataWidget::_setPose(const core::Pose_d& pose)
399  {
400  _setXyz(simox::math::position(pose.matrix()));
401  _setRpyRad(simox::math::mat4f_to_rpy(pose.matrix()));
402  }
403 
404  void
405  VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
406  {
407  x->setValue(xyz.x());
408  y->setValue(xyz.y());
409  z->setValue(xyz.z());
410  }
411 
412  Eigen::Vector3d
413  VertexDataWidget::_rpyRaw() const
414  {
415  return {roll->value(), pitch->value(), yaw->value()};
416  }
417 
418  void
419  VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
420  {
421  roll->setValue(rpy(0));
422  pitch->setValue(rpy(1));
423  yaw->setValue(rpy(2));
424  }
425 
426  void
427  VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
428  {
429  if (angleUnitDeg->isChecked())
430  {
431  _setRpyRaw(rpyDeg);
432  }
433  else if (angleUnitRad->isChecked())
434  {
435  _setRpyRaw(simox::math::deg_to_rad(rpyDeg));
436  }
437  }
438 
439  void
440  VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
441  {
442  if (angleUnitDeg->isChecked())
443  {
444  _setRpyRaw(simox::math::rad_to_deg(rpyRad));
445  }
446  else if (angleUnitRad->isChecked())
447  {
448  _setRpyRaw(rpyRad);
449  }
450  }
451 
452  void
453  VertexDataWidget::_setFromCurrentRobotPose()
454  {
455  ARMARX_CHECK_NOT_NULL(_robotConnection);
456  {
457  QSignalBlocker blocker(this);
458  _setPose(_robotConnection->getGlobalPose().cast<qreal>());
459  }
460  _updateVertexAttribs();
461  }
462 
463  void
464  VertexDataWidget::_loadFramedPoseFromVertex(const GuiGraph::Vertex& vertex)
465  {
466  _updateAgentMenu();
467  QString currAgent = QString::fromStdString(vertex.attrib().getPose().agent);
468  if (agent->findText(currAgent) < 0)
469  {
470  agent->insertItem(0, currAgent);
471  }
472  agent->setCurrentText(currAgent);
473 
474  _updateFrameMenu();
475 
476  QString currFrame = QString::fromStdString(vertex.attrib().getPose().frame);
477  if (frame->findText(currFrame) < 0)
478  {
479  frame->insertItem(0, currFrame);
480  }
481  frame->setCurrentText(currFrame);
482  }
483 
484  void
485  VertexDataWidget::_updateAgentMenu()
486  {
487  agent->clear();
488 
489  if (_objectPosesConnection == nullptr)
490  {
491  return;
492  }
493 
494  agent->addItem(QString::fromAscii(""));
495  objpose::ObjectPoseMap map = _objectPosesConnection->getObjectPoseMap();
496  for (const auto& [id, object] : map)
497  {
498  agent->addItem(QString::fromStdString(id.str()));
499  }
500  }
501 
502  void
503  VertexDataWidget::_updateFrameMenu()
504  {
505  frame->clear();
506 
507  if (_objectPosesConnection == nullptr)
508  {
509  return;
510  }
511 
512  const auto nodes = [&]() -> std::optional<std::vector<std::string>>
513  {
514  const auto& currentObj = agent->currentText().toStdString();
515  if (currentObj.empty())
516  {
517  return std::vector<std::string>({armarx::GlobalFrame});
518  }
519  else
520  {
521  const auto& finder = _objectPosesConnection->getObjectClient().getObjectFinder();
522  std::optional<ObjectInfo> info = finder.findObject(currentObj);
523  if (!info.has_value())
524  {
525  return std::nullopt;
526  }
527 
528  std::optional<PackageFileLocation> articulatedModel = info->articulatedSimoxXML();
529  if (!articulatedModel.has_value())
530  {
531  return std::nullopt;
532  }
533 
534  const auto modelPath = armarx::PackagePath(articulatedModel.value().package,
535  articulatedModel.value().relativePath);
536  const auto robot = VirtualRobot::RobotIO::loadRobot(
537  modelPath.toSystemPath(), VirtualRobot::BaseIO::eStructure);
538  if (robot == nullptr)
539  {
540  return std::nullopt;
541  }
542 
543  return robot->getRobotNodeNames();
544  }
545  };
546  std::vector<std::string> frames = nodes().value_or(std::vector<std::string>{"root"});
547 
548  for (const auto& i : frames)
549  {
550  frame->addItem(QString::fromStdString(i));
551  };
552  }
553 
554  void
555  VertexDataWidget::_framedPoseCoordinateConversion()
556  {
557  objpose::ObjectPoseMap objects = _objectPosesConnection->getObjectPoseMap();
558  std::vector<ObjectInfo> objectInfo = _objectPosesConnection->getObjectInfo();
559 
560  // convert current location into the global frame
561  const auto globalPose =
562  core::resolveLocation(objects, objectInfo, _vertex->attrib().getPose());
563  if (!globalPose.pose.has_value())
564  {
565  return;
566  }
567  const core::Pose_d global_T_location = globalPose.pose.value().cast<double>();
568 
569  // convert from the global frame into the new object relative frame
570  const std::string newAgent = agent->currentText().toStdString();
571  const std::string newFrame = frame->currentText().toStdString();
572 
573  const auto object = core::resolveLocation(
574  objects, objectInfo, FramedPose(core::Pose::Identity().matrix(), newFrame, newAgent));
575  if (!object.pose.has_value())
576  {
577  return;
578  }
579  const core::Pose_d global_T_object = object.pose.value().cast<double>();
580  const core::Pose_d object_T_global = global_T_object.inverse();
581 
582  const core::Pose_d object_T_location = object_T_global * global_T_location;
583  _setPose(object_T_location);
584  }
585 
586 } // namespace armarx::navigation::qt_plugins::location_graph_editor
armarx::navigation::qt_plugins::location_graph_editor::objectposes::Connection
Definition: ObjectPoseClientWidget.h:57
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
armarx::navigation::core::resolveLocation
void resolveLocation(Graph::Vertex &vertex, const aron::data::DictPtr &locationData)
Definition: Graph.cpp:244
armarx::navigation::qt_plugins::location_graph_editor::objectposes::Connection::getObjectPoseMap
objpose::ObjectPoseMap getObjectPoseMap()
Definition: ObjectPoseClientWidget.cpp:69
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
armarx::armem::attachment::ObjectID
armem::MemoryID ObjectID
Definition: types.h:79
MemoryID.h
armarx::navigation::qt_plugins::location_graph_editor::robotvisu::Connection
Definition: RobotVisuWidget.h:92
armarx::navigation::core::VertexAttribs::getName
std::string getName() const
Definition: Graph.cpp:41
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::navigation::qt_plugins::location_graph_editor::objectposes::Connection::getObjectInfo
std::vector< ObjectInfo > getObjectInfo()
Definition: ObjectPoseClientWidget.cpp:75
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::rpyDeg
Eigen::Vector3d rpyDeg() const
Definition: VertexDataWidget.cpp:186
armarx::navigation::core::Pose_d
Eigen::Isometry3d Pose_d
Definition: basic_types.h:32
armarx::navigation::qt_plugins::location_graph_editor::objectposes::Connection::getObjectClient
objpose::ObjectPoseClient & getObjectClient()
Definition: ObjectPoseClientWidget.cpp:63
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::setRobotConnection
void setRobotConnection(robotvisu::Connection *connection)
Definition: VertexDataWidget.cpp:220
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::clearVertex
void clearVertex()
Definition: VertexDataWidget.cpp:173
ObjectPoseClientWidget.h
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::setVertex
void setVertex(GuiGraph::Vertex vertex)
Definition: VertexDataWidget.cpp:163
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::vertexDataChanged
void vertexDataChanged()
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::objpose::ObjectPoseClient::getObjectFinder
const ObjectFinder & getObjectFinder() const
Get the internal object finder.
Definition: ObjectPoseClient.cpp:97
RobotVisuWidget.h
armarx::navigation::qt_plugins::location_graph_editor::VertexData
The NodeData struct holds data required for the node.
Definition: GuiGraph.h:43
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
LocationUtils.h
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::xyz
Eigen::Vector3d xyz() const
Definition: VertexDataWidget.cpp:180
core.h
VertexDataWidget.h
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition: Pose.cpp:334
armarx::navigation::qt_plugins::location_graph_editor
Definition: GuiGraph.cpp:29
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::vertex
std::optional< GuiGraph::Vertex > vertex()
Definition: VertexDataWidget.cpp:157
armarx::navigation::core::VertexAttribs::getPose
FramedPose getPose() const
Definition: Graph.cpp:65
aron_conversions.h
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::setObjectPoseConnection
void setObjectPoseConnection(objectposes::Connection *connection)
Definition: VertexDataWidget.cpp:227
armarx::navigation::qt_plugins::location_graph_editor::robotvisu::Connection::getGlobalPose
core::Pose getGlobalPose(bool synchronize=true)
Definition: RobotVisuWidget.cpp:103
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::pose
core::Pose_d pose() const
Definition: VertexDataWidget.cpp:214
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
armarx::ObjectFinder::findObject
std::optional< ObjectInfo > findObject(const std::string &dataset, const std::string &name) const
Definition: ObjectFinder.cpp:65
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::VertexDataWidget
VertexDataWidget()
Definition: VertexDataWidget.cpp:55
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
armarx::PackagePath
Definition: PackagePath.h:55
armarx::navigation::qt_plugins::location_graph_editor::VertexDataWidget::rpyRad
Eigen::Vector3d rpyRad() const
Definition: VertexDataWidget.cpp:200
armarx::navigation::core::VertexAttribs::aron
armarx::navigation::core::arondto::Vertex aron
Definition: Graph.h:45
armarx::objpose::ObjectPoseMap
std::map< ObjectID, ObjectPose > ObjectPoseMap
Definition: forward_declarations.h:21