Checks if the pre-grasps of the grasps in the database are valid. More...
#include <preGraspCheckTask.h>
Public Member Functions | |
PreGraspCheckTask (TaskDispatcher *disp, db_planner::DatabaseManager *mgr, db_planner::TaskRecord rec) | |
Just a stub for now. | |
virtual void | start () |
Actually does all the work. | |
~PreGraspCheckTask () | |
Removes the object that has been used from the sim world, but not the hand. | |
Static Public Member Functions | |
static bool | preGraspCheck (Hand *hand) |
Checks if the pre-grasp starting from the current hand position is valid. | |
Protected Member Functions | |
bool | checkSetGrasp (db_planner::Grasp *grasp) |
Checks and sets the pre-grasp for a given grasp; returns false if an error is encountered. | |
bool | computePreGrasp (db_planner::Grasp *grasp) |
Computes the pre-grasp fora given grasp; returns true if pre-grasp is not reachable. | |
void | emptyGraspList (std::vector< db_planner::Grasp * > &graspList) |
Empties a list of grasps; should really use some smart memory management here. | |
void | loadHand () |
Loads the needed hand in the simulation world, if it's not already there. | |
void | loadObject () |
Loads the needed object in the simulation world. | |
Protected Attributes | |
Hand * | mHand |
The hand we are planning with. | |
GraspableBody * | mObject |
The object we are planning on. | |
db_planner::PlanningTaskRecord | mPlanningTask |
The record of the actual planning task. |
Checks if the pre-grasps of the grasps in the database are valid.
Given a hand and an object, will load of the grasps from the database. For each grasp, will then check if a pre-grasp (defined as below) is valid and also if the path from pre-grasp to grasp is valid. Finally, will compute the min distance between the hand and the object for the pre-grasp.
A pre-grasp is defined starting from a grasp as follows: first, open the gripper a pre-specified amount. Then, back up along the approach direction for a pre-specified ammount.
Definition at line 49 of file preGraspCheckTask.h.
PreGraspCheckTask::PreGraspCheckTask | ( | TaskDispatcher * | disp, | |
db_planner::DatabaseManager * | mgr, | |||
db_planner::TaskRecord | rec | |||
) |
Just a stub for now.
Definition at line 41 of file preGraspCheckTask.cpp.
PreGraspCheckTask::~PreGraspCheckTask | ( | ) |
Removes the object that has been used from the sim world, but not the hand.
Definition at line 48 of file preGraspCheckTask.cpp.
bool PreGraspCheckTask::checkSetGrasp | ( | db_planner::Grasp * | grasp | ) | [protected] |
Checks and sets the pre-grasp for a given grasp; returns false if an error is encountered.
Reimplemented in TableCheckTask.
Definition at line 149 of file preGraspCheckTask.cpp.
bool PreGraspCheckTask::computePreGrasp | ( | db_planner::Grasp * | grasp | ) | [protected] |
Computes the pre-grasp fora given grasp; returns true if pre-grasp is not reachable.
Definition at line 214 of file preGraspCheckTask.cpp.
void PreGraspCheckTask::emptyGraspList | ( | std::vector< db_planner::Grasp * > & | graspList | ) | [protected] |
Empties a list of grasps; should really use some smart memory management here.
Definition at line 57 of file preGraspCheckTask.cpp.
void PreGraspCheckTask::loadHand | ( | ) | [protected] |
Loads the needed hand in the simulation world, if it's not already there.
Definition at line 65 of file preGraspCheckTask.cpp.
void PreGraspCheckTask::loadObject | ( | ) | [protected] |
Loads the needed object in the simulation world.
Definition at line 96 of file preGraspCheckTask.cpp.
bool PreGraspCheckTask::preGraspCheck | ( | Hand * | hand | ) | [static] |
Checks if the pre-grasp starting from the current hand position is valid.
Definition at line 224 of file preGraspCheckTask.cpp.
void PreGraspCheckTask::start | ( | ) | [virtual] |
Actually does all the work.
Implements Task.
Reimplemented in CompliantGraspCopyTask, and TableCheckTask.
Definition at line 111 of file preGraspCheckTask.cpp.
Hand* PreGraspCheckTask::mHand [protected] |
The hand we are planning with.
Definition at line 52 of file preGraspCheckTask.h.
GraspableBody* PreGraspCheckTask::mObject [protected] |
The object we are planning on.
Definition at line 54 of file preGraspCheckTask.h.
The record of the actual planning task.
Reimplemented in TableCheckTask.
Definition at line 56 of file preGraspCheckTask.h.