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);
}
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);
}
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;
}
}
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;
}
/*! 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;
}
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 );
}
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;
}
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);
}
}
/*! 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);
}
/*! 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;
}
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);
}
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;
}
/*! 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;
}
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);
}
/*!
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;
}