mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix MoveRelative::getJointStateFromOffset
Fix for #114: Accessing variable indexes was screwed.
This commit is contained in:
parent
145bec1ed3
commit
3887b32ba1
@ -78,15 +78,16 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||
static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg,
|
||||
moveit::core::RobotState& robot_state) {
|
||||
try {
|
||||
const auto& accepted = jmg->getJointModelNames();
|
||||
const auto& accepted = jmg->getActiveJointModels();
|
||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(direction);
|
||||
for (const auto& j : joints) {
|
||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||
auto jm = robot_state.getRobotModel()->getJointModel(index);
|
||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end())
|
||||
auto jm = robot_state.getRobotModel()->getJointModel(j.first);
|
||||
if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end())
|
||||
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" +
|
||||
jmg->getName() + "'");
|
||||
|
||||
if (jm->getVariableCount() != 1)
|
||||
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
|
||||
auto index = jm->getFirstVariableIndex();
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user