/*! Adds a virtual contact on the body \a body1, which is assumed to
be the \a l-th link on the \a f-th finger of a robot. Works by
first creating a traditional contact, then converting it to a
virtual contact.
The whole VirtualContact scheme is in bad need of an overhaul.
*/
void
addVirtualContacts(Body *body1, int f, int l, Body *body2, ContactReport &contactSet,
bool softContactsOn )
{
if ( softContactsOn && body1->isElastic() ) {
findSoftNeighborhoods( body1, body2, contactSet );
mergeSoftNeighborhoods(body1, body2, contactSet);
}
ContactReport::iterator cp;
for (cp=contactSet.begin(); cp!=contactSet.end(); cp++) {
Contact *c1;
if ( softContactsOn && body1->isElastic() ) {
c1 = new SoftContact( body1, body2, cp->b1_pos, cp->b1_normal, &(cp->nghbd1) );
c1->setMate(NULL);
((SoftContact *)c1)->setUpFrictionEdges();
} else {
c1 = new PointContact(body1, body2, cp->b1_pos, cp->b1_normal);
c1->setMate(NULL);
}
VirtualContact *vc = new VirtualContact(f, l, c1);
vc->setBody(body1);
body1->addVirtualContact(vc);
delete c1;
}
}
/*! This formulation combines virtual contact energy with autograsp energy. Virtual contact energy is used to "guide"
initial stages of the search and to see if we should even bother computing autograsp quality. Autograsp is a couple
of orders of magnitude higher and so should work very well with later stages of the sim ann search
*/
double
GuidedAutoGraspQualityEnergy::energy() const
{
//first compute regular contact energy; also count how many links are "close" to the object
VirtualContact *contact;
vec3 p, n, cn;
double virtualError = 0; int closeContacts = 0;
//collect virtual contacts first
mHand->getGrasp()->collectVirtualContacts();
for (int i = 0; i < mHand->getGrasp()->getNumContacts(); i++) {
contact = (VirtualContact *)mHand->getGrasp()->getContact(i);
contact->getObjectDistanceAndNormal(mObject, &p, &n);
double dist = p.len();
if ((-1.0 * p) % n < 0) { dist = -dist; }
//BEST WORKING VERSION, strangely enough
virtualError += fabs(dist);
cn = -1.0 * contact->getWorldNormal();
double d = 1 - cn % n;
virtualError += d * 100.0 / 2.0;
if (fabs(dist) < 20 && d < 0.3) { closeContacts++; }
}
virtualError /= mHand->getGrasp()->getNumContacts();
//if more than 2 links are "close" go ahead and compute the true quality
double volQuality = 0, epsQuality = 0;
if (closeContacts >= 2) {
mHand->autoGrasp(false, 1.0);
//now collect the true contacts;
mHand->getGrasp()->collectContacts();
if (mHand->getGrasp()->getNumContacts() >= 4) {
mHand->getGrasp()->updateWrenchSpaces();
volQuality = mVolQual->evaluate();
epsQuality = mEpsQual->evaluate();
if (epsQuality < 0) { epsQuality = 0; } //QM returns -1 for non-FC grasps
}
DBGP("Virtual error " << virtualError << " and " << closeContacts << " close contacts.");
DBGP("Volume quality: " << volQuality << " Epsilon quality: " << epsQuality);
}
//now add the two such that the true quality is a couple of orders of magn. bigger than virtual quality
double q;
if (volQuality == 0) { q = virtualError; }
else { q = virtualError - volQuality * 1.0e3; }
if (volQuality || epsQuality) {DBGP("Final quality: " << q);}
//DBGP("Final value: " << q << std::endl);
return q;
}
double ContactEnergy::energy() const
{
/*
this is if we don't want to use pre-specified contacts, but rather the closest points between each link
and the object all iterations. Not necessarily needed for contact energy, but useful for GQ energy
*/
if (mContactType == CONTACT_LIVE && mType != ENERGY_AUTOGRASP_QUALITY && mType != ENERGY_STRICT_AUTOGRASP)
{
mHand->getWorld()->findVirtualContacts(mHand, mObject);
DBGP("Live contacts computation");
}
mHand->getGrasp()->collectVirtualContacts();
//DBGP("Contact energy computation")
//average error per contact
VirtualContact *contact;
vec3 p, n, cn;
double totalError = 0;
for (int i = 0; i < mHand->getGrasp()->getNumContacts(); i++)
{
contact = (VirtualContact *)mHand->getGrasp()->getContact(i);
contact->getObjectDistanceAndNormal(mObject, &p, NULL);
double dist = p.len();
//this should never happen anymore since we're never inside the object
//if ( (-1.0 * p) % n < 0) dist = -dist;
//BEST WORKING VERSION, strangely enough
totalError += fabs(dist);
//let's try this some more
//totalError += distanceFunction(dist);
//cn = -1.0 * contact->getWorldNormal();
//new version
cn = contact->getWorldNormal();
n = normalise(p);
double d = 1 - cn % n;
totalError += d * 100.0 / 2.0;
}
totalError /= mHand->getGrasp()->getNumContacts();
//DBGP("Contact energy: " << totalError);
return totalError;
}
double
PotentialQualityEnergy::energy() const
{
mHand->getGrasp()->collectVirtualContacts();
bool verbose = false;
VirtualContact *contact;
vec3 p,n,cn;
int count = 0;
//DBGP("Potential quality energy computation")
for (int i=0; i<mHand->getGrasp()->getNumContacts(); i++)
{
contact = (VirtualContact*)mHand->getGrasp()->getContact(i);
contact->computeWrenches(true, false);
contact->getObjectDistanceAndNormal(mObject, &p, NULL);
n= contact->getWorldNormal();
double dist = p.len();
p = normalise(p); //idiot programmer forgot to normalise
double cosTheta = n % p;
double factor = potentialQualityScalingFunction(dist, cosTheta);
if (verbose)
{
fprintf(stderr,"VC %d on finger %d link %d\n",i,contact->getFingerNum(), contact->getLinkNum());
fprintf(stderr,"Distance %f cosTheta %f\n",dist,cosTheta);
fprintf(stderr,"Scaling factor %f\n\n",factor);
}
contact->scaleWrenches( factor );
if (factor > 0.25)
{
count++;
contact->mark(true);
} else contact->mark(false);
}
double gq = -1;
//to make computations more efficient, we only use a 3D approximation
//of the 6D wrench space
std::vector<int> forceDimensions(6,0);
forceDimensions[0] = forceDimensions[1] = forceDimensions[2] = 1;
if (count >= 3) {
mHand->getGrasp()->updateWrenchSpaces(forceDimensions);
gq = mEpsQual->evaluate();
}
if (verbose) {
fprintf(stderr,"Quality: %f\n\n",gq);
}
if (count) {
DBGP("Count: " << count << "; Gq: " << gq << ";");
}
return -gq;
return 10;
}
/* Loads virtual contacts from a file for a single kinematic chain.
*
*@param finger - The kinematic chain describing the finger
*@param filename - The file containing the virtual contacts
*@param fingerNumber - This is currently only used as a sanity check. a finger number < 0 is an error condition.
*/
bool EigenHandLoader::loadContactData(KinematicChain * finger, QString filename, int fingerNumber)
{
// Open the file
FILE *fp = fopen(filename.latin1(), "r");
if (!fp) {
std::cout<< "Could not open contact file " << filename.latin1();
return 0;
}
int numContacts;
VirtualContact *newContact;
fscanf(fp,"%d",&numContacts);
for (int i=0; i<numContacts; i++)
{
newContact = new VirtualContact();
newContact->readFromFile(fp);
newContact->setFingerNum(fingerNumber);
int f = fingerNumber;
int l = newContact->getLinkNum();
if ( f >= 0) {
newContact->setBody(finger->getLink(l) );
finger->getLink(l)->addVirtualContact( newContact );
} else {
fprintf(stderr,"Wrong finger number on contact!!!\n");
delete newContact;
continue;
}
newContact->computeWrenches(false,false);
}
return 1;
}
