MoveRelative: Interpret direction relative to IKFrame

bugfix
This commit is contained in:
v4hn 2021-12-14 14:29:29 +01:00
parent 191ff253fd
commit 84f96ec74c

View File

@ -243,9 +243,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// compute absolute transform for link
linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular;
target_eigen = link_pose;
target_eigen = ik_pose_world;
target_eigen.linear() =
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
target_eigen.translation() += linear;
goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */
@ -269,7 +269,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// compute absolute transform for link
linear = frame_pose.linear() * linear;
target_eigen = link_pose;
target_eigen = ik_pose_world;
target_eigen.translation() += linear;
} catch (const boost::bad_any_cast&) {
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
@ -278,7 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
COMPUTE:
// transform target pose such that ik frame will reach there if link does
target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);