Beispiel #1
0
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);
}
Beispiel #3
0
void 
VariableSet::setParameter(QString name, double value)
{
	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() << " not found!");
		assert(0);
		return;
	}
	it->set(value);
}
Beispiel #4
0
double 
VariableSet::getParameter (QString name) const
{
	std::vector<SearchParameter>::const_iterator it;
	for(it = mParameters.begin(); it!=mParameters.end(); it++) {
		if ( it->name() == name ) break;
	}
	if (it==mParameters.end()) {
		DBGA("Parameter " << name.latin1() << " not found!");
		assert(0);
		return 0;
	}
	return it->get();
}
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;
  }
}
Beispiel #7
0
bool 
VariableSet::readFromArray(std::vector<double> array)
{
	if(getNumVariables() != (int) array.size())
	{
		DBGA("size does not match" << getNumVariables() << " " << array.size());
		return false;
	}
	for(int i = 0; i < getNumVariables(); i ++)
	{
		mVariables[i]->setValue(array[i]);
	}
	return true;
}
Beispiel #8
0
/*! 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;
}
Beispiel #9
0
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();
}
Beispiel #10
0
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);
}
Beispiel #13
0
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());
}
Beispiel #14
0
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;
}
Beispiel #15
0
void
EGPlanner::startPlanner()
{
	if ( getState() != READY ) {
		DBGA("Planner not ready to start!");
		return;
	}
	if (!mMultiThread) {
		mHand->showVirtualContacts(false);
		mIdleSensor = new SoIdleSensor(sensorCB, this);
		mIdleSensor->schedule();
	}
	PROF_RESET_ALL;
	PROF_START_TIMER(EG_PLANNER);
	mStartTime = clock();
	setState( RUNNING );
}
Beispiel #16
0
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);
}
Beispiel #18
0
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
}
Beispiel #19
0
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;
}
Beispiel #20
0
void
SensorInputDlg::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);
	}
}
Beispiel #21
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);
}
Beispiel #22
0
/*! If the raw glove has not yet been initialized, it initializes it.
	Also schedules the timer.
*/
void 
SensorInputDlg::gloveStartButton_clicked()
{
	if (!mGloveRunning) {
		if (!mGlove) {
			if (!initGlove()) {
				DBGA("Glove init failed");
				return;
			}
		}
		mGloveRunning = true;
		if (!mTimerSensor->isScheduled()) {
			mTimerSensor->schedule();
		}
		gloveStartButton->setText("Stop");
	} else {
		mGloveRunning = false;
		gloveStartButton->setText("Start");
	}
}
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;
}
Beispiel #24
0
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);
}
Beispiel #26
0
void
EGPlanner::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.
*/
void
VirtualContact::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;
}
Beispiel #28
0
/*! 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;
}
Beispiel #29
0
void
EigenGraspInterface::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);
}
Beispiel #30
0
/*!
  Load the object from the task record.

 */
void GraspPlanningTask::getObject(){

  //load the model
  GraspitDBModel *model = static_cast<GraspitDBModel*>(mRecord.model);
  if (model->load(graspItGUI->getIVmgr()->getWorld()) != SUCCESS) {
    //attempt repair
    DBGA("Grasp Planning Task: failed to load model");
    mStatus = ERROR;
    return;
  }
  
  //Get the object from the model
  mObject = model->getGraspableBody();
  mObject->addToIvc();
  //Add the body to the world
  graspItGUI->getIVmgr()->getWorld()->addBody(mObject);
  mObject->setMaterial(wood);
  
  //Uncomment to add table obstacle to the world
  //	Body * table = graspItGUI->getIVmgr()->getWorld()->importBody("Body", QString(getenv("GRASPIT"))+ QString("/models/obstacles/zeroplane.xml"));
  //	graspItGUI->getIVmgr()->getWorld()->addBody(table);
  return;
}