mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRelative: Interpret direction relative to IKFrame
bugfix
This commit is contained in:
parent
191ff253fd
commit
84f96ec74c
@ -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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user