自学教程：C++ DBGA函数代码示例

voidVariableSet::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);}

voidGraspTester::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;}

voidOnLineGraspInterface::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);}

voidOnLinePlanner::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);}

voidOnLinePlanner::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;}

voidSensorInputDlg::resetPcbird(){	assert(mPcbird);	if (pcbirdModeBox->currentText()=="Camera") {		mPcbirdMode = PCBIRD_CAMERA;	} else if (pcbirdModeBox->currentText()=="Absolute") {		mPcbirdMode = PCBIRD_ABSOLUTE;	};	transf masterFlockTran = getBirdTran();	//for each body, set the base transform that it will be relative to	if (mPcbirdMode == PCBIRD_CAMERA) {		//everything gets rearranged relative to the camera		transf mount(mat3( vec3(0,1,0), vec3(0,0,-1), vec3(-1,0,0) ), vec3(0,0,0));		mCameraFlockTran.setMount(mount.inverse());		//the base is set based on current camera position		transf cameraBaseTran = graspItGUI->getIVmgr()->getCameraTransf();		mCameraFlockTran.setFlockBase(masterFlockTran);		mCameraFlockTran.setObjectBase(cameraBaseTran);		cameraBaseTran = mount.inverse() * cameraBaseTran;		//process the bodies		for (int i=0; i<mWorld->getNumBodies(); i++) {			if (!mWorld->getBody(i)->usesFlock() || 				mWorld->getBody(i)->getOwner() != mWorld->getBody(i) ) continue;			//each body will be re-arranged relative to the camera transform			mWorld->getBody(i)->getFlockTran()->setFlockBase( masterFlockTran );			mWorld->getBody(i)->getFlockTran()->setObjectBase( cameraBaseTran );		}		//process the robots		for (int i=0; i<mWorld->getNumRobots(); i++) {			if ( !mWorld->getRobot(i)->usesFlock() ) continue;			mWorld->getRobot(i)->getFlockTran()->setFlockBase( masterFlockTran );			mWorld->getRobot(i)->getFlockTran()->setObjectBase( cameraBaseTran );		}	} else if (mFlockMode == PCBIRD_ABSOLUTE) {		//we have nothing to do here; each body will get the bird tran in absolute terms	} else {		DBGA("Unknown Flock mode requested!");		assert(0);	}}

/*! Deletes the old connection to the database and creates a new one based	on the current settings in the dialog box. The new connection is then	set as the one an only Database Manager that the rest of GraspIt had	acces to.	After connecting, it also reads the model list form the database and	displays it.*/void DBaseDlg::connectButton_clicked(){	delete mDBMgr;	Hand *h = graspItGUI->getIVmgr()->getWorld()->getCurrentHand();	mDBMgr = new db_planner::SqlDatabaseManager(hostLineEdit->text().toStdString(),												atoi(portLineEdit->text().latin1()),												usernameLineEdit->text().toStdString(),												passwordLineEdit->text().toStdString(),												databaseLineEdit->text().toStdString(),												new GraspitDBModelAllocator(),												new GraspitDBGraspAllocator(h));	if (mDBMgr->isConnected()) {		getModelList();	} else {		DBGA("DBase Browser: Connection failed");		delete mDBMgr;		mDBMgr = NULL;	}	graspItGUI->getIVmgr()->setDBMgr(mDBMgr);}

quint32 GraspitProtobufConnection::getMessageSize(){    // Try to read the message size prefix    google::protobuf::uint32 message_length = 0;    int prefix_length = sizeof(message_length);    QByteArray prefix = sock->peek(prefix_length);    if(prefix_length == prefix.size())    {        google::protobuf::io::CodedInputStream::ReadLittleEndian32FromArray(reinterpret_cast<unsigned char *>(prefix.data()),                                                                            &message_length);        //If the message size is greater than the read buffer, fail noisily        if(message_length > maxLen)        {            DBGA("GraspitProtobufServer::Message size > socket buffer size: " << message_length);            return 0;        }    }    return message_length;}

int8 wizpf_flash_write(uint32 addr, const uint8 *data, uint16 len){	FLASH_Status ret;	uint16 word, left;	DBGA("Flash Write - Addr(0x%x),Len(%d)", addr, len);	if(addr&0x3 != 0 || len == 0) {		ERR("Address should be 4-byte aligned, Length should not be zero");		return RET_NOK;	// 4 byte alignment	}	word = len / 4;	left = len % 4;	FLASH_Unlock();	while(word--) {		ret = FLASH_ProgramWord(addr, *(uint32*)data);		if(ret != FLASH_COMPLETE) {			ERRA("Flash write(chunk) failed - ret(%d)", ret);			FLASH_Lock();			return RET_NOK;		} //else DBG("Flash write(chunk) SUCC");		addr += 4;		data += 4;	}	if(left) {		uint32 tmp = 0;		memcpy(&tmp, data, left);		ret = FLASH_ProgramWord(addr, *(uint32*)data);		if(ret != FLASH_COMPLETE) {			ERRA("Flash write(residue) failed - ret(%d)", ret);			FLASH_Lock();			return RET_NOK;		} //else DBG("Flash write(residue) SUCC");	}	FLASH_Lock();	return RET_OK;}

void loadGraspListFromFile(std::vector<GraspRecord*> *list, const char *filename){	FILE *fp = fopen(filename, "r");	if (fp==NULL) {		fprintf(stderr,"Unable to open file %s for reading/n",filename);		return;	}	GraspRecord *newGrasp;	int nGrasps;	if(fscanf(fp,"%d",&nGrasps)) { 	  DBGA("loadGraspListFromFile - failed to read number of grasps");	  return;	}	for(int i=0; i<nGrasps; i++) {		newGrasp = new GraspRecord(0);		newGrasp->readFromFile(fp);		list->push_back(newGrasp);	}	fclose(fp);}

voidEGPlanner::createAndUseClone(){    if (isActive()) {        DBGA("Can not change hands while planner is running");        return;    }    if (mMultiThread) {        //let collision detection know this is a new thread        mHand->getWorld()->getCollisionInterface()->newThread();    }    Hand *clone;    if (mHand->isA("Barrett")) {        clone = new Barrett(mHand->getWorld(), "Barrett clone");    } else if (mHand->isA("Pr2Gripper")) {        clone = new Pr2Gripper(mHand->getWorld(), "PR2 Gripper clone");    } else if (mHand->isA("RobotIQ")) {        clone = new RobotIQ(mHand->getWorld(), "RobotIQ clone");    } else {        clone = new Hand(mHand->getWorld(), "Hand clone");    }    clone->cloneFrom(mHand);    clone->setRenderGeometry(false);    clone->showVirtualContacts(false);    if (mMultiThread) {        //we do not want to add the robot to the scene graph from here        //it is ideal not to touch the scene graph from outside the main thread        mHand->getWorld()->addRobot(clone, false);    } else {        mHand->getWorld()->addRobot(clone, true);    }    mHand->getWorld()->toggleCollisions(false, mHand, clone);    clone->setTran(mHand->getTran());    mHand = clone;    mUsesClone = true;    if (mCurrentState) {        mCurrentState->changeHand(mHand);    }}

/*! Saves all the info needed for this contact (what body and link    it's on, location, normal, friction edges and coefficient) to    a file.*/voidVirtualContact::writeToFile(std::ofstream &outFile){  if (!outFile.is_open())  {    DBGA("VirtualContact::writeToFile: failed to open file");    return;  }  //finger and link number  outFile << mFingerNum << " " << mLinkNum << std::endl;  //numFrictionEdges  outFile << numFrictionEdges << std::endl;  //frictionEdges  for (int i = 0; i < numFrictionEdges; i++) {    for (int j = 0; j < 6; j++) {      outFile << frictionEdges[6 * i + j] << " ";    }    outFile << std::endl;  }  //loc  outFile << loc.x() << " " << loc.y() << " " << loc.z() << std::endl;  //frame  Quaternion q = frame.rotation();  vec3 t = frame.translation();  outFile << q.w << " " << q.x << " " << q.y << " " << q.z << std::endl;  outFile << t.x() << " " << t.y() << " " << t.z() << std::endl;  //normal  outFile << normal.x() << " " << normal.y() << " " << normal.z() << std::endl;  //sCof  outFile << sCof << std::endl;}

/*! Given a vector of EG amplitudes (a point in EG subspace) this 	function returns it's equivalent in DOF space. Essentially, it	back-projects a point from eg space to dof space.	If the interface is rigid, we add the amplitudes to the pre-specified 	eigen space origin. This means we DISCARD whatever component was in 	the pose that was not from eigenspace.		If the interface is not rigid, we add the change in amplitudes to the 	current position of the robot. This means we KEEP the position component 	that was not in eigen space.*/void EigenGraspInterface::getDOF(const double *amp, double *dof) const {	double *origin = new double[dSize];	double *rigidAmp = new double[eSize];		for (int e=0; e < eSize; e++) {		if ( !mGrasps[e]->mFixed )			rigidAmp[e] = amp[e];		else {			rigidAmp[e] = mGrasps[e]->fixedAmplitude;			DBGA(e << " fixed at " << mGrasps[e]->fixedAmplitude);		}	}	if (mRigid) {		//if the interface is rigid, we add the amplitudes to the pre-specified eigen space origin		//it means we DISCARD whatever component was in the pose that was not from eigenspace		mOrigin->getEigenGrasp(origin);		toDOFSpace(rigidAmp, dof, origin);	} else {		//if it is not, we just add the change in amplitudes to the current position of the robot		//this means we KEEP the position component that was not in eigen space		double *currentAmp = new double[eSize];		double *relativeAmp = new double[eSize];		mRobot->getDOFVals(origin);		getAmp(currentAmp, origin);		for (int e=0; e < eSize; e++) {			relativeAmp[e] = rigidAmp[e] - currentAmp[e];		}		toDOFSpace(relativeAmp, dof, origin);		delete [] currentAmp;		delete [] relativeAmp;			}	delete [] rigidAmp;	delete [] origin;}

voidEigenGraspInterface::computeProjectionMatrices(){	if (mP) delete mP;	if (mPInv) delete mPInv;	//first build the E matrix that just contains the (potentially non-orthonormal) bases	Matrix E(eSize, dSize);	for (int e=0; e<eSize; e++) {		for (int d=0; d<dSize; d++) {			E.elem(e,d) = mGrasps[e]->getAxisValue(d);		}	}	//the trivial case (assumes ortho-normal E)	//mP = new Matrix(E);	//mPInv = new Matrix(E.transposed());	//general case	//remember: P(PInv(a)) = a (always)	//		    PInv(P(x)) != x (usually)	// P = (E*ET)'*E	// P' = ET	Matrix ET(E.transposed());	Matrix EET(eSize, eSize);	matrixMultiply(E, ET, EET);	Matrix EETInv(eSize, eSize);	int result = matrixInverse(EET, EETInv);	if (result) {		DBGA("Projection matrix is rank deficient!");		mP = new Matrix(Matrix::ZEROES<Matrix>(eSize, dSize));		mPInv = new Matrix(Matrix::ZEROES<Matrix>(dSize, eSize));		return;	}	mP = new Matrix(eSize, dSize);	matrixMultiply(EETInv, E, *mP);	mPInv = new Matrix(ET);}