search

moveit / moveit_task_constructor

A hierarchical multi-stage manipulation planner
https://moveit.github.io/moveit_task_constructor
BSD 3-Clause "New" or "Revised" License
184 stars 152 forks source link

Min distance not reached when setting the same min/max values in the MoveRelative stage #586

Closed captain-yoshi closed 4 months ago

captain-yoshi commented 5 months ago

Consider the case where I want to move up a link by using the setMinMaxDistance API with the same distance for min and max. (Normally for such a case, we would not set the min/max distance and just set the direction accordingly)

    auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift up", solver);

    stage->setMinMaxDistance(0.06, 0.06);

    // Set upward direction
    geometry_msgs::Vector3Stamped vec;
    vec.header.frame_id = from_object;
    vec.vector.x = -1.0;
    stage->setDirection(vec);

In some cases, I get the error below:

image

I think we should add a min_distance_tolerance property. What do you think ?

// min_distance reached?
if (min_distance > 0.0) {
    success = distance >= min_distance - props.get<double>("min_distance_tolerance");
if (!success) {
    char msg[100];
    snprintf(msg, sizeof(msg), "min_distance not reached (%.17g < %.17g)", distance, min_distance);
    solution.setComment(msg);
}