From 504ae254d16e551acf2dc24cbfefaa0d8b437a02 Mon Sep 17 00:00:00 2001 From: Tobias Zaenker Date: Thu, 31 Jul 2025 19:47:41 +0000 Subject: [PATCH] Get kinematics tip frames from joint model group - enables multiple tip frames for groups with multiple end effectors - uses old method (last link in chain) as fallback --- .../src/kinematics_plugin_loader.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index ead7aa044a..653e403b23 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -88,13 +88,17 @@ class KinematicsPluginLoader::KinematicsLoaderImpl std::vector chooseTipFrames(const moveit::core::JointModelGroup* jmg) { std::vector tips; - // get the last link in the chain - RCLCPP_DEBUG(logger_, - "Choosing tip frame of kinematic solver for group %s" - "based on last link in chain", - jmg->getName().c_str()); - tips.push_back(jmg->getLinkModels().back()->getName()); + // try to retrieve the tip frames from the joint model group + if (!jmg->getEndEffectorTips(tips)) + { + // get the last link in the chain as fallback + RCLCPP_DEBUG(logger_, + "Choosing tip frame of kinematic solver for group %s" + "based on last link in chain", + jmg->getName().c_str()); + tips.push_back(jmg->getLinkModels().back()->getName()); + } // Error check if (tips.empty())