Uses a simple Simulated Annealing loop to plans grasps for the hand and object, and stores them in the database. More...
#include <grasp_planning_task.h>
Public Slots | |
void | plannerComplete () |
Connected to the complete() signal of the planner. | |
void | plannerLoopUpdate () |
Connected to the loopUpdate() signal of the planner. | |
Public Member Functions | |
GraspPlanningTask (graspit_dbase_tasks::DBTaskDispatcher *disp, db_planner::DatabaseManager *mgr, db_planner::TaskRecord rec) | |
Just a stub for now. | |
virtual void | start () |
Loads the hand and the object, initializes and starts a loop planner. | |
virtual | ~GraspPlanningTask () |
Removes the object that has been used from the sim world, but not the hand. | |
Protected Member Functions | |
bool | computePreGrasp (const GraspPlanningState *final_gps, GraspPlanningState **pre_gps) |
Computes a pre-grasp for the given grasp. | |
bool | saveGrasp (const GraspPlanningState *pre_gps, const GraspPlanningState *final_gps) |
Saves a solution grasp to the database. | |
virtual bool | setPreGrasp (const GraspPlanningState *graspState) |
Sets the hand in the pre-grasp pose given the grasp. | |
Protected Attributes | |
Hand * | mHand |
The hand we are planning with. | |
int | mLastSolution |
The index of the last solution that was already saved in the database. | |
GraspableBody * | mObject |
The object we are planning on. | |
EGPlanner * | mPlanner |
The planner that we are using. | |
db_planner::PlanningTaskRecord | mPlanningTask |
The record of the actual planning task. |
Uses a simple Simulated Annealing loop to plans grasps for the hand and object, and stores them in the database.
This task perform grasp planning for simpe robot hands. It uses the Simulated Annealing planner - see the Eigengrasp Planning chapter in the GraspIt! User's Manual for more details. This planner is well suited for relatively simple hands such as parallel grippers where all that is needed is to bring the fingerpads in contact with the object.
It is well suited for planning with the PR2 Gripper.
For more complex hands, see the GuidedGraspPlanner inside this same package.
For now, it does its own cleanup in the sense that it will remove from the world and delete the object that was used for planning. However, it will leave the hand in, as it might be used by subsequent tasks.
On startup, it will load the hand it needs, unless that hand is already the currently selected hand in the world. It will also load the object it needs. It will not delete anything from the world ar startup.
The init and cleanup so that the world is used by subsequent tasks is not well-defined yet, needs more work. Exactly what initialization and cleanup in the GraspIt world such a planner should do is unclear, might change in the future.
Definition at line 74 of file grasp_planning_task.h.
dbase_grasp_planner::GraspPlanningTask::GraspPlanningTask | ( | graspit_dbase_tasks::DBTaskDispatcher * | disp, |
db_planner::DatabaseManager * | mgr, | ||
db_planner::TaskRecord | rec | ||
) |
Just a stub for now.
Definition at line 56 of file grasp_planning_task.cpp.
Removes the object that has been used from the sim world, but not the hand.
Definition at line 65 of file grasp_planning_task.cpp.
bool dbase_grasp_planner::GraspPlanningTask::computePreGrasp | ( | const GraspPlanningState * | final_gps, |
GraspPlanningState ** | pre_gps | ||
) | [protected] |
Computes a pre-grasp for the given grasp.
Definition at line 216 of file grasp_planning_task.cpp.
void dbase_grasp_planner::GraspPlanningTask::plannerComplete | ( | ) | [slot] |
Connected to the complete() signal of the planner.
Definition at line 181 of file grasp_planning_task.cpp.
void dbase_grasp_planner::GraspPlanningTask::plannerLoopUpdate | ( | ) | [slot] |
Connected to the loopUpdate() signal of the planner.
Definition at line 189 of file grasp_planning_task.cpp.
bool dbase_grasp_planner::GraspPlanningTask::saveGrasp | ( | const GraspPlanningState * | pre_gps, |
const GraspPlanningState * | final_gps | ||
) | [protected] |
Saves a solution grasp to the database.
Definition at line 276 of file grasp_planning_task.cpp.
bool dbase_grasp_planner::GraspPlanningTask::setPreGrasp | ( | const GraspPlanningState * | graspState | ) | [protected, virtual] |
Sets the hand in the pre-grasp pose given the grasp.
Reimplemented in dbase_grasp_planner::VeloGraspPlanningTask.
Definition at line 240 of file grasp_planning_task.cpp.
void dbase_grasp_planner::GraspPlanningTask::start | ( | ) | [virtual] |
Loads the hand and the object, initializes and starts a loop planner.
Implements graspit_dbase_tasks::DBTask.
Reimplemented in dbase_grasp_planner::VeloGraspPlanningTask.
Definition at line 77 of file grasp_planning_task.cpp.
Hand* dbase_grasp_planner::GraspPlanningTask::mHand [protected] |
The hand we are planning with.
Definition at line 82 of file grasp_planning_task.h.
int dbase_grasp_planner::GraspPlanningTask::mLastSolution [protected] |
The index of the last solution that was already saved in the database.
Definition at line 84 of file grasp_planning_task.h.
GraspableBody* dbase_grasp_planner::GraspPlanningTask::mObject [protected] |
The object we are planning on.
Definition at line 78 of file grasp_planning_task.h.
EGPlanner* dbase_grasp_planner::GraspPlanningTask::mPlanner [protected] |
The planner that we are using.
Definition at line 80 of file grasp_planning_task.h.
db_planner::PlanningTaskRecord dbase_grasp_planner::GraspPlanningTask::mPlanningTask [protected] |
The record of the actual planning task.
Definition at line 86 of file grasp_planning_task.h.