Skip to content

Commit

Permalink
Type safety for CartesianInterpolator
Browse files Browse the repository at this point in the history
  • Loading branch information
@wyattrees
wyattrees committed Jun 7, 2022
1 parent 99c73e6 commit 97c5a1e
Show file tree
Hide file tree
Showing 2 changed files with 90 additions and 47 deletions.
67 changes: 56 additions & 11 deletions moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
View file
Expand Up @@ -97,6 +97,51 @@ class CartesianInterpolator
// TODO(mlautman): Eventually, this planner should be moved out of robot_state

public:
struct Percentage
{
// value must be in [0,1]
Percentage(double value) : value(value)
{
if (value < 0.0 || value > 1.0)
throw std::runtime_error("Percentage values must be between 0 and 1, inclusive");
}
operator double()
{
return value;
}
double operator*()
{
return value;
}
Percentage operator*(const Percentage& p)
{
Percentage res(value * p.value);
return res;
}
double value;
};

struct Distance
{
Distance(double meters) : meters(meters)
{
}
operator double()
{
return meters;
}
double operator*()
{
return meters;
}
Distance operator*(const Percentage& p)
{
Distance res(meters * p.value);
return res;
}
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
Expand Down Expand Up @@ -127,7 +172,7 @@ class CartesianInterpolator
For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold
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
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,
Expand All @@ -141,7 +186,7 @@ class CartesianInterpolator
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. */
static double
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,
Expand All @@ -155,7 +200,7 @@ class CartesianInterpolator
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. */
static double
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,
Expand All @@ -180,8 +225,8 @@ class CartesianInterpolator
TODO: move to more appropriate location
*/
static double checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const JumpThreshold& jump_threshold);
static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const JumpThreshold& jump_threshold);

/** \brief Tests for relative joint space jumps of the trajectory \e traj.
Expand All @@ -195,9 +240,9 @@ class CartesianInterpolator
TODO: move to more appropriate location
*/
static double checkRelativeJointSpaceJump(const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj,
double jump_threshold_factor);
static Percentage checkRelativeJointSpaceJump(const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj,
double jump_threshold_factor);

/** \brief Tests for absolute joint space jumps of the trajectory \e traj.
Expand All @@ -213,9 +258,9 @@ class CartesianInterpolator
TODO: move to more appropriate location
*/
static double checkAbsoluteJointSpaceJump(const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj,
double revolute_jump_threshold, double prismatic_jump_threshold);
static Percentage checkAbsoluteJointSpaceJump(const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj,
double revolute_jump_threshold, double prismatic_jump_threshold);
};

} // end of namespace core
Expand Down
70 changes: 34 additions & 36 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
View file
Expand Up @@ -52,13 +52,11 @@ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.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 GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options)
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 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
Expand All @@ -72,16 +70,16 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
target_pose.translation() += rotated_direction * distance;

// 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 CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, target_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)
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)
{
const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
// make sure that continuous joints wrap
Expand Down Expand Up @@ -170,16 +168,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons

last_valid_percentage *= checkJointSpaceJump(group, traj, jump_threshold);

return last_valid_percentage;
return CartesianInterpolator::Percentage(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)
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)
{
double percentage_solved = 0.0;
for (std::size_t i = 0; i < waypoints.size(); ++i)
Expand Down Expand Up @@ -211,11 +207,12 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons

percentage_solved *= checkJointSpaceJump(group, traj, jump_threshold);

return percentage_solved;
return CartesianInterpolator::Percentage(percentage_solved);
}

double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
const JumpThreshold& jump_threshold)
CartesianInterpolator::Percentage CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group,
std::vector<RobotStatePtr>& traj,
const JumpThreshold& jump_threshold)
{
double percentage_solved = 1.0;
if (traj.size() <= 1)
Expand All @@ -227,12 +224,12 @@ double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group,
if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
percentage_solved *= checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);

return percentage_solved;
return CartesianInterpolator::Percentage(percentage_solved);
}

double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group,
std::vector<RobotStatePtr>& traj,
double jump_threshold_factor)
CartesianInterpolator::Percentage CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group,
std::vector<RobotStatePtr>& traj,
double jump_threshold_factor)
{
if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
{
Expand Down Expand Up @@ -264,12 +261,13 @@ double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup*
break;
}

return percentage;
return CartesianInterpolator::Percentage(percentage);
}

double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group,
std::vector<RobotStatePtr>& traj, double revolute_threshold,
double prismatic_threshold)
CartesianInterpolator::Percentage CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group,
std::vector<RobotStatePtr>& traj,
double revolute_threshold,
double prismatic_threshold)
{
bool check_revolute = revolute_threshold > 0.0;
bool check_prismatic = prismatic_threshold > 0.0;
Expand Down Expand Up @@ -316,10 +314,10 @@ double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup*
{
double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
traj.resize(traj_ix + 1);
return percent_valid;
return CartesianInterpolator::Percentage(percent_valid);
}
}
return 1.0;
return CartesianInterpolator::Percentage(1.0);
}

} // end of namespace core
Expand Down

0 comments on commit 97c5a1e

Please sign in to comment.