MPPI: Clip path up to in-place rotation #5648
Conversation
Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Codecov Report❌ Patch coverage is
... and 4 files with indirect coverage changes 🚀 New features to boost your workflow:
|
|
@mini-1235 this seems highly related to your controller server refactor PR. |
| path.poses[start_idx].pose.position.y; | ||
| float initial_translation = sqrtf(ab_x * ab_x + ab_y * ab_y); | ||
|
|
||
| if (initial_translation >= translation_threshold) { |
There was a problem hiding this comment.
This should be some very small epsilon, right? 1e-3 or so? Would we really need this to be a parameter? An in-place rotation should be unambiguously in the same spot
| } | ||
|
|
||
| // Start of potential rotation sequence | ||
| float start_yaw = tf2::getYaw(path.poses[start_idx].pose.orientation); |
There was a problem hiding this comment.
Can't we just compare the heading if they're within 1e-3 of each other if they're different. If so, that's an in-place rotation? It really seems like this should be much simpler.
| * @param rotation_threshold Minimum total rotation to consider significant | ||
| * @return Index after the rotation sequence if found, path size if no rotation detected | ||
| */ | ||
| inline unsigned int findFirstPathRotation( |
There was a problem hiding this comment.
For consideration: Do we really need this to be another function? We could just have this be a possible inversion check on findFirstPathInversion with a bool for check_rotations (probably a better argument name, but you get the idea). If so, we check if there's an inplace rotation to return.
| return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; | ||
| } | ||
|
|
||
| bool PathHandler::isWithinRotationTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) |
There was a problem hiding this comment.
Ditto in the path handler -- could we have a cool for if we check rotations in the existing isWithinInversionTolerances as to not complicate this more? I think the in-place rotation could count as something to prune for. Maybe we re-name "inversion" with something else ("discontinuity"?) or honestly leave as-is but have an arg for whether to include the in-place rotation check
| float angle_distance = angles::shortest_angular_distance( | ||
| tf2::getYaw(robot_pose.pose.orientation), | ||
| tf2::getYaw(last_pose.pose.orientation)); | ||
| return distance <= rotation_xy_tolerance_ && fabs(angle_distance) <= rotation_yaw_tolerance_; |
There was a problem hiding this comment.
Would you want the in-place rotation vs inversion to be any different? If not, then that would also speak to removing duplication so the same tolerance parameters can be used either way :-) Thats what you have the defaults set to, so that's making me think that's a good idea
2 participants
Basic Info
Description of contribution in a few bullet points
With SMACLattice I now have in-place rotations in my path and want to force MPPI to execute them. The concept is similar to how cusps are handled.
enforce_path_rotationandenforce_path_inversionaretrue, the path will be clipped to the first occurenceDescription of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers:
backport-*.