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,
|
static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg,
|
||||||
moveit::core::RobotState& robot_state) {
|
moveit::core::RobotState& robot_state) {
|
||||||
try {
|
try {
|
||||||
const auto& accepted = jmg->getJointModelNames();
|
const auto& accepted = jmg->getActiveJointModels();
|
||||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(direction);
|
const auto& joints = boost::any_cast<std::map<std::string, double>>(direction);
|
||||||
for (const auto& j : joints) {
|
for (const auto& j : joints) {
|
||||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
auto jm = robot_state.getRobotModel()->getJointModel(j.first);
|
||||||
auto jm = robot_state.getRobotModel()->getJointModel(index);
|
if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end())
|
||||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end())
|
|
||||||
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" +
|
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" +
|
||||||
jmg->getName() + "'");
|
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.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||||
robot_state.enforceBounds(jm);
|
robot_state.enforceBounds(jm);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user