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