C++ (Cpp) testGraspStatic Examples

Example #1

File: graspit_db_planner.cpp Project: CURG/graspit_handop

// test current grasp
bool GraspitDBPlanner::testCurrentGrasp(TestType t, DynamicCode* c){
	// test current grasp in coresponding test type - static/dynamic
	if(t == STATIC)
		return testGraspStatic();
	else{
		if(!c)
			return false;
		return testGraspDynamic(c);
	}
}

Example #2

File: graspit_db_planner.cpp Project: graspit-simulator/graspit

/*! Every grasp in graspList will be tested and stored in testedGraspList
    even if their test scores, epsilon quality and volume quality, are
  not positive, which means they are not stable grasps.
*/
bool GraspitDBPlanner::testGrasps(TestType t,
                                  std::vector<db_planner::Grasp *> graspList,
                                  std::vector<db_planner::Grasp *> *testedGraspList) {
  mTestedGrasps.clear();
  if (testedGraspList) {
    testedGraspList->clear();
  }

  // test each of the grasps in graspList
  for (int i = 0; i < ((int)graspList.size()); i ++)
  {
    GraspitDBGrasp *tempGrasp = new GraspitDBGrasp(*static_cast<GraspitDBGrasp *>(graspList[i]));
    // load this grasp into the world
    static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->setTran(transf::IDENTITY);

    GraspPlanningState *tempState = new GraspPlanningState(tempGrasp->getPreGraspPlanningState());
    tempState->setRefTran(transf::IDENTITY);
    tempGrasp->setPreGraspPlanningState(tempState);

    float elmts[16];
    if (mAligner->Align(tempGrasp->SourceModel(), *mObject, elmts)) {
      tempGrasp->Transform(elmts);
    }

    mHand->getGrasp()->setObject(static_cast<GraspitDBModel *>(mObject)->getGraspableBody());
    // apply it to the hand
    tempGrasp->getPreGraspPlanningState()->execute();
    bool testResult;
    DynamicCode dynCode;
    // test this grasp
    if (t == STATIC) {
      testResult = testGraspStatic();
      dynCode = NO_DYNAMICS;
    } else if (t == DYNAMIC) {
      testResult = testGraspDynamic(&dynCode);
      //DBGA("Result: " << testResult << "; dynamics code: " << dynCode);
    }
    float eq = -1.0, vq;
    // decide whether to record
    if (testResult) {
      computeQuality(eq, vq);
      if (!static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getNumContacts()) {
        dynCode = DYNAMIC_OBJECT_EJECTED;
      } else if (eq <= 0.0) {
        dynCode = DYNAMIC_NO_FC;
      }
    } else { return false; }

    if (eq < 0) { continue; }
    //we now save all grasps in mTestedGrasps
    GraspPlanningState *newGrasp = new GraspPlanningState(mHand);
    newGrasp->setPositionType(SPACE_COMPLETE, false);
    newGrasp->setPostureType(POSE_DOF, false);
    //it is possible the object moved after dynamics, so we need to re-set the reference posture
    newGrasp->setRefTran(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getTran());
    newGrasp->saveCurrentHandState();
    newGrasp->setEpsilonQuality(eq);
    newGrasp->setVolume(vq);
    newGrasp->setIndex(i);
    //this is a hack; just gives us a field to save the dynamics code
    newGrasp->setDistance((double)dynCode);
    mTestedGrasps.push_back(newGrasp);
    //std::cout << "grasp tested, quality: " << eq << std::endl;
    if (!testedGraspList) {
      continue;
    }
    // record this to synthesize the output
    db_planner::Grasp *recordGrasp = static_cast<db_planner::Grasp *>(new GraspitDBGrasp(*tempGrasp));
    tempState = new GraspPlanningState(static_cast<GraspitDBGrasp *>(recordGrasp)->getPreGraspPlanningState());
    tempState->copyFrom(newGrasp);
    static_cast<GraspitDBGrasp *>(recordGrasp)->setPreGraspPlanningState(tempState);
    testedGraspList->push_back(recordGrasp);

    delete tempGrasp;
  }
  if (mTestedGrasps.size() != 0) {
    return true;
  }
  return false;
}