mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix determination of JMG
This commit is contained in:
parent
b5bc97b3eb
commit
f7a98ef2ff
@ -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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user