void CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, std::list<GraspPlanningState*> *sampling) { mat3 R(x, y, z); int rotSamples=2; double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0; while (m1 < 2*sz1){ double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0; while (m2 < 2*sz2) { vec3 myTln(tln); myTln = myTln + (m1 - sz1)* y; myTln = myTln + (m2 - sz2)* z; transf tr(R, myTln); for(int rot=0; rot < rotSamples; rot++) { double angle = M_PI * ((double)rot) / rotSamples; transf rotTran(Quaternion(angle, vec3(1,0,0)), vec3(0,0,0)); tr = rotTran * tr; GraspPlanningState* seed = new GraspPlanningState(mHand); seed->setObject(mObject); seed->setRefTran(mObject->getTran(), false); seed->setPostureType(POSE_DOF, false); seed->setPositionType(SPACE_COMPLETE, false); seed->reset(); seed->getPosition()->setTran(tr); sampling->push_back(seed); } m2+=res; } m1 += res; } }
OnLinePlanner * createDefaultPlanner(){ World * w = getWorld(); if(!w->getCurrentHand()) { DBGA("plannerTools::createDefaultPlanner::No current Hand!"); return NULL; } GraspPlanningState *mHandObjectState = new GraspPlanningState(w->getCurrentHand()); mHandObjectState->setPositionType(SPACE_AXIS_ANGLE); mHandObjectState->setObject(w->getGB(0)); mHandObjectState->setRefTran(w->getGB(0)->getTran()); mHandObjectState->reset(); OnLinePlanner * op = new OnLinePlanner(w->getCurrentHand()); op->setContactType(CONTACT_PRESET); op->setEnergyType(ENERGY_CONTACT_QUALITY); op->setMaxSteps(2000); op->setModelState(mHandObjectState); w->setCurrentPlanner(op); op->showSolutionClone(true); op->resetPlanner(); return op; }