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;
}
/*! 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;
}
}
