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:
Robert Haschke 2022-08-26 22:28:54 +02:00
parent 076957d4dc
commit ec366b26ee

View File

@ -237,13 +237,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
angular *= -1.0; 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; linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular; angular = frame_pose.linear() * angular;
target_eigen = ik_pose_world; auto R = Eigen::AngleAxisd(angular_norm, angular);
target_eigen.linear() = auto p = ik_pose_world.translation();
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
target_eigen.translation() += linear;
goto COMPUTE; goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */ } 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) if (dir == Interface::BACKWARD)
linear *= -1.0; linear *= -1.0;
// compute absolute transform for link // compute target transform for ik_frame applying delta transform of twist
linear = frame_pose.linear() * linear; linear = frame_pose.linear() * linear;
target_eigen = ik_pose_world; target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
target_eigen.translation() += linear;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
return false; return false;