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;
}
// move the hand by v, no rotation is applied
void GraspitDBPlanner::moveBy(vec3 v)
{
transf newTran;
// calculate the new position of the hand after the transform of v
newTran = translate_transf(v * mHand->getApproachTran()) * mHand->getTran();
mHand->setTran(newTran);
}
/*! Given an array of desired joint values, this computed an infinitesimal motion
of each link as motion *from the current values* towards the desired values is
started. Used mainly to see if any contacts prevent this motion.
*/
void
KinematicChain::infinitesimalMotion(const double *jointVals, std::vector<transf> &newLinkTranVec) const
{
//start with the link jacobian in local link coordinates
//but keep just the actuated part
Matrix J(actuatedJacobian(linkJacobian(false)));
//joint values matrix
Matrix theta(numJoints, 1);
//a very small motion
//either 0.1 radians or 0.1 mm, should be small enough
double inf = 0.1;
//a very small threshold
double eps = 1.0e-6;
for(int j=0; j<numJoints; j++) {
int sign;
if ( jointVals[firstJointNum + j] + eps < jointVec[j]->getVal() ) {
sign = -1;
} else if ( jointVals[firstJointNum + j] > jointVec[j]->getVal() + eps ) {
sign = 1;
} else {
sign = 0;
}
theta.elem(j,0) = sign * inf;
}
//compute infinitesimal motion
Matrix dm(6*numLinks, 1);
matrixMultiply(J, theta, dm);
//and convert it to transforms
for (int l=0; l<numLinks; l++) {
transf tr = rotXYZ( dm.elem(6*l+3, 0), dm.elem(6*l+4, 0), dm.elem(6*l+5, 0) ) *
translate_transf( vec3( dm.elem(6*l+0, 0), dm.elem(6*l+1, 0), dm.elem(6*l+2, 0) ) );
newLinkTranVec[l] = tr;
}
}
/*!
Sets a new d value for prismatic joints and recomputes the current value
of the transform.
*/
void DHTransform::setD(double q)
{
d = q;
dtrans[2] = d;
tr2 = translate_transf(dtrans);
tran = tr4TimesTr3 * tr2 * tr1;
}
/*! Initializes the DHTransform with the 4 DH parameters. */
DHTransform::DHTransform(double thval,double dval,double aval,double alval) :
theta(thval),d(dval),a(aval),alpha(alval)
{
transf tr3,tr4;
dtrans[0] = 0.0;
dtrans[1] = 0.0;
dtrans[2] = d;
atrans[0] = a;
atrans[1] = 0.0;
atrans[2] = 0.0;
tr1 = rotate_transf(theta,vec3(0,0,1));
tr2 = translate_transf(dtrans);
tr3 = translate_transf(atrans);
tr4 = rotate_transf(alpha,vec3(1,0,0));
tr4TimesTr3 = tr4 * tr3;
tran = tr4TimesTr3 * tr2 * tr1;
}
/*!
Sets a new theta value for the linear offset along x and recomputes the current value
of the transform.
*/
void DHTransform::setA(double q)
{
a = q;
atrans[0] = a;
transf tr3,tr4;
tr3 = translate_transf(atrans);
tr4 = rotate_transf(alpha,vec3(1,0,0));
tr4TimesTr3 = tr4 * tr3;
tran = tr4TimesTr3 * tr2 * tr1;
}
transf PositionStateAA::getCoreTran() const
{
double tx = readVariable("Tx");
double ty = readVariable("Ty");
double tz = readVariable("Tz");
double theta = readVariable("theta");
double phi = readVariable("phi");
double alpha = readVariable("alpha");
transf coreTran = rotate_transf(alpha, vec3( sin(theta)*cos(phi) , sin(theta)*sin(phi) , cos(theta) )) *
translate_transf(vec3(tx,ty,tz));
//transform now returned relative to hand approach transform
return mHand->getApproachTran().inverse() * coreTran;
}
/*!
If the hand is not yet loaded, load the hand and make sure that there is a set of virtual contacts for the hand.
*/
void GraspPlanningTask::getHand(){
World *world = graspItGUI->getIVmgr()->getWorld();
mHand = NULL;
//check if the currently selected hand is the same as the one we need
//if not, load the hand we need
if (world->getCurrentHand() &&
GraspitDBGrasp::getHandDBName(world->getCurrentHand()) == QString(mRecord.handName.c_str())) {
DBGA("Grasp Planning Task: using currently loaded hand");
mHand = world->getCurrentHand();
} else {
//Get the hand from the database name stored in the task.
//mHand = GraspitDBGrasp::loadHandFromDBName(QString(mRecord.handName.c_str()));
//If that didn't work, try the eigenhand loader
if ( !mHand )
{
eh = mDBMgr->getEigenhand(mRecord.handId);
if (eh){
mHand = eh->loadHand(world);
if (mHand){
transf t = translate_transf(vec3(0,-1000,0));
mHand->setTran(t);
}
}
}
if ( !mHand || !mDBMgr->setHand(mHand)) {
DBGA("Failed to load hand");
mStatus = ERROR;
return;
}
if(mRecord.params.size() >= 6)
mHand->getGrasp()->setTaskWrench(&mRecord.params[0]);
}
if (mRecord.params.size() < 8){
mRecord.params.push_back(0);
mRecord.params.push_back(0);
}
//check for virtual contacts
if (mHand->getNumVirtualContacts()==0) {
DBGA("Specified hand does not have virtual contacts defined");
mStatus = ERROR;
return;
}
//scale the links of the hand if necessary.
//scaleHand(mHand, mRecord);
}
void
SensorInputDlg::flockAndGloveFuzzyGrasp(std::vector<transf> &birdTransf)
{
for (int i=0; i<mWorld->getNumRobots(); i++) {
Robot *robot = mWorld->getRobot(i);
if (!robot->usesFlock() || !robot->useCyberGlove()) continue;
//compute robot transform, same as in processFlockRobots
transf robotTran;
transf birdTran = birdTransf[mWorld->getRobot(i)->getBirdNumber()];
if (mFlockMode == FLOCK_ABSOLUTE) {
robotTran = mWorld->getRobot(i)->getFlockTran()->getAbsolute( birdTran );
} else {
robotTran = mWorld->getRobot(i)->getFlockTran()->get( birdTran );
}
//force hand in position
robot->setTran(robotTran);
//compute collisions only for the palm
CollisionReport colReport, originalColReport;
std::vector<Body*> interestList;
interestList.push_back(robot->getBase());
//if palm collision, back up until out of it
transf collisionTran;
bool collision = false;
int steps = 0, maxSteps = 3;
//we need to remember the original collision report for later when we interpolate
int numCollisions = mWorld->getCollisionReport(&originalColReport, &interestList);
while ( numCollisions && steps < maxSteps) {
steps++;
collision = true;
collisionTran = robot->getTran();
transf newTran = translate_transf(vec3(0,0,-10.0) *
robot->getApproachTran()) * robot->getTran();
robot->setTran(newTran);
numCollisions = mWorld->getCollisionReport(&colReport, &interestList);
}
bool palmSuccess;
if (steps == maxSteps) {
//we have not managed to resolve the collision
palmSuccess = false;
robot->setTran(robotTran);
DBGP("Can not resolve collision");
} else if (collision) {
//if we have resolved a collision, go back in until contact
if (!robot->interpolateTo(robot->getTran(), collisionTran, originalColReport)) {
DBGA("Palm interpolation failed");
palmSuccess = false;
} else {
mWorld->findContacts(originalColReport);
palmSuccess = true;
}
} else {
//no collision from the beginning
palmSuccess = true;
}
//read and prepare the glove dof values
std::vector<double> dofVals(robot->getNumDOF(), 0.0);
robot->getDOFVals(&dofVals[0]);
for (int i=0; i<robot->getNumDOF(); i++) {
if ( robot->getGloveInterface()->isDOFControlled(i) ) {
dofVals[i] = robot->getGloveInterface()->getDOFValue(i);
}
}
robot->checkSetDOFVals(&dofVals[0]);
//force the required dof values, no collision check
robot->forceDOFVals(&dofVals[0]);
if (!palmSuccess) {
//we have failed with the palm; leave the chains like this and we are done
DBGA("Palm failure");
//mark the palm in red
continue;
}
numCollisions = mWorld->getCollisionReport(&colReport);
//for each chain, if collision snap to contacts
bool success = true;
for (int c=0; c<robot->getNumChains(); c++) {
success = success && robot->snapChainToContacts(c, colReport);
}
if (success) {
mWorld->updateGrasps();
} else {
//mark colliding bodies
DBGP("Chain failure");
}
}
}
