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