Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improve Cartesian interpolation #1547

Merged
merged 4 commits into from
Oct 28, 2022
Merged

Improve Cartesian interpolation #1547

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
11ef463
Cleanup CartesianInterpolator
rhaschke Aug 27, 2022
959c795
Generalize computeCartesianPath() to consider a link_offset
rhaschke Aug 27, 2022
16ce470
Augment reference to argument global_reference_frame
rhaschke Sep 16, 2022
37267f5
fix formatting
rhaschke Sep 16, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 37 additions & 19 deletions moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,18 +140,16 @@ class CartesianInterpolator
double meters;
};

/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group.

The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin
The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin
of a robot link (\e link). The direction is assumed to be either in a global reference frame or in the local
reference frame of the link. In the latter case (\e global_reference_frame is false) the \e direction is rotated
accordingly. The link needs to move in a straight line, following the specified direction, for the desired \e
distance. The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in
/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link.

The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link.
This vector is assumed to be specified either in the global reference frame or in the local
reference frame of the link (\e global_reference_frame is false).
The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in
Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which
provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal
call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to
the distance that was computed and for which corresponding states were added to the path. At the end of the
the distance that was achieved and for which corresponding states were added to the path. At the end of the
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
function call, the state of the group corresponds to the last attempted Cartesian pose.

During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a
Expand All @@ -173,43 +171,63 @@ class CartesianInterpolator

Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */
static Distance
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link.

In contrast to the previous function, the translation vector is specified as a (unit) direction vector and
a distance. */
static Distance
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn())
{
return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
jump_threshold, validCallback, options, cost_function);
}

/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group.
/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame.

In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target)
for the origin of a robot link (\e link). The target frame is assumed to be either in a global reference frame or
in the local reference frame of the link. In the latter case (\e global_reference_frame is false) the \e target is
rotated accordingly. All other comments from the previous function apply. */
for a virtual frame attached to the robot \e link with the given \e link_offset.
The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame (\e global_reference_frame is false).
This function returns the percentage (0..1) of the path that was achieved.
All other comments from the previous function apply. */
static Percentage
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
henningkayser marked this conversation as resolved.
Show resolved Hide resolved

/** \brief Compute the sequence of joint values that perform a general Cartesian path.

In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially
reached for the origin of a robot link (\e link). The waypoints are transforms given either in a global reference
frame or in the local reference frame of the link at the immediately preceding waypoint. The link needs to move
in a straight line between two consecutive waypoints. All other comments apply. */
reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global
reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs
to move in a straight line between two consecutive waypoints. All other comments apply. */
static Percentage
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());

/** \brief Tests joint space jumps of a trajectory.

Expand Down
44 changes: 22 additions & 22 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,46 +56,45 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.cart

CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const Eigen::Vector3d& direction, bool global_reference_frame, double distance, const MaxEEFStep& max_step,
const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function)
{
// this is the Cartesian pose we start from, and have to move in the direction indicated
// getGlobalLinkTransform() returns a valid isometry by contract
const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link);
const double distance = translation.norm();
// The target pose is obtained by adding the translation vector to the link's current pose
Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link);

// the direction can be in the local reference frame (in which case we rotate it)
const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction;

// The target pose is built by applying a translation to the start pose for the desired direction and distance
Eigen::Isometry3d target_pose = start_pose; // valid isometry
target_pose.translation() += rotated_direction * distance;
// the translation direction can be specified w.r.t. the local link frame (then rotate into global frame)
pose.translation() += global_reference_frame ? translation : pose.linear() * translation;

// call computeCartesianPath for the computed target pose in the global reference frame
return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, target_pose,
true, max_step, jump_threshold, validCallback,
return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true,
max_step, jump_threshold, validCallback,
options, cost_function);
}

CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function,
const Eigen::Isometry3d& link_offset)
{
// check unsanitized inputs for non-isometry
ASSERT_ISOMETRY(target)
ASSERT_ISOMETRY(link_offset)

const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
// make sure that continuous joints wrap
for (const JointModel* joint : cjnt)
start_state->enforceBounds(joint);

// this is the Cartesian pose we start from, and we move in the direction indicated
// getGlobalLinkTransform() returns a valid isometry by contract
Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry

ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry
// Cartesian pose we start from
Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link) * link_offset;
Eigen::Isometry3d offset = link_offset.inverse();

// the target can be in the local reference frame (in which case we rotate it)
Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; // valid isometry
Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;

Eigen::Quaterniond start_quaternion(start_pose.linear());
Eigen::Quaterniond target_quaternion(rotated_target.linear());
Expand Down Expand Up @@ -160,7 +159,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(

// Explicitly use a single IK attempt only: We want a smooth trajectory.
// Random seeding (of additional attempts) would probably create IK jumps.
if (start_state->setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options,
if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options,
cost_function))
traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
else
Expand All @@ -178,7 +177,8 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function,
const Eigen::Isometry3d& link_offset)
{
double percentage_solved = 0.0;
for (std::size_t i = 0; i < waypoints.size(); ++i)
Expand All @@ -188,7 +188,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
std::vector<RobotStatePtr> waypoint_traj;
double wp_percentage_solved =
computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function);
NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset);
if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
{
percentage_solved = (double)(i + 1) / (double)waypoints.size();
Expand Down