C++ DBGP函数代码示例

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 &center, 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);   
}