mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRelative: Correctly compute motion transform
The twist motion performs an angular rotation about the given axis _and_ the origin of ik_frame as well as a linear translation. Both transforms are expressed w.r.t. the model frame and thus require left-multiplication to ik_frame's current pose.
This commit is contained in:
parent
076957d4dc
commit
ec366b26ee
@ -237,13 +237,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
angular *= -1.0;
|
||||
}
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying motion transform of twist
|
||||
// linear+angular are expressed w.r.t. model frame and thus we need left-multiplication
|
||||
linear = frame_pose.linear() * linear;
|
||||
angular = frame_pose.linear() * angular;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.linear() =
|
||||
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
|
||||
target_eigen.translation() += linear;
|
||||
auto R = Eigen::AngleAxisd(angular_norm, angular);
|
||||
auto p = ik_pose_world.translation();
|
||||
target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
|
||||
goto COMPUTE;
|
||||
} catch (const boost::bad_any_cast&) { /* continue with Vector */
|
||||
}
|
||||
@ -264,10 +264,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
if (dir == Interface::BACKWARD)
|
||||
linear *= -1.0;
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying delta transform of twist
|
||||
linear = frame_pose.linear() * linear;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.translation() += linear;
|
||||
target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
|
||||
} catch (const boost::bad_any_cast&) {
|
||||
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
|
||||
return false;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user