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

Type safety for CartesianInterpolator (backport #1325) #1375

Merged
merged 1 commit into from
Jun 16, 2022
Merged

Type safety for CartesianInterpolator (backport #1325) #1375

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
67 changes: 56 additions & 11 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 @@ -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
Open in desktop
Original file line number Diff line number Diff line change
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