-
Notifications
You must be signed in to change notification settings - Fork 2
Configure Reposition Planner
Much TODO here...should explain the three top-level policies for generating candidate base poses, and references to variables in the source code that may be modified.
Unfortunately, most of the configuration of the reposition planner currently requires source code changes. The relevant code is all in RepositionBaseExecutor.cpp.
The reposition planner samples poses for the robot at different radii with respect to the object pose. For some reason, the initial sampled poses are for the "frontmost part of the robot". To modify this, update all instances of the variable @baseOffsetx@ to a desired value. This will generate poses closer to or farther from the object.
This is the threshold distance to the object at which the reposition planner will ask the arm planner whether a feasible arm motion to a grasp pose exists. If the planner returns success, the reposition planner will return only the current pose.
The reposition planner considers a number of base pose candidates, which are checked for collisions against the 2D map and ranked according to graspability, visibility, and the existence of kinematics solutions.
The samples are generated at (1) different radii away from the object, (2) different orientations around the object, and (3) different deviations from "directly facing" the object.
The reposition planner has embedded "preferred" values for each of the sample parameters (radius, theta, and heading deviation). The nominal configuration is expected to have the highest probability of a successful grasp, and the surrounding candidates' probabilities are scaled down using a quadratic function. See RepositionBaseExecutor::computeGraspProbabilities() for preferred values.
The reposition planner also computes a probability for each candidate representing the probability the manipulator will be unable to reach the grasp pose due to collisions. The probability the planner will succeed is considered 0 if the base of the arm is at or below a minimum threshold away from obstacles, 1 if at or above a maximum threshold away from obstacles, and scaled by a quadratic function in between. An identical check is done, using different thresholds, for the base of the robot. These checks are all done against the 2D map.
Looks like this was commented out at the last demo in the nominal case...