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();
|
const auto& accepted = jmg->getJointModelNames();
|
||||||
auto& robot_state = scene->getCurrentStateNonConst();
|
auto& robot_state = scene->getCurrentStateNonConst();
|
||||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
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);
|
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||||
auto jm = scene->getRobotModel()->getJointModel(index);
|
auto jm = scene->getRobotModel()->getJointModel(index);
|
||||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
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);
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
||||||
continue;
|
return false;
|
||||||
}
|
}
|
||||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||||
robot_state.enforceBounds(jm);
|
robot_state.enforceBounds(jm);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user