mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo: support map<string,double> joint target
There was a corresponding setGoal method already, but no implementation...
This commit is contained in:
parent
ee6c50ad31
commit
310ca3ef15
@ -120,6 +120,20 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
|
||||
} catch (const boost::bad_any_cast&) {
|
||||
}
|
||||
|
||||
try {
|
||||
const std::map<std::string, double>& joint_map = boost::any_cast<std::map<std::string, double>>(goal);
|
||||
const auto& accepted = jmg->getJointModelNames();
|
||||
for (const auto& joint : joint_map) {
|
||||
if (std::find(accepted.begin(), accepted.end(), joint.first) == accepted.end())
|
||||
throw InitStageException(*this,
|
||||
"Joint '" + joint.first + "' is not part of group '" + jmg->getName() + "'");
|
||||
state.setVariablePosition(joint.first, joint.second);
|
||||
}
|
||||
state.update();
|
||||
return true;
|
||||
} catch (const boost::bad_any_cast&) {
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user