mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
747bb29c7a
commit
f79c6c537e
@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
|
|||||||
|
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
|
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(),
|
p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
|
||||||
"constraints to maintain during trajectory");
|
"constraints to maintain during trajectory");
|
||||||
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
|
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user