mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRel: handle unknown links with hard failure
This commit is contained in:
parent
41e64ed7e1
commit
015b1466f4
@ -103,12 +103,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
const auto& accepted = jmg->getJointModelNames();
|
||||
auto& robot_state = scene->getCurrentStateNonConst();
|
||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
||||
for (auto j : joints) {
|
||||
for (const auto& j : joints) {
|
||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||
auto jm = scene->getRobotModel()->getJointModel(index);
|
||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Ignoring joint " << j.first << " that is not part of group " << group);
|
||||
continue;
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
||||
return false;
|
||||
}
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user