Skip to content

Commit

Permalink
Merge PR #3197: Improve computeCartesianPath()
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Sep 9, 2022
2 parents 9225971 + 6363766 commit 1245f15
Show file tree
Hide file tree
Showing 9 changed files with 398 additions and 290 deletions.
6 changes: 5 additions & 1 deletion moveit_core/robot_state/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,14 +20,18 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})

# Unit tests
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)

catkin_add_gtest(test_robot_state test/robot_state_test.cpp)
target_link_libraries(test_robot_state ${MOVEIT_LIB_NAME} moveit_test_utils)

# As an executable, this benchmark is not run as a test by default
add_executable(robot_state_benchmark test/robot_state_benchmark.cpp)
target_link_libraries(robot_state_benchmark ${MOVEIT_LIB_NAME} moveit_test_utils ${GTEST_LIBRARIES})

catkin_add_gtest(test_cartesian_interpolator test/test_cartesian_interpolator.cpp)
add_rostest_gtest(test_cartesian_interpolator
test/test_cartesian_interpolator.test
test/test_cartesian_interpolator.cpp)
target_link_libraries(test_cartesian_interpolator ${MOVEIT_LIB_NAME} moveit_test_utils)

catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp)
Expand Down
54 changes: 35 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 @@ -97,18 +97,16 @@ class CartesianInterpolator
// TODO(mlautman): Eventually, this planner should be moved out of robot_state

public:
/** \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.
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 @@ -128,40 +126,58 @@ class CartesianInterpolator
for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and
the returned path is truncated up to just before the jump.*/
static double
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());

/** \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 double
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());
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
{
return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
jump_threshold, validCallback, options);
}

/** \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.
This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */
static double
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());
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
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 preceeding 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 double
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());
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());

/** \brief Tests joint space jumps of a trajectory.
Expand Down
56 changes: 26 additions & 30 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,48 +54,46 @@ const std::string LOGNAME = "cartesian_interpolator";

double 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 JumpThreshold& jump_threshold,
const Eigen::Vector3d& translation, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options)
{
// 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);

// 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;
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 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 (distance * computeCartesianPath(start_state, group, traj, link, target_pose, true, max_step, jump_threshold,
validCallback, options));
return distance * computeCartesianPath(start_state, group, traj, link, pose, true, max_step, jump_threshold,
validCallback, options);
}

double 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)
const kinematics::KinematicsQueryOptions& options,
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 @@ -161,7 +159,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons

// 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))
traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
else
break;
Expand All @@ -174,13 +172,11 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
return last_valid_percentage;
}

double 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)
double 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, const Eigen::Isometry3d& link_offset)
{
double percentage_solved = 0.0;
for (std::size_t i = 0; i < waypoints.size(); ++i)
Expand All @@ -190,7 +186,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
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);
NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset);
if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
{
percentage_solved = (double)(i + 1) / (double)waypoints.size();
Expand Down
Loading

0 comments on commit 1245f15

Please sign in to comment.