By following Getting Started Tutorials(https://moveit.picknik.ai/humble/doc/tutorials/getting_started/getting_started.html), I have created a workspace for MoveIt2. By command "ros2 run moveit_setup_assistant moveit_setup_assistant", I can successfully access to the client. I can successfully generate collision matrix and add planning group. Howeveer, everytime I try to move the robot from the "Robot poses" menu, the client will crash. Running in verbose mode the appearing errors are the following:
[INFO] [1720080734.863226271] [moveit_setup_assistant.Start Screen]: Loading Setup Assistant Complete
[INFO] [1720080742.214238854] [collision_updater]: -------------------------------------------------------------------------------
[INFO] [1720080742.214266885] [collision_updater]: Statistics:
[INFO] [1720080742.214304860] [collision_updater]: Total Links : 7
[INFO] [1720080742.214318505] [collision_updater]: Total possible collisions : 21.000000
[INFO] [1720080742.214321084] [collision_updater]: Always in collision : 0
[INFO] [1720080742.214322915] [collision_updater]: Never in collision : 9
[INFO] [1720080742.214325204] [collision_updater]: Default in collision : 0
[INFO] [1720080742.214327758] [collision_updater]: Adjacent links disabled : 6
[INFO] [1720080742.214330077] [collision_updater]: Sometimes in collision : 6
[INFO] [1720080742.214332751] [collision_updater]: TOTAL DISABLED : 15
[INFO] [1720080742.214599312] [moveit_setup_assistant.Self-Collisions]: Thread complete 21
[INFO] [1720080744.190334533] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080744.190363171] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
Warning: Group 'arm' is empty.
at line 262 in /home/bjae/ws_moveit2/src/srdfdom/src/model.cpp
[INFO] [1720080753.371842138] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080753.371868937] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1720080753.572603630] [moveit_robot_model.robot_model]: Group 'arm' must have at least one valid joint
[WARN] [1720080753.572621321] [moveit_robot_model.robot_model]: Failed to add group 'arm'
[INFO] [1720080759.038625870] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080759.038659728] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1720080768.429340876] [moveit_robot_state.robot_state]: Returning dirty collision body transforms
moveit_setup_assistant: /home/bjae/ws_moveit2/src/moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.h:1397: const Isometry3d& moveit::core::RobotState::getCollisionBodyTransform(const moveit::core::LinkModel*, std::size_t) const: Assertion `checkCollisionTransforms()' failed.
Stack trace (most recent call last):
#31 Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cb9e39, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#30 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b2796c712, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#29 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279cbfd4, in
#28 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279c8d3f, in
#27 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27972e46, in QApplicationPrivate::sendMouseEvent(QWidget*, QMouseEvent*, QWidget*, QWidget*, QWidget**, QPointer<QWidget>&, bool, bool)
#26 Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cb9e39, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#25 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27974363, in QApplication::notify(QObject*, QEvent*)
#24 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b2796c712, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#23 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279af4ed, in QWidget::event(QEvent*)
#22 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27b164a2, in QSlider::mouseMoveEvent(QMouseEvent*)
#21 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27a66229, in QAbstractSlider::setValue(int)
#20 Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27a65aa1, in QAbstractSlider::valueChanged(int)
#19 Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cf17c7, in
#18 Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 247, in qt_static_metacall [0x7f6b1138488a]
244: (void)_t;
245: switch (_id) {
246: case 0: _t->jointValueChanged((*reinterpret_cast< const std::string(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
> 247: case 1: _t->changeJointValue((*reinterpret_cast< int(*)>(_a[1]))); break;
248: case 2: _t->changeJointSlider(); break;
249: default: ;
250: }
#17 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 790, in changeJointValue [0x7f6b11375b9d]
787: joint_value_->setText(QString("%1").arg(double_value, 0, 'f', 4));
788:
789: // Send event to parent widget
> 790: Q_EMIT jointValueChanged(joint_model_->getName(), double_value);
791: }
792:
793: // ******************************************************************************************
#16 Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 318, in jointValueChanged [0x7f6b11384b1a]
315: void moveit_setup::srdf_setup::SliderWidget::jointValueChanged(const std::string & _t1, double _t2)
316: {
317: void *_a[] = { nullptr, const_cast<void*>(reinterpret_cast<const void*>(std::addressof(_t1))), const_cast<void*>(reinterpret_cast<const void*>(std::addressof(_t2))) };
> 318: QMetaObject::activate(this, &staticMetaObject, 0, _a);
319: }
320: QT_WARNING_POP
321: QT_END_MOC_NAMESPACE
#15 Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cf17c7, in
#14 Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 127, in qt_static_metacall [0x7f6b11384665]
124: case 7: _t->loadJointSliders((*reinterpret_cast< const QString(*)>(_a[1]))); break;
125: case 8: _t->showDefaultPose(); break;
126: case 9: _t->playPoses(); break;
> 127: case 10: _t->updateRobotModel((*reinterpret_cast< const std::string(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
128: default: ;
129: }
130: } else if (_c == QMetaObject::RegisterMethodArgumentMetaType) {
#13 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 687, in updateRobotModel [0x7f6b113751d3]
684: robot_state.setVariablePosition(name, value);
685:
686: // Update the robot model/rviz
> 687: updateStateAndCollision(robot_state);
688: }
689:
690: void RobotPosesWidget::updateStateAndCollision(const moveit::core::RobotState& robot_state)
#12 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 696, in updateStateAndCollision [0x7f6b1137522e]
694: // if in collision, show warning
695: // if no collision, hide warning
> 696: collision_warning_->setHidden(!setup_step_.checkSelfCollision(robot_state));
697: }
698:
699: // ******************************************************************************************
#11 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp", line 115, in checkSelfCollision [0x7f6b11340ba4]
112: {
113: // Decide if current state is in collision
114: collision_detection::CollisionResult result;
> 115: srdf_config_->getPlanningScene()->checkSelfCollision(request_, result, robot_state, allowed_collision_matrix_);
116: return !result.contacts.empty();
117: }
#10 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h", line 474, in checkSelfCollision [0x7f6b257fdf60]
471: const collision_detection::AllowedCollisionMatrix& acm) const
472: {
473: // do self-collision checking with the unpadded version of the robot
> 474: getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
475: }
476:
477: /** \brief Get the names of the links that are involved in collisions for the current state */
#9 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 258, in checkSelfCollision [0x7f6b225c1364]
255: void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
256: const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
257: {
> 258: checkSelfCollisionHelper(req, res, state, &acm);
259: }
260:
261: void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
#8 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 266, in checkSelfCollisionHelper [0x7f6b225c13d8]
263: const AllowedCollisionMatrix* acm) const
264: {
265: FCLManager manager;
> 266: allocSelfCollisionBroadPhase(state, manager);
267: CollisionData cd(&req, &res, acm);
268: cd.enableGroup(getRobotModel());
269: manager.manager_->collide(&cd, &collisionCallback);
#7 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 245, in allocSelfCollisionBroadPhase [0x7f6b225c128d]
242: {
243: manager.manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
244:
> 245: constructFCLObjectRobot(state, manager.object_);
246: manager.object_.registerTo(manager.manager_.get());
247: }
#6 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 211, in constructFCLObjectRobot [0x7f6b225c0e2c]
208: for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
209: if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
210: {
> 211: transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
212: robot_geoms_[i]->collision_geometry_data_->shape_index),
213: fcl_tf);
214: auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
#5 Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.h", line 1397, in getCollisionBodyTransform [0x7f6b225c4d8e]
1395: const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1396: {
>1397: assert(checkCollisionTransforms());
1398: return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1399: }
#4 Source "./assert/assert.c", line 101, in __assert_fail [0x7f6b26239e95]
#3 Source "./assert/assert.c", line 92, in __assert_fail_base [0x7f6b2622871a]
#2 Source "./stdlib/abort.c", line 79, in abort [0x7f6b262287f2]
#1 Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f6b26242475]
#0 | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
| Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f6b262969fc]
Aborted (Signal sent by tkill() 321327 1000)
Your environment
ROS Distro: Humble
OS Version: Ubuntu 22.04
Source build
If source, which branch: Humble
Which RMW (Fast DDS or Cyclone DDS)? rmw_cyclonedds_cpp.
Description
By following Getting Started Tutorials(https://moveit.picknik.ai/humble/doc/tutorials/getting_started/getting_started.html), I have created a workspace for MoveIt2. By command "ros2 run moveit_setup_assistant moveit_setup_assistant", I can successfully access to the client. I can successfully generate collision matrix and add planning group. Howeveer, everytime I try to move the robot from the "Robot poses" menu, the client will crash. Running in verbose mode the appearing errors are the following:
Your environment
Steps to reproduce
ros2 launch moveit_setup_assistant moveit_setup_assistantExpected behaviour
Hope to move the robot from the 'Robot poses' menu.
Actual behaviour
Slide the control slider, then exit the window.
Backtrace or Console output
Window crashes.