41 #include <geometric_shapes/check_isometry.h>
42 #include <rclcpp/logger.hpp>
43 #include <rclcpp/logging.hpp>
44 #include <rcpputils/asserts.hpp>
53 static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;
63 std::optional<int> hasRelativeJointSpaceJump(
const std::vector<moveit::core::RobotStatePtr>& waypoints,
66 if (waypoints.size() < MIN_STEPS_FOR_JUMP_THRESH)
69 "The computed path is too short to detect jumps in joint-space. "
70 "Need at least %zu steps, only got %zu. Try a lower max_step.",
71 MIN_STEPS_FOR_JUMP_THRESH, waypoints.size());
74 std::vector<double> dist_vector;
75 dist_vector.reserve(waypoints.size() - 1);
76 double total_dist = 0.0;
77 for (std::size_t i = 1; i < waypoints.size(); ++i)
79 const double dist_prev_point = waypoints[i]->distance(*waypoints[i - 1], &group);
80 dist_vector.push_back(dist_prev_point);
81 total_dist += dist_prev_point;
85 double thres = jump_threshold_factor * (total_dist /
static_cast<double>(dist_vector.size()));
86 for (std::size_t i = 0; i < dist_vector.size(); ++i)
88 if (dist_vector[i] > thres)
97 std::optional<int> hasAbsoluteJointSpaceJump(
const std::vector<moveit::core::RobotStatePtr>& waypoints,
99 double prismatic_threshold)
101 const bool check_revolute = revolute_threshold > 0.0;
102 const bool check_prismatic = prismatic_threshold > 0.0;
105 for (std::size_t i = 1; i < waypoints.size(); ++i)
107 for (
const auto& joint : joints)
109 const double distance = waypoints[i]->distance(*waypoints[i - 1], joint);
110 switch (joint->getType())
113 if (check_revolute &&
distance > revolute_threshold)
119 if (check_prismatic &&
distance > prismatic_threshold)
126 "Joint %s has not supported type %s. \n"
127 "hasAbsoluteJointSpaceJump only supports prismatic and revolute joints. Skipping joint jump "
128 "check for this joint.",
129 joint->getName().c_str(), joint->getTypeName().c_str());
155 rcpputils::require_true(
revolute > 0.0);
156 rcpputils::require_true(
prismatic > 0.0);
180 const double distance = translation.norm();
185 pose.translation() += global_reference_frame ? translation : pose.linear() * translation;
189 max_step, jump_threshold, validCallback,
195 const Eigen::Isometry3d& target,
bool global_reference_frame,
const MaxEEFStep& max_step,
198 const Eigen::Isometry3d& link_offset)
201 ASSERT_ISOMETRY(target)
202 ASSERT_ISOMETRY(link_offset)
211 Eigen::Isometry3d offset = link_offset.inverse();
214 Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;
216 Eigen::Quaterniond start_quaternion(start_pose.linear());
217 Eigen::Quaterniond target_quaternion(rotated_target.linear());
222 "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
223 "MaxEEFStep.translation components must be non-negative and at least one component must be "
224 "greater than zero");
228 double rotation_distance = start_quaternion.angularDistance(target_quaternion);
229 double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
232 std::size_t translation_steps = 0;
234 translation_steps = floor(translation_distance / max_step.
translation);
236 std::size_t rotation_steps = 0;
238 rotation_steps = floor(rotation_distance / max_step.
rotation);
241 std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
242 if (jump_threshold.
relative_factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
243 steps = MIN_STEPS_FOR_JUMP_THRESH;
246 std::vector<double> consistency_limits;
252 switch (jm->getType())
264 limit = jm->getMaximumExtent();
265 consistency_limits.push_back(limit);
270 path.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
272 double last_valid_percentage = 0.0;
273 for (std::size_t i = 1; i <= steps; ++i)
275 double percentage =
static_cast<double>(i) /
static_cast<double>(steps);
277 Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
278 pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
282 if (start_state->
setFromIK(group, pose * offset, link->
getName(), consistency_limits, 0.0, validCallback,
options,
285 path.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
292 last_valid_percentage = percentage;
297 return CartesianInterpolator::Percentage(last_valid_percentage);
301 RobotState* start_state,
const JointModelGroup* group, std::vector<RobotStatePtr>& path,
const LinkModel* link,
302 const EigenSTL::vector_Isometry3d& waypoints,
bool global_reference_frame,
const MaxEEFStep& max_step,
305 const Eigen::Isometry3d& link_offset)
307 double percentage_solved = 0.0;
308 for (std::size_t i = 0; i < waypoints.size(); ++i)
312 std::vector<RobotStatePtr> waypoint_path;
313 double wp_percentage_solved =
314 computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step,
315 NO_JOINT_SPACE_JUMP_TEST, validCallback,
options, cost_function, link_offset);
316 if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
318 percentage_solved =
static_cast<double>((i + 1)) /
static_cast<double>(waypoints.size());
319 std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
320 if (i > 0 && !waypoint_path.empty())
321 std::advance(start, 1);
322 path.insert(path.end(), start, waypoint_path.end());
326 percentage_solved += wp_percentage_solved /
static_cast<double>(waypoints.size());
327 std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
328 if (i > 0 && !waypoint_path.empty())
329 std::advance(start, 1);
330 path.insert(path.end(), start, waypoint_path.end());
337 return CartesianInterpolator::Percentage(percentage_solved);
341 std::vector<RobotStatePtr>& path,
346 double percentage_solved = 1.0;
347 if (jump_index.has_value())
349 percentage_solved =
static_cast<double>(*jump_index) /
static_cast<double>(path.size());
351 path.resize(jump_index.value());
361 if (waypoints.size() <= 1)
368 return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.
relative_factor);
373 return hasAbsoluteJointSpaceJump(waypoints, group, jump_threshold.
revolute, jump_threshold.
prismatic);
std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, const moveit::core::JointModelGroup *, const std::vector< double > &)> IKCostFn
Signature for a cost function used to evaluate IK solutions.
static Distance computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &path, 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(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular li...
static Percentage checkJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &path, const JumpThreshold &jump_threshold)
Checks if a path has a joint-space jump, and truncates the path at the jump.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
A link from the robot. Contains the constant transform applied to the link and its geometry.
const std::string & getName() const
The name of this link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
bool setFromIK(const JointModelGroup *group, const geometry_msgs::msg::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
Vec3fX< details::Vec3Data< double > > Vector3d
Core components of MoveIt.
std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
std::optional< int > hasJointSpaceJump(const std::vector< moveit::core::RobotStatePtr > &waypoints, const moveit::core::JointModelGroup &group, const moveit::core::JumpThreshold &jump_threshold)
Checks if a joint-space path has a jump larger than the given threshold.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
double distance(const urdf::Pose &transform)
A set of options for the kinematics solver.
Struct with options for defining joint-space jump thresholds.
static JumpThreshold relative(double relative_factor)
Detect joint-space jumps relative to the average joint-space displacement along the path.
static JumpThreshold absolute(double revolute, double prismatic)
Detect joint-space jumps greater than the given absolute thresholds.
static JumpThreshold disabled()
Do not define any jump threshold, i.e., disable joint-space jump detection.
Struct for containing max_step for computeCartesianPath.