void ArizonaTest::initializeTest()
{
if(!mObject){
DBGA("ArizonaTest: object not present");
return;
}
if (mContacts.empty()) {
DBGA("ArizonaTest: no contacts");
return;
}
std::vector<int> arizonaTestD(6,0);
if (mIsBuildIn3D) {
//fx, tx and ty
arizonaTestD[2] = arizonaTestD[3] = arizonaTestD[4] = 1;
} else {
arizonaTestD = Grasp::ALL_DIMENSIONS;
}
DBGP("AT: updating grasp");
mGrasp->update(arizonaTestD); // arizonaTestD is used when isBuildIn3D = true;
//create the 3D hull that we are interested in
DBGP("AT: creating 3D hull");
createGWSProjection();
if(mQual->evaluate() < 0){
DBGA("Non-ForceClosure");
return;
}
for(int i = 1; i <= NUMCMPATTERN; i ++){
forceScales[i-1] = getMinimunForce(i);
}
DBGP("AT: all tests initialized");
}
/* create directory recursively */
int mkdir_p(const char *path)
{
int n;
char buf[PATH_MAX], *p = buf;
assert(path);
if ((n = strlen(path)) >= PATH_MAX) {
DBGP("over length: %d, %d", n, PATH_MAX);
return -1;
}
DBGP("len=%d; path='%s'\n", n, path);
strcpy(buf, path);
while ((p = strchr(p+1, '/'))) {
struct stat sb;
*p = '\0';
if (stat(buf, &sb) && mkdir(buf, ACCESSPERMS)) {
DBGE("stat/create '%s' failed", buf);
return -1;
}
*p = '/';
}
return n;
}
/*! For elastic bodies, calculates the contact neighborhoods and puts
them in the contact set. Not done for rigid contacts.
*/
void findSoftNeighborhoods( Body *body1, Body *body2, ContactReport &contactSet )
{
ContactReport::iterator itr;
for( itr = contactSet.begin(); itr != contactSet.end(); itr++ ) {
DBGP("Contact finding regions:");
//right now, findregion assumes point is in body frame
//the units for the threshold and the radius should be in mm, NOT cm
//The input radius is proportional to the fourth root of the youngs mod/depth of mattress
//(units for Youngs Mod and mattress depth are in Pa and meters)
//This gives a radius around 6 mm, for rubber with youngs = 1.5E6 and h = 3E-3which is reasonable
double rad = pow( 1/(MAX( body1->getYoungs(), body2->getYoungs() )), 0.333 ) * 1000.0 * 0.4;
//the 0.4 is a fudge factor for the time being
//hack to ensure that the fit is at least resonable
if( rad <= 3.0 && rad >= 10.0 ) rad = 5.0;
body1->getWorld()->FindRegion( body1, itr->b1_pos, itr->b1_normal, rad, &(itr->nghbd1) );
DBGP("Neighborhood on body1 has " << itr->nghbd1.size() << " points");
body2->getWorld()->FindRegion( body2, itr->b2_pos, itr->b2_normal, rad, &(itr->nghbd2) );
DBGP("Neighborhood on body2 has " << itr->nghbd2.size() << " points");
}
}
bool HandObjectState::readFromFile(FILE *fp)
{
//this whole read-write system is one big hack.
int type; fpos_t pos;
fgetpos(fp,&pos);
if (!fscanf(fp,"%d",&type)) return false;
DBGP("Pose type: " << type);
if ( (StateType)type != POSE_DOF && (StateType)type != POSE_EIGEN ) return false;
if ( type != mPosture->getType() ) {
setPostureType((StateType)type);
}
fsetpos(fp,&pos);
if ( !mPosture->readFromFile(fp) ) {
DBGA("Failed");
return false;
}
fgetpos(fp,&pos);
if (!fscanf(fp,"%d",&type)) return false;
DBGP("Space type: " << type);
if ( (StateType)type != SPACE_COMPLETE && (StateType)type != SPACE_APPROACH &&
(StateType)type != SPACE_AXIS_ANGLE && (StateType)type != SPACE_ELLIPSOID ) return false;
if ( type != mPosition->getType() ) {
setPositionType((StateType)type);
}
fsetpos(fp,&pos);
if ( !mPosition->readFromFile(fp) ) return false;
return true;
}
void
GraspTester::mainLoop()
{
GraspPlanningState *s = popCandidate();
if (!s) {
DBGP("Empty buffer for tester");
msleep(100);
return;
}
s->changeHand(mHand, true);
testGrasp(s);
DBGP("TESTER: candidate has energy " << s->getEnergy());
mHand->breakContacts();
if (s->isLegal() && s->getEnergy() < -1.2) {
//save the final grasping position that has resulted from autograsp
s->setPositionType(SPACE_COMPLETE);
s->setPostureType(POSE_DOF);
//save the current transform in absolute terms
s->setRefTran(transf::IDENTITY);
s->saveCurrentHandState();
postSolution(s);
DBGP("Tester posting a solution at iteration " << s->getItNumber());
} else {
DBGP("Tester removing candidate");
delete s;
}
}
/*! Given a solution (grasping position) this function finds
a correct finger posture for the CURRENT position of the
hand, so that it's as close as possible to the solution
without hitting the object. It also checks if there is a
path between the CURRENT position of the fingers and the
desired one.
*/
bool
OnLineGraspInterface::getSuggestedDOF(const GraspPlanningState *s, double *initialDof, double *finalDof)
{
s->readPosture()->getHandDOF(finalDof);
mHand->forceDOFVals(finalDof);
//close fingers gradually as we move closer to the target state
transf handTran = mHand->getTran();
transf solTran = s->getTotalTran();
vec3 app = handTran.translation() - solTran.translation();
double dist = app.len();
//first find how much we should open the fingers, based on distance from solution
double openFingers = dist / 200.0;
DBGP("Open fingers to " << openFingers);
if (!mHand->quickOpen(openFingers)) {
DBGP("Open finger position not found");
return false;
}
mHand->getDOFVals(finalDof);
//also check if we can get from here to there
mHand->forceDOFVals(initialDof);
if (mHand->checkDOFPath(finalDof, 0.16)) {
return true;
} else {
DBGP("Open finger found, but not reachable");
return false;
}
}
/*! Attempts to maintain a list of unique solutions. Therefore, whenever
a new state is added to the list, we check if any of the states that
are already in the list are within a given distance of the new state.
If so, the best one is kept and the other one is thrown away. This
method does not gurantee unique states, but it comes close and runs
in linear time for each addition, rather than square time for
maintenance.
*/
bool
EGPlanner::addToListOfUniqueSolutions(GraspPlanningState *s, std::list<GraspPlanningState *> *list, double distance)
{
std::list<GraspPlanningState *>::iterator it;
it = list->begin();
bool add = true;
while (it != list->end()) {
double d = stateDistance(s, *it);
if (fabs(d) < distance) {
DBGP("Distance: " << fabs(d));
//states are close to each other
if (s->getEnergy() < (*it)->getEnergy()) {
//new state is better; remove old one from list
delete(*it);
it = list->erase(it);
DBGP("Old state removed");
} else {
//old state is better; we don't want to add the new one
add = false;
break;
}
} else {
//states are not close, proceed through the list
it++;
}
}
if (add) {
list->push_back(s);
}
return add;
}
/*
* Increments which cycles are cut. If all combinations have been tried, fixes
* the graph and returns 0. Otherwise returns 1.
*/
int inc_cycles(cycle_counter counter, graph* g, graph* f) {
ll_node* cc = counter->counter;
int carry = 0;
do {
ll_node* cycle = (ll_node*)cc->data;
cc_node n = *(cc_node*)cycle->data;
fix_edge(counter->g_info,g->verts + n.src,n.g_cc_i);
fix_edge(counter->f_info,f->verts + n.dest,n.f_cc_i);
if(cycle->next->data == NULL) {
carry = 1;
cc->data = cycle->next->next;
n = *(cc_node*)cycle->next->next->data;
} else {
cc->data = cycle->next;
n = *(cc_node*)cycle->next->data;
}
DBGP(1);
DBGP(2);
DBGP(3);
delete_edge(counter->g_info,g->verts + n.src,n.g_cc_i);
delete_edge(counter->f_info,f->verts + n.dest,n.f_cc_i);
cc = cc->next;
} while(carry == 1 && cc != NULL);
return carry != 1;
}
void
CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed,
std::list<GraspPlanningState*> *sampling,
int samples, double x, double y, double z)
{
//redundant, but easier...
double a = seed.readPosition()->getParameter("a");
double b = seed.readPosition()->getParameter("b");
//double c = seed.readPosition()->getParameter("c");
//compute angular values
//from HandObjectStateImpl:
//x = a * cos(beta) * cos(gamma);
//y = b * cos(beta) * sin(gamma);
//z = c * sin(beta);
double beta = asin(z / sqrt(x*x + y*y + z*z));
double gamma = atan2(y/b, x/a);
DBGP("x: " << x << "; y: " << y <<"; z: " << z);
DBGP("gamma: " << gamma << "; beta: " << beta);
//sample roll angle as well
for (int m=0; m<samples; m++) {
//only sample from 0 to almost PI, as the HH is symmetric
double tau = M_PI * ((double)m) / samples;
GraspPlanningState *newState = new GraspPlanningState(&seed);
newState->getPosition()->getVariable("tau")->setValue(tau);
newState->getPosition()->getVariable("gamma")->setValue(gamma);
newState->getPosition()->getVariable("beta")->setValue(beta);
sampling->push_back(newState);
}
}
/*!
After a timestep has been completed, this computes the current joint angle
and velocity from the relative positions and velocities of the connected
links.
*/
void
RevoluteDynJoint::updateValues()
{
transf b1JointTran = prevFrame * prevLink->getTran();
//the z axis of the previous link, by definition the axis of the one joint
vec3 axis = b1JointTran.affine().row(2);
joint->setWorldAxis(axis);
double vel1 = vec3(prevLink->getVelocity()[3],
prevLink->getVelocity()[4],
prevLink->getVelocity()[5]) % axis;
double vel2 = vec3(nextLink->getVelocity()[3],
nextLink->getVelocity()[4],
nextLink->getVelocity()[5]) % axis;
joint->setVelocity(vel2-vel1);
transf diffTran = joint->getTran(0.0).inverse() * nextLink->getTran() * b1JointTran.inverse();
double val;
diffTran.rotation().ToAngleAxis(val,axis);
if (axis.z() < 0) val = -val;
DBGP("link "<< prevLink->getName().latin1() <<" - link "<<nextLink->getName().latin1());
DBGP(" joint angle: "<<val*180.0/M_PI<<" radians: "<<val<< " velocity: "<<vel2-vel1);
joint->setDynamicsVal(val);
}
void Leaf::fitBox(const mat3 &R, vec3 ¢er, vec3 &halfSize)
{
vec3 x = R.row(0);
vec3 y = R.row(1);
vec3 z = R.row(2);
vec3 max(-1.0e10, -1.0e10, -1.0e10);
vec3 min( 1.0e10, 1.0e10, 1.0e10);
std::list<Triangle>::iterator it;
for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
}
DBGP("Max: " << max);
DBGP("Min: " << min);
for (int i=0; i<3; i++) {
halfSize[i] = 0.5 * (max[i] - min[i]);
}
DBGP("computed halfsize: " << halfSize);
//halfSize = 0.5 * (max - min);
center = min + halfSize;
center = R.inverse() * center;
//sanity check
for (int i=0; i<3; i++) {
if (halfSize[i] < TOLERANCE) {
if (halfSize[i] < 0.5 * TOLERANCE) {
DBGA("Warning: degenerate box computed");
}
halfSize[i] = TOLERANCE;
}
}
DBGP("returned halfsize: " << halfSize);
}
static int piapidev_parse( const char *initstr, unsigned int *saddr, unsigned int *sport )
{
int shift = 24;
char *token;
DBGP( "Info: received initialization string %s\n", initstr );
*saddr = 0;
while( shift >= 0 ) {
if( (token = strtok( (shift!=24) ? NULL : (char *)initstr, (shift!=0) ? "." : ":" )) == 0x0 ) {
fprintf( stderr, "Error: invalid server IP address in initialization string %s\n", initstr );
return -1;
}
*saddr |= ( atoi(token) << shift );
shift -= 8;
}
if( (token = strtok( NULL, ":" )) == 0x0 ) {
fprintf( stderr, "Error: missing server port separator in initialization string %s\n", initstr );
return -1;
}
*sport = atoi(token);
DBGP( "Info: extracted initialization string (SADDR=%08x, SPORT=%u)\n", *saddr, *sport );
return 0;
}
double ArizonaTest::getQuality()
{
DBGP("AT: evaluating quality");
double q = mQual->evaluate();
DBGP("Quality: " << q);
if (q < 0) q = -1;
return q;
}
void Node::getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs)
{
if (currentDepth == desiredDepth || isLeaf() ) {
bvs->push_back( BoundingBox(mBbox) );
DBGP("BBox tran: " << mBbox.getTran());
DBGP("BBox size: " << mBbox.halfSize);
return;
}
}
/*!
After a timestep has been completed, this computes the current joint angles
and velocities from the relative positions and velocities of the connected
links.
*/
void
UniversalDynJoint::updateValues()
{
vec3 axis,ax0,ax1,ax2;
double val,vel1,vel2;
transf b1JointTran = prevFrame * prevLink->getTran();
transf b2JointTran = nextFrame * nextLink->getTran();
// the z axis of the previous link - by definition, the rotation direction of the next joint
ax0 = b1JointTran.affine().row(2);
ax2 = b2JointTran.affine().row(2);
ax1 = normalise(ax2*ax0);
DBGP("ax0: "<<ax0<<" len "<<ax0.len());
DBGP("ax1: "<<ax1<<" len "<<ax1.len());
DBGP("ax2: "<<ax2<<" len "<<ax2.len());
axis = ax1*ax2;
joint1->setWorldAxis(axis);
vel1 = vec3(prevLink->getVelocity()[3],
prevLink->getVelocity()[4],
prevLink->getVelocity()[5]) % axis;
vel2 = vec3(nextLink->getVelocity()[3],
nextLink->getVelocity()[4],
nextLink->getVelocity()[5]) % axis;
joint1->setVelocity(vel2-vel1);
vec3 ref1 = (joint2->getTran(0.0)*joint1->getTran(0.0)*b1JointTran).affine().row(2);
//original GraspIt:
val = atan2 (ax2 % (ax0 * ref1), ax2 % ref1);
DBGP("link " << prevLink->getName().latin1() << " - link " << nextLink->getName().latin1() << ":");
DBGP(" joint1 angle: " << val*180.0/M_PI << " " << val << " (rad)");
joint1->setDynamicsVal(val);
//is this right here? It's different from what's done for joint1
axis = b2JointTran.affine().row(2);
//joint2->setWorldAxis(axis);
joint2->setWorldAxis(ax0*ax1);
vel1 = vec3(prevLink->getVelocity()[3],
prevLink->getVelocity()[4],
prevLink->getVelocity()[5]) % axis;
vel2= vec3(nextLink->getVelocity()[3],
nextLink->getVelocity()[4],
nextLink->getVelocity()[5]) % axis;
joint2->setVelocity(vel2-vel1);
vec3 ref2 = (joint2->getTran(0.0)*joint1->getTran(0.0)).inverse().affine().row(2) * b2JointTran;
val = atan2 (ref2 % ax1, ref2 % (ax1*ax2));
joint2->setDynamicsVal(val);
}
/*! 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
SimAnn::biasedNeighborDistribution(double T, double in, double conf)
{
double u1, u2, u;
double sigma_sq, mean;
//we get the confidence as a linear value between 0 and 1
//if we don't invert the nbr fct first, a variance of 0.1 seems to be "average confidence"
//convert to a log so that we exploit mainly the area between 0 and 0.1
//this also inverts it, mapping conf = 1 (max confidence) to sigma = 0 (always produce a 0)
//if (conf < 0.01) sigma_sq = 1;
//else sigma_sq = -log( conf ) / 10;
//for the case where we use the inverse neighbor, it's best to set the variance more conservatively
//which means that most play is actually between 0.1 and 1
//so all we gotta do is invert the confidence
sigma_sq = 1.0 - conf;
DBGP("In: " << in << " conf: " << conf << " simga_sq: " << sigma_sq);
//the target value we get here is normalized and capped to -1..1
//we set the mean of the gaussian to the value that the neighbor function would map to our target value
mean = neighborInverse(T, in);
//alternatively, we could just set it directly to the target and let the neighbor fctn map it to
//something depending on the temperature, like this:
//mean = in;
//but this means that even for infinitely strict target (confidence = 1.0), we can never reach it exactly
//as the nbr fctn will map it to smaller and smaller steps.
//compute a normal distribution, centered at input and clamped btw -1 and 1
u = 2;
double loops = 0;
while (u > 1 || u < -1) {
//start with uniform distribution
u1 = ((double)rand()) / RAND_MAX;
u2 = ((double)rand()) / RAND_MAX;
//turn it into a normal distribution with the Box-Muller transform
u = sqrt(-2 * log(u1)) * cos(2 * M_PI * u2);
//set variance based on confidence and mean based on input
u = mean + sqrt(sigma_sq) * u;
loops++;
}
//just check that we're not spending too much time here
if (loops > 20) { DBGA("Biased distribution loops: " << loops); }
//use it to generate Ingber neighbor
double y = T * (pow(1.0 + 1.0 / T , fabs(u)) - 1);
if (u < 0) { y = -1.0 * y; }
DBGP("u: " << u << " y: " << y << " loops: " << loops);
return y;
}
/*! Sets the max and min vals of the DOF from the smallest range of the joint
limits. Also forces defaultValue to be inside those limits.*/
void DOF::updateMinMax()
{
maxq = (*jointList.begin())->getMax()/getStaticRatio(*jointList.begin());
minq = (*jointList.begin())->getMin()/getStaticRatio(*jointList.begin());
DBGP("Joint 0 min " << minq << " max " << maxq);
if (maxq < minq) std::swap(maxq, minq);
DBGP("maxq " << maxq << " minq " << minq);
std::vector<Joint *>::iterator j;
double testMin, testMax;
DBGST(int num = 0;)
for(j=++jointList.begin();j!=jointList.end();j++) {
void
Joint::applyPassiveInternalWrenches()
{
double f = getFriction();
DBGP("Friction coeffs: " << f0 << " " << f1);
DBGP("Friction force: " << f);
if (f != 0.0) applyInternalWrench(f);
f = getSpringForce();
DBGP("Spring force: " << f);
applyInternalWrench(-f);
}
double AutoGraspQualityEnergy::energy() const
{
DBGP("Autograsp quality computation");
mHand->autoGrasp(false, 1.0);
mHand->getGrasp()->collectContacts();
mHand->getGrasp()->updateWrenchSpaces();
double volQual = mVolQual->evaluate();
double epsQual = mEpsQual->evaluate();
if (epsQual < 0) { epsQual = 0; } //returns -1 for non-FC grasps
DBGP("Autograsp quality: " << volQual << " volume and " << epsQual << " epsilon.");
return - (30 * volQual) - (100 * epsQual);
}
double ArizonaTest::binarySearch(const vec3 &direction, PQP_Model &volume, double startValue, double endValue)
{
//we assume that startValue is inside the model and endValue is outside the model
//we find the value in between that is exactly on the model
double currentValue = startValue;
double currentInterval = (endValue - startValue)/2.0;
PQP_REAL pt[3];
PQP_REAL R[3][3]={{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}};
PQP_REAL T[3]={0.0,0.0,0.0};
double closest_dist = 1.0e9, thresh = 0.0, distance;
PQP_REAL closest_pt[3], closest_normal[3];
bool success = false;
while (1) {
pt[0] = currentValue * direction.x();
pt[1] = currentValue * direction.y();
pt[2] = currentValue * direction.z();
distance = GetShortestDist(pt, &volume, R, T, closest_dist, closest_pt, closest_normal, thresh);
DBGP("disturbance: " << pt[0] << " " << pt[1] << " " << pt[2]);
DBGP("Closest point: " << closest_pt[0] << " " << closest_pt[1] << " " << closest_pt[2] << "; Distance is: " << distance);
DBGP("Current interval: " << currentInterval);
if(distance < EPSILON){
success = true;
break;
}
if(isOutside(&volume, pt)){
DBGP("outside" << std::endl);
if (currentValue <= startValue) {
DBGP("Error: startValue is outside of model!");
break;
}
currentValue -= currentInterval;
} else {
DBGP("inside" << std::endl);
if (currentValue >= endValue) {
DBGP("Error: endValue is inside model!");
break;
}
currentValue += currentInterval;
}
currentInterval /= 2;
if ( fabs(currentInterval) < 1.0e-10 ) {
DBGP("Max loops exceeded!");
break;
}
}
if (distance > 10e-5) {
DBGA("Binary search failed! distance is " << distance);
} else {
DBGP("Binary search success!");
}
return currentValue;
}
/*! If the two paths have no common root, the returned path is
identical to \a absolutePath.
Code adapted from;
http://mrpmorris.blogspot.com/2007/05/convert-absolute-path-to-relative-path.html
*/
QString
relativePath(QString absolutePath, QString relativeToDir)
{
absolutePath.replace("\\", "/");
relativeToDir.replace("\\", "/");
QStringList absoluteDirectories = absolutePath.split('/', QString::SkipEmptyParts);
QStringList relativeDirectories = relativeToDir.split('/', QString::SkipEmptyParts);
//Get the shortest of the two paths
int length = std::min(absoluteDirectories.count(), relativeDirectories.count());
//Use to determine where in the loop we exited
int lastCommonRoot = -1;
int index;
DBGP("Absolute path: " << absolutePath.latin1());
DBGP("Relative to : " << relativeToDir.latin1());
//Find common root
for (index = 0; index < length; index++) {
if (absoluteDirectories[index] == relativeDirectories[index]) {
lastCommonRoot = index;
} else {
break;
}
}
DBGP("Last common root: " << lastCommonRoot);
//If we didn't find a common prefix then return full absolute path
if (lastCommonRoot == -1) {
return absolutePath;
}
//Build up the relative path
QString relativePath;
//Add on the ..
for (index = lastCommonRoot + 1; index < relativeDirectories.count(); index++) {
if (relativeDirectories[index].length() > 0) {
relativePath.append("../");
}
}
//Add on the folders
for (index = lastCommonRoot + 1; index < absoluteDirectories.count() - 1; index++) {
relativePath.append(absoluteDirectories[index]).append("/");
}
relativePath.append(absoluteDirectories[absoluteDirectories.count() - 1]);
DBGP("Relative path: " << relativePath.latin1());
return relativePath;
}
/*!
Given a pointer to the GWS to remove, this decrements the reference count
for that GWS. If the reference count becomes 0, the GWS is deleted.
*/
void
Grasp::removeGWS(GWS *gws)
{
DBGP("removing gws");
gws->unref();
if (gws->getRefCount() == 0) {
DBGP("deleting gws");
gwsList.remove(gws);
delete gws;
} else {
DBGP("gws refcount: "<<gws->getRefCount());
}
}
/*! Computes an analytical surface of the form ax^2 + bx + c in a small patch
around the contact on body1. The fit is in the local body1 coordinate
system.
*/
void SoftContact::FitPoints( )
{
double *coeffs = new double [3];
FitParaboloid( bodyNghbd, numPts, coeffs );
a = coeffs[0];
b = coeffs[1];
c = coeffs[2];
RotateParaboloid( coeffs, &r1, &r2, &fitRot, &fitRotAngle );
DBGP(getBody1()->getName().latin1() << ": " << "a=" << a << " b=" << b << " c=" <<c);
DBGP("r1=" << r1 << " r2=" <<r2);
}
void
SimAnn::variableNeighbor(VariableSet *set, double T, VariableSet *target)
{
SearchVariable *var;
double v, tv = 0, conf = 0;
for (int i = 0; i < set->getNumVariables(); i++) {
var = set->getVariable(i);
if (var->isFixed()) { continue; }
v = var->mMaxVal + 1.0; //start off ilegal
int loop = 0;
while (v > var->mMaxVal || v < var->mMinVal) {
loop++;
if (!target || !target->getVariable(i)->isFixed()) {
//we have no target value; use regular sim ann neighbor distribution
v = var->getValue() + neighborDistribution(T) * var->mMaxJump;
} else {
//we have a target value and a confidence level
tv = target->getVariable(i)->getValue();
DBGP(target->getVariable(i)->getName().toStdString().c_str() << " input: " << tv);
conf = target->getVariable(i)->getConfidence();
assert(conf >= 0 && conf <= 1);
//normalize desired change to -1..1 interval relative to the max jump
double change = tv - var->getValue();
if (change > var->mMaxJump) { change = var->mMaxJump; }
else if (change < -1 * var->mMaxJump) { change = -1 * var->mMaxJump; }
change = change / var->mMaxJump;
//call the appropriate neighbor generator
DBGP(var->getName().toStdString().c_str() << " value: " << var->getValue() << " Target: " << tv << " Change: " << change);
v = var->getValue() + biasedNeighborDistribution(T, change, conf) * var->mMaxJump;
}
if (var->isCircular()) {
DBGP("Circular variable! " << var->getName().toStdString().c_str());
if (v > var->mMaxVal) { v -= var->getRange(); }
else if (v < var->mMinVal) { v += var->getRange(); }
}
if (v > var->mMaxVal && v - var->mMaxVal < TINY) { v = var->mMaxVal; }
if (v < var->mMinVal && v - var->mMinVal > -TINY) { v = var->mMinVal; }
if (loop == 100) {
DBGA("value: " << var->getValue() << " Mj: " << var->mMaxJump);
DBGA("min val: " << var->mMinVal << " max val: " << var->mMaxVal);
if (target->getVariable(i)->isFixed()) {
DBGA("Target: " << tv << "; Nbr: " << biasedNeighborDistribution(T, tv - var->getValue(), conf));
}
break;
}
}
if (loop > 10) { DBGA("Neighbor gen loops: " << loop); }
var->setValue(v);
}
}
int pwr_piapidev_readv( pwr_fd_t fd, unsigned int arraysize,
const PWR_AttrName attrs[], void *values, PWR_Time timestamp[], int status[] )
{
unsigned int i;
while( piapidev_reading ) sched_yield();
piapidev_reading = 1;
DBGP( "Info: reading counter for port %d\n", PWR_PIAPIFD(fd)->port );
if( piapi_counter( (PWR_PIAPIFD(fd)->dev)->cntx, PWR_PIAPIFD(fd)->port ) < 0 ) {
fprintf( stderr, "Error: powerinsight hardware read failed\n" );
return -1;
}
while( piapidev_reading ) sched_yield();
for( i = 0; i < arraysize; i++ ) {
switch( attrs[i] ) {
case PWR_ATTR_VOLTAGE:
*((double *)values+i) = (double)piapidev_counter.raw.volts;
break;
case PWR_ATTR_CURRENT:
*((double *)values+i) = (double)piapidev_counter.raw.amps;
break;
case PWR_ATTR_POWER:
*((double *)values+i) = (double)piapidev_counter.raw.watts;
break;
case PWR_ATTR_POWER_LIMIT_MIN:
*((double *)values+i) = (double)piapidev_counter.min.watts;
break;
case PWR_ATTR_POWER_LIMIT_MAX:
*((double *)values+i) = (double)piapidev_counter.max.watts;
break;
case PWR_ATTR_ENERGY:
*((double *)values+i) = (double)piapidev_counter.energy;
break;
default:
fprintf( stderr, "Warning: unknown PWR reading attr (%u) requested\n", attrs[i] );
break;
}
timestamp[i] = piapidev_counter.time_sec*1000000000ULL +
piapidev_counter.time_usec*1000;
DBGP( "Info: reading of type %u at time %llu with value %lf\n",
attrs[i], *((unsigned long long *)timestamp+i), *((double *)values+i) );
status[i] = pwr_piapidev_read( fd, attrs[i], (double *)values+i, sizeof(double), timestamp+i );
}
return 0;
}
/*! After this is called, the planner can no longer be re-started.
This differentiation is needed mainly for the multi-threaded case:
this function stops the planner's thread.
*/
void
EGPlanner::stopPlanner()
{
if (getState()==DONE || getState()==EXITED) return;
//this will stop the planner REGARDLESS of what state it is in!
pausePlanner();
//this also finishes the thread
setState(DONE);
if (mMultiThread) {
DBGP("Waiting for exit");
//wait for the thread to stop spinning
while (getState()!=EXITED);
DBGP("Exited");
}
}
void
CollisionInterface::compactContactSet(ContactReport *contacts)
{
if (contacts->size() < 2) return;
DBGP("Compacting total contacts: " << contacts->size());
double NORMAL_TOLERANCE = 1e-3;
double DISTANCE_TOLERANCE = 1.0e-1;
//this will hold all the groups of contacts that have the same normal
std::list<ContactReport> contactGroups;
//create lists of contacts with the same normal
ContactReport::iterator cp;
std::list<ContactReport>::iterator sp;
for (cp=contacts->begin(); cp!=contacts->end(); cp++) {
for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {
if ( sp->begin()->b1_normal % cp->b1_normal > 1.0-NORMAL_TOLERANCE) break;
}
if (sp == contactGroups.end()) {
contactGroups.push_back(ContactReport());
contactGroups.back().push_back(*cp);
} else {
// we need to make sure it's not in there already since it's a list, not a set
ContactReport::iterator it;
for (it=sp->begin(); it!=sp->end(); it++) {
vec3 dist = it->b1_pos - cp->b1_pos;
if ( dist.len_sq() < DISTANCE_TOLERANCE) break;
}
if (it==sp->end()) sp->push_back(*cp);
}
}
//take perimeter of sets with same normal
for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {
DBGP("Set with same normal: " << sp->size());
if (sp->size() > 1) replaceContactSetWithPerimeter(*sp);
DBGP("Perimeter contacts: " << sp->size());
}
//insert compacted sets back in result
contacts->clear();
for (sp=contactGroups.begin(); sp!=contactGroups.end(); sp++) {
for (cp=sp->begin();cp!=sp->end();cp++) {
contacts->push_back(*cp);
}
}
}
void
EGPlanner::render()
{
if (mMultiThread) {
//for now, multi-threaded planners are not allowed to render
//rendering should only be done by the main thread
return;
}
if (mRenderType == RENDER_BEST) {
if (mBestList.empty()) {
return;
}
if (mLastRenderState == mBestList.front()) {
return;
}
mLastRenderState = mBestList.front();
mBestList.front()->execute();
} else if (mRenderType == RENDER_LEGAL) {
if (mRenderCount >= 20) {
DBGP("Render: geom is " << mHand->getRenderGeometry());
mRenderCount = 0;
if (mCurrentState && mCurrentState->isLegal()) {
mCurrentState->execute();
}
} else {
mRenderCount++;
}
} else if (mRenderType == RENDER_ALWAYS) {
mCurrentState->execute();
} else if (mRenderType == RENDER_NEVER) {
return;
}
}
void EGPlanner::threadLoop()
{
bool done = false;
while (!done) {
PlannerState s = getState();
switch (s) {
case STARTING_THREAD: //do nothing
break;
case INIT:
sleep(0.1);
break;
case READY:
sleep(0.1);
break;
case RUNNING:
mainLoop();
break;
case DONE:
done = true;
break;
case EXITED: //Do nothing
break;
}
if (!done) {
checkTerminationConditions();
}
}
setState(EXITED);
DBGP("Thread is done!");
}