From 51c476b22be70fc4f4998230ef16d016d76fb331 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 24 Jan 2024 12:37:55 -0700 Subject: [PATCH 1/3] Fix getLinkModelNamesWithCollisionGeometry to include the base link of the planning group --- moveit_core/robot_model/src/joint_model_group.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index dc9bfb39a5..4b3d1a731e 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -229,6 +229,13 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode { link_model_map_[link_model->getName()] = link_model; link_model_name_vector_.push_back(link_model->getName()); + // if this is the first link of the group with geometry (for example `base_link`) it should included + if(link_model_with_geometry_vector_.empty() && !link_model->getParentLinkModel()->getShapes().empty()) + { + link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel()); + link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName()); + } + // all child links with collision geometry should also be included if (!link_model->getShapes().empty()) { link_model_with_geometry_vector_.push_back(link_model); From 3d1d0b15da3eb56ccae8dd2585cb648d4e66a325 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 24 Jan 2024 13:07:39 -0700 Subject: [PATCH 2/3] pre-commit --- moveit_core/robot_model/src/joint_model_group.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 4b3d1a731e..e323a64aa1 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -230,8 +230,8 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode link_model_map_[link_model->getName()] = link_model; link_model_name_vector_.push_back(link_model->getName()); // if this is the first link of the group with geometry (for example `base_link`) it should included - if(link_model_with_geometry_vector_.empty() && !link_model->getParentLinkModel()->getShapes().empty()) - { + if (link_model_with_geometry_vector_.empty() && !link_model->getParentLinkModel()->getShapes().empty()) + { link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel()); link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName()); } From bb6b86f1a6e2d9431a4f3f393ed36c9b00b59902 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 24 Jan 2024 13:36:28 -0700 Subject: [PATCH 3/3] validate link has parent --- moveit_core/robot_model/src/joint_model_group.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index e323a64aa1..4f758b67bd 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -229,8 +229,9 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode { link_model_map_[link_model->getName()] = link_model; link_model_name_vector_.push_back(link_model->getName()); - // if this is the first link of the group with geometry (for example `base_link`) it should included - if (link_model_with_geometry_vector_.empty() && !link_model->getParentLinkModel()->getShapes().empty()) + // if this is the first link of the group with a valid parent and includes geometry (for example `base_link`) it should included + if (link_model_with_geometry_vector_.empty() && link_model->getParentLinkModel() && + !link_model->getParentLinkModel()->getShapes().empty()) { link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel()); link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName());