Connect: Relax validity check of reached end state (#542)

Looks like Rolling's MoveIt uses a more relaxed goal constraint threshold than Humble.
For this reason, all end states reached by Connect solutions of pick+place demo are rejected.
This commit relaxes the max_distance threshold of Connect accordingly.
This commit is contained in:
Robert Haschke 2024-05-26 20:45:23 +02:00 committed by GitHub
parent 747bb29c7a
commit f79c6c537e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194

View File

@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<double>("max_distance", 1e-4, "maximally accepted distance between end and goal sate");
p.declare<double>("max_distance", 1e-2, "maximally accepted distance between end and goal sate");
p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
"constraints to maintain during trajectory");
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",