Skip to content

Commit

Permalink
Improve Cartesian interpolation (#1547) (#1670)
Browse files Browse the repository at this point in the history 
* Generalize computeCartesianPath() to consider a link_offset
  which allows performing a circular motion about a non-link origin.
* Augment reference to argument global_reference_frame

Co-authored-by: AndyZe <andyz@utexas.edu>
(cherry picked from commit 749c5ae)

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>
  • Loading branch information
@mergify @rhaschke
mergify[bot] and rhaschke committed Oct 28, 2022
1 parent 32a42eb commit b01111b
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 41 deletions.
56 changes: 37 additions & 19 deletions moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
View file
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
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,
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());

/** \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
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,
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

0 comments on commit b01111b

Please sign in to comment.