/*! The joint Jacobian is a 6x1 matrix that relates movement of a joint
(1 DOF) to movement (translation and rotation) of a point in 3D space
placed at world coordinates \a toTarget. If worldCoords is true, the
jacobian is expressed in world coordinate system; otherwise, it is
expressed in the local coordinate system of the target point.
*/
Matrix
Joint::jacobian(const Joint *joint, const transf &jointTran,
const transf &toTarget, bool worldCoords)
{
transf T;
if (worldCoords) {
// the translation from joint coordinate system to world coordinate system
T = transf(Quaternion::IDENTITY, toTarget.translation()) * jointTran.inverse();
} else {
T = toTarget * jointTran.inverse();
}
double M[36];
T.jacobian(M);
Matrix fullJ(M,6,6,true);
Matrix J(Matrix::ZEROES<Matrix>(6,1));
if (joint->getType() == REVOLUTE) {
//keep rotation against z
J.copySubBlock(0, 0, 6, 1, fullJ, 0, 5);
} else if (joint->getType() == PRISMATIC) {
//keep translation along z
J.copySubBlock(0, 0, 6, 1, fullJ, 0, 2);
} else {
assert(0);
}
return J;
}
void PositionStateComplete::setTran(const transf &t)
{
getVariable("Tx")->setValue( t.translation().x() );
getVariable("Ty")->setValue( t.translation().y() );
getVariable("Tz")->setValue( t.translation().z() );
getVariable("Qw")->setValue( t.rotation().w );
getVariable("Qx")->setValue( t.rotation().x );
getVariable("Qy")->setValue( t.rotation().y );
getVariable("Qz")->setValue( t.rotation().z );
}
/*! This is copied over from the distance function of the SearchState so that we use
the same metric as when doing the actual grasp planning.
*/
bool CompliantGraspCopyTask::searchSimilarity(const transf &t1, const transf &t2)
{
//we use the same threshold as in grasp planning
double DISTANCE_THRESHOLD = 0.01;
vec3 dvec = t1.translation() - t2.translation();
double d = dvec.len() / mObject->getMaxRadius();
Quaternion qvec = t1.rotation() * t2.rotation().inverse();
vec3 axis; double angle;
qvec.ToAngleAxis(angle, axis);
//0.5 weight out of thin air
double q = 0.5 * fabs(angle) / M_PI;
if (std::max(d, q) > DISTANCE_THRESHOLD) { return false; }
return true;
}
double perpendicularDistInBTrans(const position & p){
position pInB = p*bodyTrans.inverse();
//positions cannot be dotted with themselves because they aren't really meant to be used as vectors
// return sqrt(pInB%pInB - pInB%v);
return sqrt(pInB.x()*pInB.x() + pInB.y()*pInB.y() + pInB.z()*pInB.z()- pow(pInB%v,2));
}
/*!
Creates a 6x6 matrix, \a transformMat, that transforms a wrench expressed
in one coordinate system to wrench expressed in another. \a T is the
transform, and \a p is the new torque origin expressed within the new
coordinate system.
*/
void buildForceTransform(transf &T,vec3 &p,double *transformMat)
{
static int j,k;
static double R[9];
static double crossMat[9];
static double Rcross[9];
static vec3 radius;
R[0] = T.affine().element(0,0);
R[1] = T.affine().element(0,1);
R[2] = T.affine().element(0,2);
R[3] = T.affine().element(1,0);
R[4] = T.affine().element(1,1);
R[5] = T.affine().element(1,2);
R[6] = T.affine().element(2,0);
R[7] = T.affine().element(2,1);
R[8] = T.affine().element(2,2);
/*
R[0] = T.affine().element(0,0);
R[1] = T.affine().element(1,0);
R[2] = T.affine().element(2,0);
R[3] = T.affine().element(0,1);
R[4] = T.affine().element(1,1);
R[5] = T.affine().element(2,1);
R[6] = T.affine().element(0,2);
R[7] = T.affine().element(1,2);
R[8] = T.affine().element(2,2);
*/
for (j=0;j<9;j++)
if (fabs(R[j]) < MACHINE_ZERO) R[j] = 0.0;
else if (R[j] > 1.0 - MACHINE_ZERO) R[j] = 1.0;
else if (R[j] < -1.0 + MACHINE_ZERO) R[j] = -1.0;
radius = T.translation() - p;
crossMat[0]=0.0; crossMat[3]=-radius.z();crossMat[6]=radius.y();
crossMat[1]=radius.z();crossMat[4]=0.0; crossMat[7]=-radius.x();
crossMat[2]=-radius.y();crossMat[5]= radius.x();crossMat[8]=0.0;
//original graspit
//dgemm("N","N",3,3,3,1.0,R,3,crossMat,3,0.0,Rcross,3);
// mtc: new version, I believe this is the correct one
dgemm("N","N",3,3,3,1.0,crossMat,3,R,3,0.0,Rcross,3);
fillMatrixBlock(R,3,0,0,2,2,transformMat,6);
for (j=3;j<6;j++)
for (k=0;k<3;k++)
transformMat[6*j+k] = 0.0;
fillMatrixBlock(Rcross,3,3,0,5,2,transformMat,6);
fillMatrixBlock(R,3,3,3,5,5,transformMat,6);
}
/*! This uses a slightly different similarity metric, more in tune with what we use for clustering
but different from the one used during actual grasp planning, which will lead to inconsistencies
in the sampling. This *should* not create real problems though.
The angular threshold does not really play any role here as compliant copies *should* not have
any angular differences.
*/
bool CompliantGraspCopyTask::similarity(const transf &t1, const transf &t2)
{
//7.5mm distance threshold
double DISTANCE_THRESHOLD = 7.5;
//15 degrees angular threshold
double ANGULAR_THRESHOLD = 0.26;
vec3 dvec = t1.translation() - t2.translation();
double d = dvec.len();
if (d > DISTANCE_THRESHOLD) { return false; }
Quaternion qvec = t1.rotation() * t2.rotation().inverse();
vec3 axis; double angle;
qvec.ToAngleAxis(angle, axis);
if (angle > M_PI) { angle -= 2 * M_PI; }
if (angle < -M_PI) { angle += 2 * M_PI; }
if (fabs(angle) > ANGULAR_THRESHOLD) { return false; }
return true;
}
/*! Given a point that has world transform \a toTarget, this computes the 6x6 Jacobian
of this joint relative to that point. The Jacobian is either expressed in global
world coordinates or in the local coordinates of the target.
*/
void
DynJoint::jacobian(transf toTarget, Matrix *J, bool worldCoords)
{
transf myTran = getPrevTrans() * getPrevLink()->getTran();
transf T;
if (worldCoords) {
// the translation from joint coordinate system to world coordinate system
T = transf(Quaternion::IDENTITY, toTarget.translation()) * myTran.inverse();
} else {
T = toTarget * myTran.inverse();
}
double M[36];
T.jacobian(M);
J->copyMatrix(Matrix(M,6,6,true));
}
/*! A helper function that gives the change between two transforms, but
disregards any change along the approach direction of the hand. I'm not
really sure this is needed anymore, might be replaced in the future.
*/
double
OnLinePlanner::distanceOutsideApproach(const transf &solTran, const transf &handTran, bool useAlignment)
{
double max_angle = M_PI / 4.0;
double max_dist = 50.0;
double f;
//relative transform between the two
transf changeTran = solTran * handTran.inverse();
//DBGP("T1: " << solTran.translation());
//DBGP("T2: " << handTran.translation());
//DBGP("Change: " << changeTran.translation() );
//get change in terms of approach direction
changeTran = mHand->getApproachTran() * changeTran * mHand->getApproachTran().inverse();
//double dotZ = (mHand->getApproachTran()*handTran).affine().row(2) * (mHand->getApproachTran()*solTran).affine().transpose().row(2);
//get angular change
double angle; vec3 axis;
changeTran.rotation().ToAngleAxis(angle, axis);
//get translation change
vec3 approach = changeTran.translation();
//change along approach direction does not count as distance
//DBGP("Approach: " << approach);
if (approach.z() < 0) {
f = -1.0;
} else {
f = 1.0;
}
double angleMod = 1.0;
if (axis.z() < 0)
angleMod = -1.0;
approach.z() = 0;
double dist = approach.len();
//compute final value
if (angle > M_PI) angle -= 2*M_PI;
if (angle < -M_PI) angle += 2*M_PI;
angle = fabs(angle) / max_angle ;
dist = dist / max_dist;
//DBGP("Angle " << angle << "; dist " << dist << std::endl);
double alignment_score = 1.0;
if (useAlignment) alignment_score = .001+(axis.z() * angleMod);
return f * std::max(angle, dist)/alignment_score;
}
/*! A helper function that gives the change between two transforms, but
disregards any change along the approach direction of the hand. I'm not
really sure this is needed anymore, might be replaced in the future.
*/
double
OnLinePlanner::distanceOutsideApproach(const transf &solTran, const transf &handTran)
{
double max_angle = M_PI / 4.0;
double max_dist = 50.0;
double f;
//relative transform between the two
transf changeTran = handTran.inverse() % solTran;
//DBGP("T1: " << solTran.translation());
//DBGP("T2: " << handTran.translation());
//DBGP("Change: " << changeTran.translation() );
//get change in terms of approach direction
changeTran = mHand->getApproachTran().inverse() % changeTran % mHand->getApproachTran();
//get angular change
double angle; vec3 axis;
Eigen::AngleAxisd aa(changeTran.rotation());
axis = aa.axis();
angle = aa.angle();
//get translation change
vec3 approach = changeTran.translation();
//change along approach direction does not count as distance
//DBGP("Approach: " << approach);
if (approach.z() < 0) {
f = -1.0;
} else {
f = 1.0;
}
approach.z() = 0;
double dist = approach.norm();
//compute final value
if (angle > M_PI) { angle -= 2 * M_PI; }
if (angle < -M_PI) { angle += 2 * M_PI; }
angle = fabs(angle) / max_angle ;
dist = dist / max_dist;
//DBGP("Angle " << angle << "; dist " << dist << std::endl);
return f * std::max(angle, dist);
}
/*! Given a start position which is expected to be collision-free, and a
new position which which causes inter-penetration, it interpolates
between the two to find the exact moment of contact. Returns false if
the interpolation fails (usually because the starting point is also in
collision).
Only looks at possible collisions in \a colReport, which the caller must
determine before calling this.
*/
bool
WorldElement::interpolateTo(transf lastTran, transf newTran, const CollisionReport &colReport)
{
vec3 nextTranslation;
Quaternion nextRotation;
transf nextTran;
int numCols = colReport.size();
//this causes the interpolation to first check the original transform
//since in many cases the original position is actually the one in contact, this can save a lot of computation
//as well as machine precision problems
//technically, it allows t to become negative, but in practice this should never happen as long as the initial position
//is collision free
double t = 0.0, deltat = 1.0, minDist;
bool done = false;
while (!done && deltat > 1.0e-20 && t >= 0) {
DBGP("move interpolation cycle")
deltat /= 2.0;
nextTranslation = (1.0-t)*lastTran.translation() + t*newTran.translation();
nextRotation = Quaternion::Slerp(t,lastTran.rotation(),newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
DBGP("moving to time : " << t);
if (setTran(nextTran) == FAILURE) {
deltat = 0;
break;
}
minDist = myWorld->getDist(colReport[0].first,colReport[0].second);
for (int i=1; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
minDist = MIN(minDist,dist);
}
DBGP("minDist: " << minDist);
if (minDist > 0) {
if (minDist < Contact::THRESHOLD * 0.5)
break;
t += deltat;
}
else
t -= deltat;
//debug code
if ( deltat <= 1.0e-20 || t < 0) {
for (int i=0; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
DBGA(colReport[i].first->getName().latin1() << " -- " <<
colReport[i].second->getName().latin1() << " is " << dist);
}
}
}
if (deltat < 1.0e-20 || t < 0) {
DBGP("deltat failsafe or negative t hit; interpolate failure");
fprintf(stdout,"WorldElement interpolation error!\n");
return false;
} else {
DBGP("deltat: " << deltat << "; minDist: " << minDist <<"; interpolate success.");
return true;
}
}
/*!
Moves the element from its current pose to the new pose specified by \a tr.
This motion is performed in several steps such that the translation
between each step does not exceed \a translStepSize and the angle of
rotation does not exceed \a rotStepSize (expressed in radians). The
intermediate poses are determined using linear interpolation for the
translation and spherical linear interpolation for the rotation. If
a collision is encountered during the motion, the point of first contact
is determined and the element is left in that position. This function
returns false if a collision was encountered (or contacts prevented the motion)
or true if no collisions were encountered and the move was completed.
*/
bool
WorldElement::moveTo(transf &newTran,double translStepSize, double rotStepSize)
{
bool moveFinished = false;
transf origTran,nextTran,motion;
Quaternion nextRotation;
vec3 nextTranslation;
double percentComplete,moveIncrement,translationLength;
double angle;
vec3 axis;
bool success;
CollisionReport contactReport;
//DBGP("moveTo called");
origTran = getTran();
/*
std::cout << "WorldElement origTran: " << origTran.translation().x() << " " <<
origTran.translation().y() << " " <<
origTran.translation().z() << " " <<
origTran.rotation().w << " " <<
origTran.rotation().x << " " <<
origTran.rotation().y << " " <<
origTran.rotation().z << " " << "\n";
*/
//calculate the difference
translationLength = (newTran.translation() - origTran.translation()).len();
nextRotation = newTran.rotation() * origTran.rotation().inverse();
nextRotation.ToAngleAxis(angle,axis);
moveIncrement = 1.0;
if (translationLength != 0.0) {
if (translStepSize == ONE_STEP)
moveIncrement = 1.0;
else
moveIncrement = MIN(moveIncrement, translStepSize / translationLength);
}
if (angle != 0.0) {
if (rotStepSize == ONE_STEP)
moveIncrement = MIN(moveIncrement, 1.0);
else
moveIncrement = MIN(moveIncrement, rotStepSize / angle);
}
// check contacts
nextTranslation = (1.0-moveIncrement)*origTran.translation() + moveIncrement*newTran.translation();
nextRotation = Quaternion::Slerp(moveIncrement,origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
motion = nextTran * getTran().inverse();
if (contactsPreventMotion(motion)) {
DBGP("contacts prevent motion")
return false;
}
percentComplete = 0.0;
while (!moveFinished) {
percentComplete += moveIncrement;
if (percentComplete >= 1.0) {
percentComplete = 1.0;
moveFinished = true;
}
nextTranslation = (1.0-percentComplete)*origTran.translation() + percentComplete*newTran.translation();
nextRotation = Quaternion::Slerp(percentComplete,origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
/*
std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " <<
nextTran.translation().y() << " " <<
nextTran.translation().z() << " " <<
nextTran.rotation().w << " " <<
nextTran.rotation().x << " " <<
nextTran.rotation().y << " " <<
nextTran.rotation().z << " " << "\n";
*/
success = jumpTo(nextTran, &contactReport);
if (!success || contactReport.size() != 0) {
moveFinished = true;
}
}
if (!success) {
DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread "
<< getWorld()->getCollisionInterface()->getThreadId());
} else {
myWorld->findContacts(contactReport);
}
if (contactReport.size() != 0) return false;
return true;
}
/*! Sets the frame, location and normal, but does not do anything with the friction edges.
*/
void VirtualContact::changeFrame(transf tr)
{
frame = tr;
loc = position(tr.translation().toSbVec3f());
normal = vec3(0, 0, 1) * tr;
}
