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;
}
