GraspStudio

GraspStudio is a library that closely incorporates with VirtualRobot and which can be used for efficient grasp planning. Based on a robot defined in VirtualRobot, any end effector can be decoupled from the model and considered for grasp planning. The Grasp Center Point (GCP) of an end effector defines the favorite grasping position and an approach direction. A grasp planner interface is provided, which is used for the implementation of a generic grasp planner. This generic grasp planner mainly relies on two exchangeable functionalities: A generator for building grasping hypothesis and a grasp evaluation component.

You can obtain further information by looking at the source code of the examples GraspPlanner and GraspEditor.

The Generic Grasp Planner

In the following setup, it is showed how the generic grasp planner can be setup and used for planning feasible grasps.

// load robot
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(filename);

// get end effector
VirtualRobot::EndEffectorPtr eef = robot->getEndEffector("Left Hand");

// set eef to a preshape configuration which is specified in the eef's XML defintion 
eef->setPreshape("Grasp Preshape");

// load object
VirtualRobot::ManipulationObjectPtr object = ObjectIO::loadManipulationObject("MashedPotatoes.xml");

Setup the Grasp Planner

To setup the grasp planner, firstly a quality measure module (GraspQualityMeasureWrenchSpace) and a grasp hypotheses generator (ApproachMovementSurfaceNormal) are built. These objects are passed to the generic grasp planner together with some parameters.

GraspStudio::GraspQualityMeasureWrenchSpacePtr qualityMeasure(new GraspStudio::GraspQualityMeasureWrenchSpace(object));
qualityMeasure->calculateObjectProperties();
GraspStudio::ApproachMovementSurfaceNormalPtr approach(new GraspStudio::ApproachMovementSurfaceNormal(object,eef));

// get the decoupled eef, which is an independent VirtualRobot::RobotPtr
VirtualRobot::RobotPtr eefCloned = approach->getEEFRobotClone();

// newly created grasps will be stored here 
VirtualRobot::GraspSetPtr grasps(new VirtualRobot::GraspSet("my new grasp set",robot->getType(),eef->getName()));

// setup the planner: The minimum quality that must be reached by a planned grasp is set to 0.2 and the grasps have to be force-closure
GraspStudio::GenericGraspPlannerPtr planner(new GraspStudio::GenericGraspPlanner(grasps, qualityMeasure, approach, 0.2, true));

Plan a Grasp

Grasps can now be planned by calling the plan method of the grasp planner object. Depending on the parameters specified on construction, the grasp planner tries to create the specified number of grasps while respecting the timeout in milliseconds.

// Search one grasp with a timeout of 1000 milliseconds
int graspsPlanned = planner->plan(1,1000);

// retrieve the pose of the grasp
if (graspsPlanned==1)
{
    // mGrasp is the pose of the grasp applied to the global object pose, resulting in the global TCP pose which is related to the grasp
    Eigen::Matrix4f mGrasp = grasps->getGrasp(0)->getTcpPoseGlobal(object->getGlobalPose());
    // now the eef can be set to a position so that it's TCP is at mPose 
    eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector("Left Hand")->getTcp(),mGrasp);
}


// the last computed quality can be retrieved with
float qual = qualityMeasure->getGraspQuality();
bool isFC = qualityMeasure->isGraspForceClosure();


// The contacts information can be retrieved, by closing the end effector
// Additionally the visualization of eefCloned will show the closed fingers
std::vector< VirtualRobot::EndEffector::ContactInfo > contacts = eefCloned->getEndEffector("Left Hand")->closeActors(object);

Store a Set of Grasps

Grasps can be easily managed by ManipulationObjects, as shown in the following example. A new object is created and cloned instances of the visualization and the collision model are passed to the constructor. Then the set of planned grasps is added and the object is stored to an XML file. The XML file will contain the filename of the visualization model and all grasping information.

// create a new object, pass cloned visualizations and collision models to it.
VirtualRobot::ManipulationObjectPtr newObject(new VirtualRobot::ManipulationObject(object->getName(),object->getVisualization()->clone(),object->getCollisionModel()->clone()));

// append the set of planned grasps
newObject->addGraspSet(grasps);

// save to XML file
try
{
    ObjectIO::saveManipulationObject(newObject, "ObjectWithGraspSet.xml");
}
catch (VirtualRobotException &e)
{
    cout << " ERROR while saving object" << endl;
    cout << e.what();
}