void CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, std::list<GraspPlanningState*> *sampling, int samples, double x, double y, double z) { //redundant, but easier... double a = seed.readPosition()->getParameter("a"); double b = seed.readPosition()->getParameter("b"); //double c = seed.readPosition()->getParameter("c"); //compute angular values //from HandObjectStateImpl: //x = a * cos(beta) * cos(gamma); //y = b * cos(beta) * sin(gamma); //z = c * sin(beta); double beta = asin(z / sqrt(x*x + y*y + z*z)); double gamma = atan2(y/b, x/a); DBGP("x: " << x << "; y: " << y <<"; z: " << z); DBGP("gamma: " << gamma << "; beta: " << beta); //sample roll angle as well for (int m=0; m<samples; m++) { //only sample from 0 to almost PI, as the HH is symmetric double tau = M_PI * ((double)m) / samples; GraspPlanningState *newState = new GraspPlanningState(&seed); newState->getPosition()->getVariable("tau")->setValue(tau); newState->getPosition()->getVariable("gamma")->setValue(gamma); newState->getPosition()->getVariable("beta")->setValue(beta); sampling->push_back(newState); } }
GraspPlanningState *SimAnn::stateNeighbor(GraspPlanningState *s, double T, GraspPlanningState *t) { GraspPlanningState *sn = new GraspPlanningState(s); if (t) { variableNeighbor(sn->getPosition(), T, t->getPosition()); variableNeighbor(sn->getPosture(), T, t->getPosture()); } else { variableNeighbor(sn->getPosition(), T, NULL); variableNeighbor(sn->getPosture(), T, NULL); } return sn; }
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; } }