C++ DBGA函数代码示例

void
VariableSet::removeParameter(QString name)
{
	std::vector<SearchParameter>::iterator it;
	for(it = mParameters.begin(); it!=mParameters.end(); it++) {
		if ( it->name() == name ) break;
	}
	if (it==mParameters.end()) {
		DBGA("Parameter " << name.latin1() << " does not exist!");
		assert(0);
		return;
	}
	mParameters.erase(it);
}

void
GraspTester::testGrasp(GraspPlanningState *s)
{
    bool legal;
    double energy;
    //test will leave the hand in the tested state, if it is legal!
    mEnergyCalculator->analyzeState(legal, energy, s, false);
    if (!legal) {
        DBGA("Illegal state in tester thread!");
        s->setLegal(false);
        return;
    }
    s->setEnergy(energy);
}

bool GraspitDBGrasp::setHandMaterialFromDBName(Hand * h, const QString &hand_db_name){
	if(hand_db_name.contains("BARRETT")){
		//get the material index
		QStringList bstringlist = hand_db_name.split('_');
		//there must be a material after the '_' for this to be a valid name
		if (bstringlist.size() < 2) 
			return false;
		materialT mat = readMaterial(bstringlist[1].toLower().toAscii());
		if (mat == invalid)
			return false;
		DBGA("Hand Material: " <<  mat <<"\n");
		RobotTools::setHandMaterial(h, mat);
		DBGA("Currently setting palm material to wood for all barret hands");
		h->getPalm()->setMaterial(readMaterial("wood"));
			return true;
	}
	if (hand_db_name.contains("EIGENHAND")){
	    materialT mat = readMaterial("rubber");
	    RobotTools::setHandMaterial(h,mat);
	}
	//for all others, currently do nothing.
	return true;
}

void
OnLineGraspInterface::useRealBarrettHand(bool s)
{
  if (s) {
    if (!mHand->isA("Barrett")) {
      DBGA("Can't use real hand: this is not a Barrett!");
      mBarrettHand = NULL;
      return;
    }
    mBarrettHand = ((Barrett *)mHand)->getRealHand();
  } else {
    mBarrettHand = NULL;
  }
}

/*! Computes the contact jacobian J. J relates joint angle motion to 
	contact motion. Its transpose, JT, relates contact forces to joint
	forces.

	The joints and the contacts must both be passed in. Their order in 
	the incoming vectors will determine their indices in the Jacobian.
	This function will make sure that only the right joints affect each
	contact: joints that come before the link in contact, on the same
	chain.

	The Jacobian is ALWAYS computed in the local coordinate system of each
	contact. When multiplied by joint torques, it will thus yield contact
	forces and torques in the local contact coordinate system of each contact.
	It is easy to do computations in world coordinates too, but then it
	is impossible to discards rows that correspond to directions that the
	contact can not apply force or torque in.
*/
Matrix 
Grasp::contactJacobian(const std::list<Joint*> &joints, 
  					   const std::list<Contact*> &contacts)
{
	//compute the world locations of all the joints of the robot
	//this is overkill, as we might not need all of them
	std::vector< std::vector<transf> > jointTransf(hand->getNumChains());
	for (int c=0; c<hand->getNumChains(); c++) {
		jointTransf[c].resize(hand->getChain(c)->getNumJoints(), transf::IDENTITY);
		hand->getChain(c)->getJointLocations(NULL, jointTransf[c]);
	}

	Matrix J( Matrix::ZEROES<Matrix>(int(contacts.size()) * 6, (int)joints.size()) );
	std::list<Joint*>::const_iterator joint_it;
	int joint_count = 0;
	int numRows = 0;
	for (joint_it=joints.begin(); joint_it!=joints.end(); joint_it++) {
		std::list<Contact*>::const_iterator contact_it;
		numRows = 0;
		for(contact_it=contacts.begin(); contact_it!=contacts.end(); contact_it++, numRows+=6) {
			if ((*contact_it)->getBody1()->getOwner() != hand) {
				DBGA("Grasp jacobian: contact not on hand");
				continue;
			}
			Link *link = static_cast<Link*>((*contact_it)->getBody1());
			//check if the contact is on the same chain that this joint belongs too
			if ( (*joint_it)->getChainNum() != link->getChainNum() ) {
				continue;
			}
			KinematicChain *chain = hand->getChain( link->getChainNum() );
			//check that the joint comes before the contact in the chain
			int last_joint = chain->getLastJoint( link->getLinkNum() );
			//remember that the index of a joint in a chain is different from
			//the index of a joint in a robot
			int jointNumInChain = (*joint_it)->getNum() - chain->getFirstJointNum();
			assert(jointNumInChain >= 0);
			if ( jointNumInChain > last_joint) continue;
			//compute the individual jacobian
			transf joint_tran = jointTransf.at(link->getChainNum()).at(jointNumInChain);
			transf contact_tran = (*contact_it)->getContactFrame() * link->getTran();
			//always get the individual jacobian in local coordinates
			Matrix indJ(Joint::jacobian(*joint_it, joint_tran, contact_tran, false));
			//place it at the correct spot in the global jacobian
			J.copySubMatrix(numRows, joint_count, indJ);
		}
		joint_count++;
	}
	return J;
}

void 
ListPlanner::setInput(std::list<GraspPlanningState*> input)
{
	if (isActive()) {
		DBGA("Can not change input while planner is running");
		return;
	}
	while (!mInputList.empty()){
		delete mInputList.back();
		mInputList.pop_back();
	}
	mInputList = input;
	mMaxSteps = input.size();
	invalidateReset();
}

void ContactExaminerDlg::showQuality()
{
	double q;
	if (!mQual || !mGrasp) {
		q = 0.0;
	} else {
		mGrasp->update();
		DBGA("Evaluating quality");
		q = mQual->evaluate();
	}
	if (q < 0) q = 0;
	QString qs;
	qs.setNum(q);
	qs.truncate(5);
	qualityLabel->setText(qs);
}

/*! Gets a new task from the database and starts it. Possible outcomes:
  - no more tasks in database; sets status to NO_TASK
  - max number of tasks exceeded; sets status to DONE
  - error in reading the task; sets status to FAILED
  - error in starting the task; sets status to READY
  - task has started and needs us to surrender control; sets status to RUNNING
  - task is finished in one shot; sets status to READY
*/
void TaskDispatcher::startNewTask()
{
    //check if something is already running
    assert(!mCurrentTask);

    // check if we have completed the max number of tasks
    if (mMaxTasks >= 0 && mCompletedTasks >= mMaxTasks) {
        mStatus = DONE;
	return;
    }

    db_planner::TaskRecord rec;
    std::vector<std::string> empty;
    if (!mDBMgr->AcquireNextTask(&rec, empty)) {
        DBGA("Dispatcher: error reading next task");
	mStatus = FAILED;
	return;
    }
    DBGA("Task id: " << rec.taskId);
    //task type 0 is reserved for no task to do
    if (!rec.taskType.empty()) {
        DBGA("Dispatcher: no tasks to be executed");
	mStatus = NO_TASK;
	return;
    }
    mCurrentTask = mFactory.getTask(this, mDBMgr,rec);
    if (!mCurrentTask) {
        DBGA("Dispatcher: can not understand task type: " << rec.taskType);
	mStatus = FAILED;
	return;
    }

    //start the next task
    mCurrentTask->start();

    if (mCurrentTask->getStatus() == Task::RUNNING) {
        //task needs us to surrender control
        mStatus = RUNNING;
	DBGA("Dispatcher: started task of type " << rec.taskType);
    } else if (mCurrentTask->getStatus() == Task::DONE) {
        //task is done in one shot
	mStatus = READY;
	DBGA("Dispatcher: completed one-shot task of type " << rec.taskType);
    } else {
        // task had an error
        mStatus = READY;
	DBGA("Dispatcher: error starting task of type " << rec.taskType);
	return;
    }
}

/*! Simply gets the locations of all the contacts in the list and calls the
  more general version that takes in std::list< std::pair<transf, Link*> > &contact_locations */
Matrix 
Grasp::contactJacobian(const std::list<Joint*> &joints, 
                       const std::list<VirtualContact*> &contacts)
{
  std::list< std::pair<transf, Link*> > contact_locations;
  std::list<VirtualContact*>::const_iterator contact_it;
  for(contact_it=contacts.begin(); contact_it!=contacts.end(); contact_it++) {
    if ((*contact_it)->getBody1()->getOwner() != hand) {
      DBGA("Grasp jacobian: contact not on hand");
      continue;
    }
    Link *link = static_cast<Link*>((*contact_it)->getBody1());
    contact_locations.push_back( std::pair<transf, Link*>((*contact_it)->getContactFrame(), link) );
  }
  return contactJacobian(joints, contact_locations);
}

void
OnLinePlanner::createSolutionClone()
{
  if (mSolutionClone) {
    DBGA("Solution clone exists already!");
    return;
  }

  mSolutionClone = new Hand(mHand->getWorld(), "Solution clone");
  mSolutionClone->cloneFrom(mHand);
  mSolutionClone->setTransparency(0.5);
  mSolutionClone->showVirtualContacts(false);
  //solution clone is always added to scene graph
  mHand->getWorld()->addRobot(mSolutionClone, true);
  mHand->getWorld()->toggleCollisions(false, mSolutionClone);
  mSolutionClone->setTran(mHand->getTran());
}

int8 act_nsend_chk(uint8 sock, uint16 *len, uint8 *dip, uint16 *dport)
{
	uint16 availlen;

	if(sockbusy[sock] == VAL_TRUE) {
		cmd_resp(RET_BUSY, VAL_NONE);
		return RET_NOK;
	}
	if(sockstat[sock] == SOCK_STAT_IDLE) {
		cmd_resp(RET_SOCK_CLS, VAL_NONE);
		return RET_NOK;
	}

	if(sockstat[sock] & SOCK_STAT_TCP_MASK) {	// TCP
		if(!(sockstat[sock] & SOCK_STAT_CONNECTED)) {
			cmd_resp(RET_NOT_CONN, VAL_NONE);
			return RET_NOK;
		}
	} else {									// UDP
		if(dip == NULL) {
			if(udpip[sock][0]==0 && udpip[sock][1]==0 && udpip[sock][2]==0 && udpip[sock][3]==0) {
				DBG("no prev udpip");
				cmd_resp(RET_WRONG_ADDR, VAL_NONE);
				return RET_NOK;
			}
			else memcpy(dip, udpip[sock], 4);
		} else memcpy(udpip[sock], dip, 4);
		if(dport == NULL) {
			if(udpport[sock] == 0) {
				DBG("no prev udpport");
				cmd_resp(RET_WRONG_ADDR, VAL_NONE);
				return RET_NOK;
			} else *dport = udpport[sock];
		} else udpport[sock] = *dport;
	}

	availlen = GetSocketTxFreeBufferSize(sock);
	if(*len > availlen) {
		DBGA("tx buf busy - req(%d), avail(%d)", *len, availlen);
		MAKE_TCMD_DIGIT(atci.tcmd.arg1, availlen);
		cmd_resp(RET_BUSY, VAL_NONE);
		return RET_NOK;
	}

	return RET_OK;
}

int8 dhcp_alarm_start(uint8 *saved_ip)
{
	GetNetInfo(&workinfo);
	if(workinfo.DHCP > NETINFO_STATIC) {
		DBGA("Already DHCP Mode(%d)", workinfo.DHCP);
		return RET_NOK;
	} else DBG("DHCP Start");
	SET_STATE(DHCP_STATE_INIT);
	di.action = DHCP_ACT_START;

	memset(&workinfo, 0, sizeof(workinfo));
	if(saved_ip) memcpy(workinfo.IP, saved_ip, 4);
	workinfo.DHCP = NETINFO_DHCP_BUSY;
	SetNetInfo(&workinfo);
	if(dhcp_alarm) alarm_set(10, dhcp_alarm_cb, 0);
	return RET_OK;
}

void 
CompliantPlannerDlg::boxSampling(double a, double b, double c, double res)
{
	std::list<GraspPlanningState*> sampling;
	res = 30;
	sampleFace( vec3(0, 1,0), vec3(-1,0,0), vec3(0,0,1) , a, c, vec3(0,-b,0), res, &sampling);
	sampleFace( vec3(0,-1,0), vec3( 1,0,0), vec3(0,0,1) , a, c, vec3(0, b,0), res, &sampling);

	sampleFace( vec3(0,0, 1), vec3(0,1,0), vec3(-1,0,0) , b, a, vec3(0,0,-c), res, &sampling);
	sampleFace( vec3(0,0,-1), vec3(0,1,0), vec3( 1,0,0) , b, a, vec3(0,0, c), res, &sampling);

	sampleFace( vec3( 1,0,0), vec3(0, 1,0), vec3(0,0,1) , b, c, vec3(-a,0,0), res, &sampling);
	sampleFace( vec3(-1,0,0), vec3(0,-1,0), vec3(0,0,1) , b, c, vec3( a,0,0), res, &sampling);

	DBGA("Sampled " << sampling.size() << " states.");
	mNumCandidates = sampling.size();
	mPlanner->setInput(sampling);
}

void
OnLinePlanner::createSolutionClone()
{
	if(mSolutionClone) {
		DBGA("Solution clone exists already!");
		return;
	}

	mSolutionClone = new Hand(mHand->getWorld(), "Solution clone");
	mSolutionClone->cloneFrom(mRefHand);//CHANGED! was mHand - for some reason this makes setting transparency not tied to mHand??
    mSolutionClone->setTransparency(0.03);//Make the clone that shows the solutions slightly transparent so we can still see the object below it.
	mSolutionClone->showVirtualContacts(false);
	mSolutionClone->setRenderGeometry(true);
	//solution clone is always added to scene graph
	mHand->getWorld()->addRobot(mSolutionClone, true);
	mHand->getWorld()->toggleCollisions(false, mSolutionClone);
	mSolutionClone->setTran( mRefHand->getTran() );//CHANGED!  was mHand
}

transf 
SensorInputDlg::getBirdTran()
{
	if (!mPcbird->instantRead() ) {
		DBGA("Error reading Pcbird!");
		mPcbirdRunning = false;
		pcbirdStartButton->setText("Start");
		return transf::IDENTITY;
	}
	
	double r[9], t[3];
	mPcbird->getRotationMatrix(r);
	mPcbird->getPosition(t);

	transf birdTran;
	birdTran.set(mat3(r), vec3(t));
	return birdTran;
}