internal Namespace
Definition
Functions Index
| void | setFakeAccelerationBounds (moveit_pro::base::RobotModel &robot_model) |
| std::optional< PlanningError > | getCollisionError (const moveit_pro::base::JointModelGroup &group, const Eigen::VectorXd &joint_positions, moveit_pro::base::planning_scene::PlanningScene &planning_scene, bool pad_environment_collisions, bool pad_self_collisions) |
|
Explain why collisionValidationFunction would refuse a configuration. More... | |
| rrtconnect::ValidationFunction | makeWorkspaceSubdividingValidator (std::function< bool(const Eigen::VectorXd &)> point_validator, rrtconnect::WorkspaceSubdivision subdivision, double configuration_space_step) |
|
Wrap a point validator into an edge-aware rrtconnect::ValidationFunction that subdivides each proposed transition by both spacing bounds. More... | |
| std::optional< PlanningError > | findTrajectoryCollision (const moveit_pro::base::JointModelGroup &group, const trajectory_msgs::msg::JointTrajectory &trajectory, const RRTParams &rrt_params, moveit_pro::base::planning_scene::PlanningScene &planning_scene) |
|
Densely re-validate a blended, time-parameterized trajectory against the actual, unpadded collision geometry. More... | |
| bool | isCheckRejection (const PlanningError &error) |
|
Whether a findTrajectoryCollision error is a rejection of the check itself rather than a detected collision. More... | |
| std::string | formatCheckRejectionSuffix (unsigned int replans_attempted, unsigned int max_replans) |
|
Suffix appended to a check-rejection error, reporting how much of the replanning budget was spent. More... | |
| std::optional< int > | replanSeed (int seed, unsigned int replan_attempt, unsigned int seed_attempts) |
|
Seed for sanity-check replanning attempt replan_attempt, decorrelated from every seed earlier attempts used. More... | |
| moveit_pro::base::collision_detection::CollisionRequest | makeValidationCollisionRequest (const moveit_pro::base::JointModelGroup &group, bool pad_environment_collisions, bool pad_self_collisions) |
| void | wrapContinuousJoints (const moveit_pro::base::JointModelGroup &group, const Eigen::VectorXd &initial_joint_positions, Eigen::VectorXd &lower_limits, Eigen::VectorXd &upper_limits, Eigen::VectorXd &goal_joint_positions) |
|
Give every active joint finite sampling bounds and pick the goal winding the planner heads for. More... | |
| Eigen::VectorXd | computeWorkspaceSweepWeights (const moveit_pro::base::JointModelGroup &group, const moveit_pro::base::RobotState &state, double geometry_scale=1.0, double geometry_padding=0.0) |
|
Compute per-active-variable workspace sweep weights for group: an upper bound, in meters, on how far any point of the robot can move per unit of each variable's motion. More... | |
Variables Index
| constexpr char | kTestRobot[] = "panda" |
| constexpr char | kTestGroup[] = "panda_arm" |
| constexpr double | kFakeAccelerationLimit = 2.0 |
Functions
computeWorkspaceSweepWeights()
|
Compute per-active-variable workspace sweep weights for group: an upper bound, in meters, on how far any point of the robot can move per unit of each variable's motion.
For a revolute variable the weight is the joint's worst-case lever arm: the maximum distance from the joint's frame origin (a point on its axis) to any point of any descendant link's collision geometry — attached bodies included — maximized over the positions of the descendant joints. Chain translations accumulate by the triangle inequality (rotations preserve distances, so each hop contributes at most its fixed origin offset plus the connecting joint's own maximum translation), and each link's geometry is covered by a bounding sphere about its frame origin. Prismatic variables move every distal point by exactly the variable delta, so their weight is 1. Only single-DOF revolute and prismatic active joints are supported — the same restriction the planner's continuous-joint wrapping already enforces; any other type is rejected (see @throws). Joints that mimic an active joint add their own scaled sweep to that joint's weight. Descendant joints outside the group may additionally be planar or floating; only their (finite) translation bounds enter the chain accumulation.
The weighted L1 norm weights · |Δq| therefore bounds the workspace distance any robot point sweeps along a straight configuration-space segment Δq, which is what makes it usable as rrtconnect::WorkspaceSubdivision::sweep_weights for collision-validation subdivision. The bound is conservative: it assumes every chain fully stretched and sums per-joint contributions, so it never under-counts the samples needed for a workspace resolution. Rather than silently weaken that guarantee, inputs it cannot bound — a descendant translation variable with a non-finite limit — are rejected (see @throws).
In internal because the only supported surface is RRTParams::workspace_step; the weight computation itself carries no compatibility guarantee.
- Parameters
-
group Joint model group to compute weights for.
state Robot state supplying the attached bodies that move with the group's links. Joint positions in the state do not affect the result — the bound is maximized over configurations.
geometry_scale Largest link scale the collision checker applies (>= 1). Collision checks run against scaled and padded geometry, whose surface points sweep farther per unit of joint motion than the nominal ones, so the bound must cover the inflated bodies.
geometry_padding Largest link padding the collision checker applies, in meters (>= 0).
- Returns
One weight per active variable of group, in the group's active-variable order.
- Exceptions
-
std::invalid_argument if an active or mimic joint is not single-DOF revolute or prismatic, if the robot model's kinematic tree is malformed (a descendant link without a parent, or an attached body whose shape and pose counts disagree), if a descendant translation-capable joint carries a non-finite or missing position bound (its sweep contribution would be unbounded, so no finite weight is conservative), if a joint reports a variable count its type does not match, or if the inflation arguments are outside their documented ranges.
Definition at line 238 of file workspace_reach.cpp.
findTrajectoryCollision()
|
Densely re-validate a blended, time-parameterized trajectory against the actual, unpadded collision geometry.
Every trajectory sample after the first is collision-checked against the scene's unpadded collision environment, for environment and self collisions alike: the check measures collision tunneling, the trajectory hitting geometry the robot would physically contact, so the padding planning used is deliberately not applied. The motion between consecutive samples is subdivided by the same bounds planning validated with: rrt_params.configuration_space_step, plus the workspace-referred bound when rrt_params.workspace_step is enabled. At typical sampling rates consecutive samples are already closer than those bounds, so the check's effective resolution is the sample spacing and the subdivision only engages at low sampling rates. The first sample is not re-checked — it is the start state the planner already validated. Only collisions are re-checked, not constraints: the blend's constraint drift is bounded by the blend tolerance, predates this check, and is not the safety-critical failure the check targets. Fails closed: a trajectory whose joint names or position counts do not match the group, or parameters the validator rejects, return an error rather than passing unchecked. This is the check RRTParams::sanity_collision_check runs after planning, exposed so measurement code and tests can run it on a trajectory they produced themselves. Symbols in internal are not part of the supported API surface.
- Parameters
-
group Joint model group the trajectory was planned for.
trajectory Trajectory to validate. Its joint_names must equal the group's active joint names and each point must hold one position per active variable of group.
rrt_params Planner parameters supplying the validation bounds. pad_self_collisions is not consulted: the check is unpadded by definition.
planning_scene Planning scene to check against, the same scene planning ran in; only its unpadded collision environment is queried, so no padding or scale the scene applies (link, attached-body, world, or per-object) changes which geometry is checked. Link padding and scale still size the workspace-referred subdivision, exactly as they did for planning, where they can only make the check sample more finely. Its current state's group joints are overwritten.
- Returns
The first collision found, naming the samples it lies between and carrying the contacts in collision_info and the colliding configuration in invalid_joint_positions, or std::nullopt if the trajectory passes the check. An engaged collision_info is the contract that distinguishes a detected collision, which a replan with a different seed can plausibly avoid, from a rejection of the check itself (an unsupported group, invalid resolution parameters, a malformed trajectory, or a rejection no contact re-query could confirm), which reports without it.
Definition at line 1062 of file pro_rrt.cpp.
formatCheckRejectionSuffix()
|
Suffix appended to a check-rejection error, reporting how much of the replanning budget was spent.
- Parameters
-
replans_attempted Replans made before the rejection surfaced; 0 when the first attempt was rejected.
max_replans The configured replanning budget.
- Returns
The sentence to append to the rejection's message, with a leading space. Symbols in internal are not part of the supported API surface.
Definition at line 1194 of file pro_rrt.cpp.
getCollisionError()
|
Explain why collisionValidationFunction would refuse a configuration.
Rejects non-finite positions before collision checking. Otherwise, runs the same checks under the same request as the predicate and reports the first that finds contacts. Callers that only need a verdict should use the predicate: this one requests contact details, so it costs more per call and is meant for the failure path.
Reports self-collisions before environment ones. That is this function's own order, not the predicate's: collisionValidationFunction issues a single checkCollision, which checks the environment first and self-collisions second. So a configuration that is in both kinds of collision is named here by its self-collision, while the predicate would have hit the environment contact first. Both are real contacts of the same configuration under the same request — what the predicate cannot express either way is which, since it answers with a bare bool.
Contacts come back in the returned PlanningError::collision_info, which is what lets a caller draw where the configuration touched rather than only that it did. Deriving them from a separate check of the caller's own devising risks reporting a contact that is not the one that refused the pose; sharing the request is the point of this function.
- Parameters
-
group Joint model group associated with joint_positions.
joint_positions Configuration to explain. Must hold one value per active variable of group, exactly as collisionValidationFunction requires.
planning_scene Planning scene to check against. For finite configurations, its current state's group joints are overwritten with joint_positions.
pad_environment_collisions Whether link padding applies to robot-environment checks.
pad_self_collisions Whether link padding applies to self-collision checks. When false, the returned message omits the padding hint for a self-collision, which would otherwise name a padding value that had no part in the refusal.
- Returns
A PlanningError naming non-finite joints, or naming the colliding bodies and carrying the contacts and joint_positions, or std::nullopt if neither check recorded a contact — which is not the same as valid, since a caller may refuse the configuration for reasons this function does not check. A non-finite configuration is not returned in invalid_joint_positions, because it is unsafe to visualize. The message is a lowercase-opening clause (collision checking… or found a…), itself period-terminated and sometimes followed by a further sentence carrying the padding hint. It is meant to follow a caller-supplied subject, as in … are not valid:; appending it after a sentence-terminating period starts a sentence lowercase, and embedding it mid-sentence breaks on the second sentence.
"No contact recorded" and "no collision" coincide only because the request asks for contacts and leaves room for them: FCL stops storing once the contact budget is spent and reports the collision through a flag instead. That budget is set here and never by the caller, so the two stay equivalent — but the branch is on the contacts, because they are what the message needs.
Definition at line 599 of file pro_rrt.cpp.
isCheckRejection()
|
Whether a findTrajectoryCollision error is a rejection of the check itself rather than a detected collision.
The single definition of the contract the sanity-check retry loop keys on: a detected collision carries the recovered contacts in collision_info, every rejection of the check itself reports without them, and only the former is worth a replan with a different seed. Symbols in internal are not part of the supported API surface.
- Parameters
-
error An error returned by findTrajectoryCollision.
- Returns
True when the error rejects the check itself rather than reporting a detected collision.
Definition at line 1205 of file pro_rrt.cpp.
makeValidationCollisionRequest()
| inline |
Collision request performed by collisionValidationFunction, exposed so measurement code can query contact details under the exact predicate the planner validates with. Inline so the planner's per-sample validation path stays free of an exported call. Symbols in internal are not part of the supported API surface.
Definition at line 594 of file pro_rrt.hpp.
makeWorkspaceSubdividingValidator()
|
Wrap a point validator into an edge-aware rrtconnect::ValidationFunction that subdivides each proposed transition by both spacing bounds.
When the origin waypoint is engaged, the transition q_origin -> q_dest is handed to the workspace-referred rrtconnect::edgeBinarySearch overload, so its samples are never spaced wider than configuration_space_step in the joint space nor than workspace_step in swept distance — structurally, whatever transition length a caller proposes. The origin itself is assumed already validated by the caller, which holds for every tree-growth and segment-validation call site in rrtconnect and rrtstar. When the origin is disengaged only the destination is validated. A transition edgeBinarySearch rejects for an unrepresentable sample count or a size mismatch fails closed. Exposed for tests; symbols in internal are not part of the supported API surface.
- Parameters
-
point_validator Predicate for a single configuration.
subdivision Sweep weights and workspace step. The step must be strictly positive and finite and the weights finite and non-negative; both are validated here. The weight vector's length is only checkable once a transition arrives, so a length that does not match the configurations fails closed at call time rather than throwing.
configuration_space_step Joint-space upper bound on sample spacing, validated here like the workspace step.
- Returns
The wrapped validation function.
- Exceptions
-
std::invalid_argument if either step or a weight entry violates the constraints above.
Definition at line 670 of file pro_rrt.cpp.
replanSeed()
|
Seed for sanity-check replanning attempt replan_attempt, decorrelated from every seed earlier attempts used.
A planning call consumes max(1, seed_attempts) consecutive Halton stride slots starting at its base seed (see the multi-seed loop in planPathToJointGoal), so replans step by whole blocks of that size. Exposed so measurement code can replicate the replanning sequence RRTParams::sanity_collision_check produces. Symbols in internal are not part of the supported API surface.
- Parameters
-
seed The base seed of the original planning call.
replan_attempt 1-based index of the replanning attempt.
seed_attempts The RRTParams::seed_attempts of the planning call; 0 is treated as 1.
- Returns
The decorrelated seed, or std::nullopt when it would leave the int range — the caller should stop replanning rather than fold the value onto a seed another attempt may have used.
Definition at line 1210 of file pro_rrt.cpp.
setFakeAccelerationBounds()
| inline |
Definition at line 31 of file planning_test_utils.hpp.
wrapContinuousJoints()
|
Give every active joint finite sampling bounds and pick the goal winding the planner heads for.
An independently periodic joint (moveit_pro::base::canShiftByWholeTurns) gets initial +/- pi bounds and its goal moved to the nearest equivalent configuration. A continuous joint that drives a mimic keeps its goal as given, because shifting it by a turn moves its followers by multiplier * 2 pi; it may still be position-unbounded, and an infinite bound makes every Halton sample non-finite, so each bound of such a joint that is still infinite is closed half a turn beyond the start and goal. That window is |goal - start| + 2 pi wide rather than the fixed 2 pi of a shiftable joint, so a goal several turns away spreads this joint's samples thinner and can slow the search. A bound a joint_range_constraint already made finite is kept. Exposed for tests; symbols in internal are not part of the supported API surface.
- Precondition
initial_joint_positions and goal_joint_positions are finite, which is asserted.
- Parameters
-
group Group whose active joints are all single-DOF.
initial_joint_positions Start positions, one per active joint.
lower_limits Lower bounds, updated in place.
upper_limits Upper bounds, updated in place.
goal_joint_positions Goal positions, updated in place.
Definition at line 559 of file pro_rrt.cpp.
Variables
kFakeAccelerationLimit
| constexpr |
Definition at line 25 of file planning_test_utils.hpp.
kTestGroup
| constexpr |
Definition at line 20 of file planning_test_utils.hpp.
kTestRobot
| constexpr |
Definition at line 19 of file planning_test_utils.hpp.
The documentation for this namespace was generated from the following files:
Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.