Mark Incomplete trajectories as failure #4
Conversation
Codecov Report
@@ Coverage Diff @@
## ros2 #4 +/- ##
==========================================
+ Coverage 41.50% 41.53% +0.03%
==========================================
Files 81 81
Lines 7926 7933 +7
==========================================
+ Hits 3289 3294 +5
- Misses 4637 4639 +2
📣 We’re building smart automated test selection to slash your CI/CD build times. Learn more |
.github/workflows/ci.yaml
Outdated
| @@ -43,7 +43,7 @@ jobs: | |||
| # TODO: Port to ROS2 | |||
| # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" | |||
| # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975 | |||
| UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 | |||
| # UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 | |||
There was a problem hiding this comment.
Why does this need to be commented out?
There was a problem hiding this comment.
CI was failing because it couldn't find clalancette/update-to-gtest-1.11 so I commented it out. Not sure what the right fix for it would be
There was a problem hiding this comment.
setup_upstream_workspace
googletest git https://github.com/ament/googletest clalancette/update-to-gtest-1.11
=== /home/runner/work/moveit_task_constructor/moveit_task_constructor/.work/upstream_ws/src/googletest (git) ===
Could not checkout ref 'clalancette/update-to-gtest-1.11': fatal: invalid reference: clalancette/update-to-gtest-1.11
'vcs import --recursive --force /home/runner/work/moveit_task_constructor/moveit_task_constructor/.work/upstream_ws/src' returned with 1
'setup_upstream_workspace' returned with code '1' after 0 min 2 sec```
There was a problem hiding this comment.
Based on this PR being merged (ament/googletest#21) last week, the branch must have been deleted recently. I think you can just change the branch name to rolling and you should be back in business.
core/src/stages/connect.cpp
Outdated
| // Pushing a nullptr instead of a failed trajectory. | ||
| sub_trajectories.push_back(nullptr); | ||
| const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint()); | ||
| if (distance > 0.05) { |
There was a problem hiding this comment.
Please make this be a parameter of the stage, so it can be tuned at stage creation time?
core/src/stages/connect.cpp
Outdated
| @@ -59,8 +59,12 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : | |||
| p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode"); | |||
| p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(), | |||
| "constraints to maintain during trajectory"); | |||
| p.declare<float>("goal_tolerance", 0.02, | |||
| "L1 distance between goal state and end of trajectory state should be within this tolerance"); | |||
There was a problem hiding this comment.
nit: But is this actually the L2 distance (square root of L2 norm)? L1 would be the Manhattan distance. I think you can just remove the "L1" for what it's worth.
There was a problem hiding this comment.
It mentioned it was the L1 norm here - https://github.com/ros-planning/moveit2/blob/c9a9e407b0a27858c8eea838ccbbd389d7109f9e/moveit_core/robot_state/include/moveit/robot_state/robot_state.h#L1460
But it makes sense not to mention that here
Sometimes, the plan function in the Connect stage returns success with an incomplete trajectory.
This predominantly happens when using pick_ik. Here is a band-aid fix to mark such trajectories as failure while I dig deeper to find the source of this error.
Video evidence of incomplete trajectories now marked as failure
https://github.com/PickNikRobotics/moveit_task_constructor/assets/17363793/e545e5c6-1574-4ac4-bc3e-5eb775e60f63