fix determination of JMG

This commit is contained in:
Robert Haschke 2018-06-12 11:00:18 +02:00
parent b5bc97b3eb
commit f7a98ef2ff

View File

@ -60,12 +60,14 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(), std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(),
joint_set.begin(), joint_set.end(), joint_set.begin(), joint_set.end(),
std::inserter(difference, difference.begin())); std::inserter(difference, difference.begin()));
unsigned int acceptable = 0;
for(const std::string& diff_joint : difference){ for(const std::string& diff_joint : difference){
const moveit::core::JointModel* diff_jm = model.getJointModel(diff_joint); const moveit::core::JointModel* diff_jm = model.getJointModel(diff_joint);
if(!diff_jm->isPassive() && !diff_jm->getMimic()) if (diff_jm->isPassive() || diff_jm->getMimic() || diff_jm->getType() == moveit::core::JointModel::FIXED)
continue; ++acceptable;
} }
return jmg; if (difference.size() == acceptable)
return jmg;
} }
} }
@ -156,10 +158,13 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
joint_names.insert(joint_names.end(), joint_names.insert(joint_names.end(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(), sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.end()); sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.end());
group = findJointModelGroup(*model, joint_names); if (joint_names.size()) {
if(!group){ group = findJointModelGroup(*model, joint_names);
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" << boost::algorithm::join(joint_names, ", ") << "}" ); if(!group){
return false; ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" << boost::algorithm::join(joint_names, ", ") << "}" );
return false;
}
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", group->getName().c_str());
} }
} }
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group); exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);