mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRelative: Allow backwards operation for joint-space delta (#436)
This commit is contained in:
parent
08dc34c5b3
commit
75e4260e2a
@ -79,11 +79,12 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|||||||
planner_->init(robot_model);
|
planner_->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg,
|
static bool getJointStateFromOffset(const boost::any& direction, const Interface::Direction dir,
|
||||||
moveit::core::RobotState& robot_state) {
|
const moveit::core::JointModelGroup* jmg, moveit::core::RobotState& robot_state) {
|
||||||
try {
|
try {
|
||||||
const auto& accepted = jmg->getActiveJointModels();
|
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);
|
||||||
|
double sign = dir == Interface::Direction::FORWARD ? +1.0 : -1.0;
|
||||||
for (const auto& j : joints) {
|
for (const auto& j : joints) {
|
||||||
auto jm = robot_state.getRobotModel()->getJointModel(j.first);
|
auto jm = robot_state.getRobotModel()->getJointModel(j.first);
|
||||||
if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end())
|
if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end())
|
||||||
@ -92,7 +93,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
|
|||||||
if (jm->getVariableCount() != 1)
|
if (jm->getVariableCount() != 1)
|
||||||
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
|
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
|
||||||
auto index = jm->getFirstVariableIndex();
|
auto index = jm->getFirstVariableIndex();
|
||||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second);
|
||||||
robot_state.enforceBounds(jm);
|
robot_state.enforceBounds(jm);
|
||||||
}
|
}
|
||||||
robot_state.update();
|
robot_state.update();
|
||||||
@ -189,7 +190,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
|
|
||||||
if (getJointStateFromOffset(direction, jmg, scene->getCurrentStateNonConst())) {
|
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||||
// plan to joint-space target
|
// plan to joint-space target
|
||||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user