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

Mark Incomplete trajectories as failure #4

Merged
merged 7 commits into from
Jul 7, 2023
Merged

Mark Incomplete trajectories as failure #4

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d39d7d9
Mark Incomplete trajectories as failure
Abishalini Jul 7, 2023
2adb19b
Test CI passes
Abishalini Jul 7, 2023
86f5c30
Fix formatting
Abishalini Jul 7, 2023
80e2d6c
Fix build
Abishalini Jul 7, 2023
17cb051
Fix CI upstream workspace
Abishalini Jul 7, 2023
b573b39
Add goal tolerance as a property
Abishalini Jul 7, 2023
2c9afca
Remove L1
Abishalini Jul 7, 2023
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
2 changes: 1 addition & 1 deletion .github/workflows/ci.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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#rolling
AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release
CCACHE_DIR: ${{ github.workspace }}/.ccache
Expand Down
6 changes: 6 additions & 0 deletions core/include/moveit/task_constructor/stages/connect.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,10 @@ class Connect : public Connecting
setProperty("path_constraints", std::move(path_constraints));
}

// To make sure the L1 distance between the goal state and the last waypoint in the planned trajectory
// are close enough.
void setGoalTolerance(float goal_tolerance) { setProperty("goal_tolerance", std::move(goal_tolerance)); }

void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void compute(const InterfaceState& from, const InterfaceState& to) override;
Expand All @@ -97,6 +101,8 @@ class Connect : public Connecting
moveit::core::JointModelGroupPtr merged_jmg_;
std::list<SubTrajectory> subsolutions_;
std::list<InterfaceState> states_;

std::string name_;
};
} // namespace stages
} // namespace task_constructor
Expand Down
20 changes: 15 additions & 5 deletions core/src/stages/connect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());

name_ = name;
}

void Connect::reset() {
Expand Down Expand Up @@ -149,6 +153,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {

bool success = false;
std::vector<double> positions;

for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
Expand All @@ -161,13 +166,18 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {

robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
sub_trajectories.push_back(trajectory);

// Do not push partial solutions
// Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise.
if (success) {
sub_trajectories.push_back(trajectory);
} else {
// Pushing a nullptr instead of a failed trajectory.
sub_trajectories.push_back(nullptr);
const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint());
const auto& goal_tolerance = props.get<float>("goal_tolerance");
if (distance > goal_tolerance) {
RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_);
RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. "
<< "Marking it as a failure");
success = false;
}
}

if (!success)
Expand Down