bugfix for chomp fixed base joint bug (#870)

Gracefully handle active root joint, which doesn't have a parent.
moveit · Apr 30, 2018 · 5579c9c · 5579c9c
1 parent 0cd5b08
commit 5579c9c
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp
@@ -201,10 +201,12 @@ void ChompOptimizer::initialize()
     fixed_link_resolution_map[joint_names_[i]] = joint_names_[i];
   }
 
-  for (size_t i = 0; i < joint_model_group_->getFixedJointModels().size(); i++)
+  for (const moveit::core::JointModel* jm : joint_model_group_->getFixedJointModels())
   {
-    const moveit::core::JointModel* model = joint_model_group_->getFixedJointModels()[i];
-    fixed_link_resolution_map[model->getName()] = model->getParentLinkModel()->getParentJointModel()->getName();
+    if (!jm->getParentLinkModel())  // root joint doesn't have a parent
+      continue;
+
+    fixed_link_resolution_map[jm->getName()] = jm->getParentLinkModel()->getParentJointModel()->getName();
   }
 
   // TODO - is this just the joint_roots_?