/*!
Initializes a new contact on body \a b1. The other contacting body is \a b2.
The contact position \a pos and the contact normal \a norm are expressed
in local body \a b1 coordinates.
*/
Contact::Contact(Body *b1, Body *b2, position pos, vec3 norm)
{
body1 = b1;
body2 = b2;
mate = NULL;
wrench = NULL;
body1Tran = b1->getTran();
body2Tran = b2->getTran();
updateCof();
normal = normalise(norm);
loc = pos;
vec3 tangentX,tangentY;
if (fabs(normal % vec3(1,0,0)) > 1.0 - MACHINE_ZERO) {
tangentX = normalise(normal * vec3(0,0,1));
} else {
tangentX = normalise(normal * vec3(1,0,0));
}
tangentY = normalise(normal * tangentX);
frame = coordinate_transf(loc,tangentX,tangentY);
coneMat = NULL;
prevBetas = NULL;
inheritanceInfo = false;
for (int i=0; i<6; i++) {
dynamicForce[i] = 0;
}
}
void AutoGraspGenerationDlg::moveHandToNextPose()
{
pcl::PointNormal pointNormalInBodyCoord = this->cloud_with_normals.at(currentMeshPointIndex);
double x = pointNormalInBodyCoord.x;
double y = pointNormalInBodyCoord.y;
double z = pointNormalInBodyCoord.z;
double normal_x = pointNormalInBodyCoord.normal_x;
double normal_y = pointNormalInBodyCoord.normal_y;
double normal_z = pointNormalInBodyCoord.normal_z;
//http://stackoverflow.com/questions/21622956/how-to-convert-direction-vector-to-euler-angles
// The nose of that airplane points towards the direction vector
// D=(XD,YD,ZD) .
vec3 D = vec3(normal_x,normal_y,normal_z);
// Towards the roof is the up vector
// U=(XU,YU,ZU) .
vec3 U = D * vec3(0,1,0) * D;
transf worldToObject = mPlanner->getTargetState()->getObject()->getTran();
transf meshPointToApproachTran = translate_transf(vec3(-150*normal_x, -150*normal_y, -150*normal_z));
transf orientHand = rotXYZ(0,-M_PI/2.0,0) * coordinate_transf(position(x,y,z),D,U) ;
mHand->setTran( orientHand * meshPointToApproachTran*worldToObject );
//showSingleNormal(mPlanner->getTargetState()->getObject(), cloud_with_normals,currentHandPositionIndex );
currentMeshPointIndex+=meshPointIncrement;
}
/*!
Sets the transform of the hand to the pose of the candidate grasp \a pg .
If \a render_in is TRUE, the new pose of the hand is rendered.
*/
bool
grasp_tester::putIt(plannedGrasp *pg, bool render_in) {
int result;
transf to =
coordinate_transf(position(pg->get_graspDirection().get_point().x(),
pg->get_graspDirection().get_point().y(),
pg->get_graspDirection().get_point().z()),
(- pg->get_fixedFingerDirection()) *
pg->get_graspDirection().get_dir(),
- pg->get_fixedFingerDirection());
result = my_hand->setTran(to);
if (render_in) {
myViewer->render();
}
return result;
}
/*!
Moves the hand a distance of backStepDist back along the grasp approach
vector, and tries the grasp again. If the grasp is of minumum quality,
it saves it and exits with TRUE. Otherwise it continues stepping the hand
back and evaluating grasps until the hand collides with another object or
the fingers no longer touch the object, or the max number of back steps,
maxItStepNr , is reached. If no grasp has a quality greater than 0, then
FALSE is returned.
*/
bool
grasp_tester::iteration(plannedGrasp &pg)
{
double backStepDist = backStepSize;
transf actTran;
vec3 toVec;
for (int step = 0; step < maxItStepNr; step++) {
/* get actual tran */
actTran = my_hand->getTran();
toVec = actTran.translation() - backStepDist * pg.get_graspDirection().get_dir() / pg.get_graspDirection().get_dir().len();
/* take a step back */
transf to = coordinate_transf(position(toVec.x(), toVec.y(), toVec.z()),
(- pg.get_fixedFingerDirection()) *
pg.get_graspDirection().get_dir(),
- pg.get_fixedFingerDirection());
if (my_hand->setTran(to)) { return false; }
/* preshape again */
preshapeIt(pg.get_preshape(), render);
if (render) {
myViewer->render();
}
if (handCollision()) { return false; }
/* dont move; close fingers directly */
my_hand->autoGrasp(render, 1);
// my_world->findAllContacts();
my_world->updateGrasps();
// if (render)
// myViewer->render();
/* if distance is already too big and there are no more contacts,
the joints have closed to max and more steps won't help */
if (!checkContactToHand(pg.get_graspableBody())) {
if (my_hand->getName().startsWith("Barrett")) {
/* if joints have closed to max */
int allClosed = 0;
for (int i = 1; i < 4; i++) {
if (my_hand->getDOF(i)->getVal() == my_hand->getDOF(i)->getMax()) {
allClosed++;
}
}
if (allClosed >= 2) {
return false;
}
}
else { return false; }
}
/* Evaluate grasp */
pg.set_quality(my_grasp->getQM(whichQM)->evaluate());
if (saveToFile) { saveGrasp(pg.get_quality()); }
/* evaluate, if stable save break */
if (pg.get_quality() > QUALITY_MIN_THRESHOLD) {
return true;
}
}
/* not successful */
return false;
}
