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);
}
}
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;
}
}
void
ListPlanner::showState(int index)
{
GraspPlanningState *state = getState(index);
if (!state) return;
state->execute();
}
OnLinePlanner * createDefaultPlanner(){
World * w = getWorld();
if(!w->getCurrentHand())
{
DBGA("plannerTools::createDefaultPlanner::No current Hand!");
return NULL;
}
GraspPlanningState *mHandObjectState = new GraspPlanningState(w->getCurrentHand());
mHandObjectState->setPositionType(SPACE_AXIS_ANGLE);
mHandObjectState->setObject(w->getGB(0));
mHandObjectState->setRefTran(w->getGB(0)->getTran());
mHandObjectState->reset();
OnLinePlanner * op = new OnLinePlanner(w->getCurrentHand());
op->setContactType(CONTACT_PRESET);
op->setEnergyType(ENERGY_CONTACT_QUALITY);
op->setMaxSteps(2000);
op->setModelState(mHandObjectState);
w->setCurrentPlanner(op);
op->showSolutionClone(true);
op->resetPlanner();
return op;
}
/*!
Responds to the updates of the planner by attempting to save the
most recently found grasps.
*/
void GraspPlanningTask::plannerLoopUpdate()
{
if (mStatus != RUNNING) return;
//save all new solutions to database
for(int i=mLastSolution; i<mPlanner->getListSize(); i++) {
//copy the solution so we can change it
GraspPlanningState *sol = new GraspPlanningState(mPlanner->getGrasp(i));
//convert it's tranform to the Quaternion__Translation format
//make sure you pass it sticky=true, otherwise information is lost in the conversion
sol->setPositionType(SPACE_COMPLETE,true);
//we will want to save exact DOF positions, not eigengrasp values
//again, make sure sticky=true
sol->setPostureType(POSE_DOF,true);
//we are ready to save it
if (!saveGrasp(sol)) {
DBGA("GraspPlanningState: failed to save solution to dbase");
mStatus = ERROR;
break;
}
}
//if something has gone wrong, stop the plan and return.
if (mStatus == ERROR) {
mPlanner->stopPlanner();
mStatus = ERROR;
} else {
DBGA(mPlanner->getListSize() - mLastSolution << " solutions saved to database");
}
mLastSolution = mPlanner->getListSize();
}
double TableCheckTask::getTableClearance(db_planner::Grasp *grasp)
{
//place the hand in position
GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(grasp)->getFinalGraspPlanningState();
graspState->execute();
//check distance for grasp
World *world = graspitCore->getWorld();
double distance = world->getDist(mHand, mTable);
if (distance < 0) {
DBGA(" Grasp is in collision with table");
return distance;
}
DBGA(" Grasp clearance: " << distance);
//check if pre-grasp is feasible
if (!computePreGrasp(grasp)) {
DBGA(" Pre-grasp is in collision with table");
return -1.0;
}
double pre_distance = world->getDist(mHand, mTable);
DBGA(" Pre-grasp clearance: " << pre_distance);
return std::min(distance, pre_distance);
}
void GraspPlanningTask::plannerLoopUpdate()
{
if (mStatus != RUNNING) return;
//save all new solutions to database
for(int i=mLastSolution; i<mPlanner->getListSize(); i++) {
//copy the solution so we can change it
GraspPlanningState *sol = new GraspPlanningState(mPlanner->getGrasp(i));
//convert it's tranform to the Quaternion__Translation format
//make sure you pass it sticky=true, otherwise information is lost in the conversion
sol->setPositionType(SPACE_COMPLETE,true);
//we will want to save exact DOF positions, not eigengrasp values
//again, make sure sticky=true
sol->setPostureType(POSE_DOF,true);
//we are ready to save it
if (!saveGrasp(sol)) {
DBGA("Grasp Planning Task: failed to save solution to dbase");
mStatus = FAILED;
break;
}
}
if (mStatus == FAILED) {
// this is a bit of a hack, but ensures that the planner will stop
// as soon as it attempts to take another step. If we specifically start
// the planner from in here, it causes problem, as this is called from inside
// the planner callback
mPlanner->setMaxSteps(0);
} else {
DBGA(mPlanner->getListSize() - mLastSolution << " solutions saved to database");
}
mLastSolution = mPlanner->getListSize();
}
bool CompliantGraspCopyTask::checkStoreGrasp(const db_planner::Grasp *original)
{
//sanity check if the world is in collision
if (!mHand->getWorld()->noCollision()) {
DBGA(" World is in collision");
return true;
}
//create the new grasp as a copy of the old one
//this should copy score and everything
const GraspitDBGrasp *graspit_original = static_cast<const GraspitDBGrasp *>(original);
std::auto_ptr<GraspitDBGrasp> newGrasp(new GraspitDBGrasp(*graspit_original));
//new grasp is a compliant copy of the old one
newGrasp->SetCompliantCopy(true);
newGrasp->SetCompliantOriginalId(original->GraspId());
//compliant copy grasps are never cluster reps
newGrasp->SetClusterRep(false);
//set the grasp posture
GraspPlanningState *graspState = new GraspPlanningState(mHand);
graspState->setPostureType(POSE_DOF, false);
graspState->setPositionType(SPACE_COMPLETE, false);
graspState->setRefTran(mObject->getTran(), false);
graspState->saveCurrentHandState();
newGrasp->setFinalGraspPlanningState(graspState);
//prepare the pre-grasp
GraspPlanningState *preGraspState = new GraspPlanningState(mHand);
preGraspState->setPostureType(POSE_DOF, false);
preGraspState->setPositionType(SPACE_COMPLETE, false);
preGraspState->setRefTran(mObject->getTran(), false);
//compute the pre-grasp posture; careful to leave the hand in the right place
mHand->saveState();
bool pre_grasp_result = computePreGrasp(newGrasp.get());
preGraspState->saveCurrentHandState();
mHand->restoreState();
//check the pre-grasp
if (!pre_grasp_result) {
DBGA(" Pre-grasp creation fails");
return true;
}
//sanity check
if (!mHand->getWorld()->noCollision()) {
DBGA(" World is in collision AFTER PREGRASP COMPUTATION");
return true;
}
//set the pre-grasp
newGrasp->setPreGraspPlanningState(preGraspState);
//compute distance to object for pre grasp
double clearance = mHand->getWorld()->getDist(mHand, mObject);
newGrasp->SetClearance(clearance);
//store the new grasp in the database
if (!mDBMgr->SaveGrasp(newGrasp.get())) {
DBGA(" Error writing new grasp to database");
return false;
}
return true;
}
bool PreGraspCheckTask::computePreGrasp(db_planner::Grasp *grasp)
{
//place the hand in position
GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(grasp)->getFinalGraspPlanningState();
graspState->execute();
//check the pre-grasp
return preGraspCheck(mHand);
}
void CompliantGraspCopyTask::start()
{
//get the details of the planning task itself
if (!mDBMgr->GetPlanningTaskRecord(mPlanningTask.taskId, &mPlanningTask)) {
DBGA("Failed to get planning record for task");
mStatus = FAILED;
return;
}
loadHand();
if (mStatus == FAILED) { return; }
if (!mHand->isA("Pr2Gripper2010")) {
DBGA("Compliant copy task only works on the PR2 gripper");
mStatus = FAILED;
return;
}
Pr2Gripper2010 *gripper = static_cast<Pr2Gripper2010 *>(mHand);
loadObject();
if (mStatus == FAILED) { return; }
//load all the grasps
std::vector<db_planner::Grasp *> graspList;
if (!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)) {
DBGA("Load grasps failed");
mStatus = FAILED;
emptyGraspList(graspList);
return;
}
bool success = true;
std::vector<db_planner::Grasp *>::iterator it;
for (it = graspList.begin(); it != graspList.end(); it++) {
GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(*it)->getFinalGraspPlanningState();
gripper->setCompliance(Pr2Gripper2010::NONE);
graspState->execute();
DBGA("Compliant copy around finger 0");
if (!compliantCopy(*it, Pr2Gripper2010::FINGER0)) {
success = false;
break;
}
gripper->setCompliance(Pr2Gripper2010::NONE);
graspState->execute();
DBGA("Compliant copy around finger 1");
if (!compliantCopy(*it, Pr2Gripper2010::FINGER1)) {
success = false;
break;
}
}
gripper->setCompliance(Pr2Gripper2010::NONE);
emptyGraspList(graspList);
if (success) { mStatus = DONE; }
else { mStatus = FAILED; }
}
GraspPlanningState *SimAnn::stateNeighbor(GraspPlanningState *s, double T, GraspPlanningState *t)
{
GraspPlanningState *sn = new GraspPlanningState(s);
if (t) {
variableNeighbor(sn->getPosition(), T, t->getPosition());
variableNeighbor(sn->getPosture(), T, t->getPosture());
} else {
variableNeighbor(sn->getPosition(), T, NULL);
variableNeighbor(sn->getPosture(), T, NULL);
}
return sn;
}
/**
* @function sampleBox_valueChanged
*/
void CanonicalPlannerDlg::sampleBox_valueChanged(int _j ) {
if( mPlanner == NULL ) {
std::cout << "No planner yet. Load base grasps first."<< std::endl;
return;
}
sampleBox->setRange(0, mPlanner->getNumSampleGrasps(baseBox->value()) - 1 );
mPlanner->showSampleGrasp( baseBox->value(), _j );
GraspPlanningState * gps = mPlanner->getSampleGrasp( baseBox->value(), _j );
double energy = gps->getEnergy();
int iter = gps->getItNumber();
std::cout << " Grasp ["<<baseBox->value()<<","<<_j<<"]: Energy: "<< energy<<" and iter: "<< iter <<std::endl;
}
/*! The main planning function. Simply takes the next input grasp from the list,
tries it and then places is in the output depending on the quality.
*/
void
ListPlanner::mainLoop()
{
//check if we are done
if (mPlanningIterator==mInputList.end()) {
mCurrentStep = mMaxSteps+1;
return;
}
//analyze the current state
//we don't allow it to leave the hand in the analysis posture
//so that after dynamics object gets put back
bool legal; double energy;
PRINT_STAT(mOut, mCurrentStep);
mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, true);
//for rendering purposes; will see later if it's needed
mCurrentState->copyFrom(*mPlanningIterator);
mCurrentState->setLegal(legal);
//put a copy of the result in list if it's legal and there's room or it's
//better than the worst solution so far
//this whole thing could go into a higher level fctn in EGPlanner
if (legal) {
double worstEnergy;
if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5;
else worstEnergy = mBestList.back()->getEnergy();
if (energy < worstEnergy) {
GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator);
insertState->setEnergy(energy);
insertState->setItNumber(mCurrentStep);
DBGP("Solution at step " << mCurrentStep);
mBestList.push_back(insertState);
mBestList.sort(GraspPlanningState::compareStates);
while ((int)mBestList.size() > BEST_LIST_SIZE) {
delete(mBestList.back());
mBestList.pop_back();
}
}
}
//advance the planning iterator
mPlanningIterator++;
mCurrentStep++;
emit update();
PRINT_STAT(mOut, std::endl);
}
void AutoGraspGenerationDlg::updateInputLayout()
{
int i;
for (i=0; i<mHandObjectState->getNumVariables(); i++) {
if ( !mPlanner || !(mPlanner->isReady() || mPlanner->isActive()) ) {
varInput[i]->setEnabled(FALSE);
varInput[i]->setChecked(false);
varTarget[i]->setText("N/A");
varTarget[i]->setEnabled(FALSE);
varConfidence[i]->setValue( 0 );
varConfidence[i]->setEnabled(FALSE);
} else {
GraspPlanningState *t = mPlanner->getTargetState();
varInput[i]->setEnabled(TRUE);
QString n;
n.setNum(t->getVariable(i)->getValue(),'f',3);
varTarget[i]->setText(n);
varConfidence[i]->setValue( t->getVariable(i)->getConfidence() * 100 );
if ( t->getVariable(i)->isFixed() ) {
varInput[i]->setChecked(TRUE);
varTarget[i]->setEnabled(TRUE);
varConfidence[i]->setEnabled(TRUE);
} else {
varInput[i]->setChecked(FALSE);
varTarget[i]->setEnabled(FALSE);
varConfidence[i]->setEnabled(FALSE);
}
if (mHandObjectState->getVariable(i)->getName() == "Tx" || mHandObjectState->getVariable(i)->getName() == "Ty") {
varInput[i]->setChecked(true);
varConfidence[i]->setEnabled(true);
varConfidence[i]->setValue(70);
mHandObjectState->getVariable(i)->setConfidence(.70);
}
}
}
for (i=mHandObjectState->getNumVariables(); i < mHand->getEigenGrasps()->getSize() + 7; i++)
{
varInput[i]->setEnabled(FALSE);
varInput[i]->setChecked(false);
varTarget[i]->setText("N/A");
varTarget[i]->setEnabled(FALSE);
varConfidence[i]->setValue( 0 );
varConfidence[i]->setEnabled(FALSE);
}
}
void
OnLinePlanner::graspLoop()
{
//DBGP("Grasp loop started");
//prepare input
GraspPlanningState *input = NULL;
if ( processInput() ) {
input = mTargetState;
}
//call simulated annealing
SimAnn::Result r = mSimAnn->iterate(mCurrentState,mEnergyCalculator,input);
mCurrentStep = mSimAnn->getCurrentStep();
if ( r == SimAnn::JUMP ) {
assert(mCurrentState->isLegal());
//we have a new state from the SimAnn
if (mCurrentState->getEnergy() < 0 || mCurrentState->getEnergy() < mCurrentBest->getEnergy()) {
DBGP("New candidate");
GraspPlanningState *insertState = new GraspPlanningState(mCurrentState);
//make solution independent of reference hand position
insertState->setPositionType(SPACE_COMPLETE,true);
insertState->setRefTran( mCurrentState->getObject()->getTran(), true);
insertState->setItNumber( mCurrentStep );
if (insertState->getEnergy() < mCurrentBest->getEnergy()) {
mCurrentBest->copyFrom( insertState );
}
if (!addToListOfUniqueSolutions(insertState, &mCandidateList,0.4)) {
DBGP("Similar to old candidate");
delete insertState;
} else {
//graspItGUI->getIVmgr()->emitAnalyzeGrasp(mCandidateList.back());
mCandidateList.sort(GraspPlanningState::compareStates);//CHANGED! was compareStates
while (mCandidateList.size() > CANDIDATE_BUFFER_SIZE) {
delete mCandidateList.back();
mCandidateList.pop_back();
}
}
DBGP("Added candidate");
}
}
if (mCurrentStep % 100 == 0) emit update();
render();
//DBGP("Grasp loop done");
}
void
CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z,
double sz1, double sz2, vec3 tln, double res,
std::list<GraspPlanningState*> *sampling)
{
mat3 R(x, y, z);
int rotSamples=2;
double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0;
while (m1 < 2*sz1){
double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0;
while (m2 < 2*sz2) {
vec3 myTln(tln);
myTln = myTln + (m1 - sz1)* y;
myTln = myTln + (m2 - sz2)* z;
transf tr(R, myTln);
for(int rot=0; rot < rotSamples; rot++) {
double angle = M_PI * ((double)rot) / rotSamples;
transf rotTran(Quaternion(angle, vec3(1,0,0)), vec3(0,0,0));
tr = rotTran * tr;
GraspPlanningState* seed = new GraspPlanningState(mHand);
seed->setObject(mObject);
seed->setRefTran(mObject->getTran(), false);
seed->setPostureType(POSE_DOF, false);
seed->setPositionType(SPACE_COMPLETE, false);
seed->reset();
seed->getPosition()->setTran(tr);
sampling->push_back(seed);
}
m2+=res;
}
m1 += res;
}
}
/*! Does the usual main loop of a SimAnn planner, but checks if the current
state is good enough to be placed in the list of seeds to be used for
children. The list of seeds is also pruned to remove similar state,
and only keep a list of "unique" seeds.
*/
void
GuidedPlanner::mainLoop()
{
// call main simann iteration
SimAnn::Result r = mSimAnn->iterate(mCurrentState, mEnergyCalculator);
if (r==SimAnn::FAIL) return;
//put result in list
double bestEnergy;
if ((int)mChildSeeds.size() < mChildSeedSize) {
//only queue good states to begin with
bestEnergy = mMinChildEnergy;
} else {
bestEnergy = mChildSeeds.back()->getEnergy();
}
if (r==SimAnn::JUMP && mCurrentState->getEnergy() < bestEnergy) {
GraspPlanningState *insertState = new GraspPlanningState(mCurrentState);
DBGP("New solution. Is it a candidate?");
if (!addToListOfUniqueSolutions(insertState,&mChildSeeds,mDistanceThreshold)) {
DBGP("No.");
delete insertState;
} else {
DBGP("Yes");
//place a visual marker in the world
mHand->getWorld()->getIVRoot()->addChild( insertState->getIVRoot() );
mChildSeeds.sort(GraspPlanningState::compareStates);
DBGP("Queued...");
while ((int)mChildSeeds.size() > mChildSeedSize) {
delete(mChildSeeds.back());
mChildSeeds.pop_back();
}
DBGP("Done.");
}
}
mCurrentStep = mSimAnn->getCurrentStep();
render();
if (mCurrentStep % 100 == 0) {
emit update();
checkChildren();
}
}
void AutoGraspGenerationDlg::variableInputChanged()
{
assert(mPlanner);
GraspPlanningState *t = mPlanner->getTargetState();
assert(t);
for (int i=0; i<mHandObjectState->getNumVariables(); i++) {
if (varInput[i]->isChecked()) {
varTarget[i]->setEnabled(TRUE);
varConfidence[i]->setEnabled(TRUE);
t->getVariable(i)->setFixed(true);
t->getVariable(i)->setConfidence( ((double)varConfidence[i]->value()) / 100.0);
DBGP("Conf: " << ((double)varConfidence[i]->value()) / 100.0);
}
else {
varTarget[i]->setEnabled(FALSE);
t->getVariable(i)->setFixed(false);
t->getVariable(i)->setConfidence(0);
varConfidence[i]->setValue(0);
varConfidence[i]->setEnabled(FALSE);
}
}
}
void DBaseDlg::sortButton_clicked()
{
if (mGraspList.empty()) return;
VisualQualityFunctor func;
if (sortBox->currentText()== "Energy") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareEnergy);
func.mType = VisualQualityFunctor::ENERGY;
} else if (sortBox->currentText()== "Epsilon") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareEpsilon);
func.mType = VisualQualityFunctor::EPSILON;
} else if (sortBox->currentText()== "Volume") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareVolume);
func.mType = VisualQualityFunctor::VOLUME;
} else {
assert(0);
}
mCurrentFrame = 0;
showGrasp(mCurrentFrame);
if (showMarkersBox->isChecked()) {
double min = func(mGraspList.front());
double max = func(mGraspList.back());
if (max==min) max=min+1.0;
std::vector<db_planner::Grasp*>::iterator it;
for (it=mGraspList.begin(); it!=mGraspList.end(); it++) {
double r = (func(*it) - min) / (max - min);
double g = 1-r;
double b = 0.0;
GraspPlanningState *state;
if(showPreGraspRadioButton->isChecked()) {
state = static_cast<GraspitDBGrasp*>(*it)->getPreGraspPlanningState();
} else {
state = static_cast<GraspitDBGrasp*>(*it)->getFinalGraspPlanningState();
}
state->setIVMarkerColor(r,g,b);
}
}
}
void importGraspsFromDBMgr( OnLinePlanner * mPlanner, db_planner::DatabaseManager * mDbMgr)
{
Hand*mHand = mPlanner->getHand();
// Get corresponding model from database
std::vector<db_planner::Model*> modelList;
QString *objectFilename = new QString('/' + mHand->getGrasp()->getObject()->getFilename().split('/').back());
mDbMgr->ModelList(&modelList,db_planner::FilterList::USE_FILE_NAME, *objectFilename);
if(modelList.empty())
{
DBGA("No Models Found \n");
return;
}
// Using the found model, retrieve the grasps
std::vector<db_planner::Grasp*> grasps;
mDbMgr->GetGrasps(*modelList.back(), GraspitDBGrasp::getHandDBName(mHand).toStdString(), &grasps);
mHand->saveState();
bool collisionState = getWorld()->collisionsAreOff(mPlanner->getHand());
getWorld()->toggleCollisions(true, mPlanner->getHand());
bool targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
// Load the grasps into the grasp planner list.
unsigned int numGrasps = std::min<unsigned int>(grasps.size(), 10);
for (unsigned int gNum = 0; gNum < numGrasps; ++gNum)
{
GraspPlanningState *s = new GraspPlanningState(static_cast<GraspitDBGrasp *>
(grasps[gNum])->getFinalGraspPlanningState());
s->setObject(mHand->getGrasp()->getObject());
s->setRefTran(mHand->getGrasp()->getObject()->getTran(), false);
targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
float testResult = -2*bci_experiment::planner_tools::testGraspCollisions(mHand, s);
while(targetsOff)
{
getWorld()->toggleCollisions(true, mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
}
s->addAttribute("graspId", gNum);
s->addAttribute("testResult", testResult);
s->addAttribute("testTime", 0);
mPlanner->addSolution(s);
}
//reorders the solutions we have found.
mPlanner->updateSolutionList();
targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
//needed to return hand to aligned with object, since it was used to testGraspCollisions
getWorld()->toggleCollisions(!collisionState, mPlanner->getHand());
mHand->restoreState();
targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject());
}
/*! Stops a child, and waits for the child thread to finish. After that,
it retrieves the solutions found by the child, and places them in
the list of solutions and also in the list of states to be avoided in
the future. Solutions are also marked visually.
Note that, for each solution recovered from a child, this function also
reconstructs the final grasp (remember that solutions are usually
pre-grasps) and saves BOTH in the list of solutions.
*/
void
GuidedPlanner::stopChild(SimAnnPlanner *pl)
{
DBGA("Child has finished!");
//this sets the state to DONE which in turn waits for the thread to stop spinning
pl->stopPlanner();
DBGP("Thread has stopped.");
int j = pl->getListSize();
if (j) {
//place a copy of the best state (which is the pre-grasp) in my best list
GraspPlanningState *s = new GraspPlanningState( pl->getGrasp(0) );
s->printState();
//recall that child used its own cloned hand for planning
s->changeHand(mHand, true);
mBestList.push_back(s);
//use this part to also save the final grasp from the child planner,
//not just the pre-grasp
pl->showGrasp(0);
GraspPlanningState *finalGrasp = new GraspPlanningState(pl->getGrasp(0));
finalGrasp->setPositionType(SPACE_COMPLETE);
//the final grasp is not in eigengrasp space, so we must save it in DOF space
finalGrasp->setPostureType(POSE_DOF);
finalGrasp->saveCurrentHandState();
//change the hand
finalGrasp->changeHand(mHand, true);
//and save it
mBestList.push_back(finalGrasp);
//place a copy of the pre-grasp in the avoid list and mark it red
GraspPlanningState *s2 = new GraspPlanningState(s);
mAvoidList.push_back(s2);
mHand->getWorld()->getIVRoot()->addChild( s2->getIVRoot() );
if (s2->getEnergy() < 10.0) {
s2->setIVMarkerColor(1,0,0);
} else {
s2->setIVMarkerColor(0.1,0.1,0.1);
}
DBGA("Enrgy from child: " << s2->getEnergy());
//the avoid list gets emptied at cleanup
}
else
{
DBGA("Child has no solutions");
}
}
/*! Samples an ellipsoid by sampling uniformly a grid with the same aspect
ratio and projecting the resulting points on the ellipsoid. Not ideal,
but at least much better then sampling angular variables directly */
void
CompliantPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed,
std::list<GraspPlanningState*> *sampling,
int samples)
{
double a = seed.readPosition()->getParameter("a");
double aRes = 2.0 * a / samples;
double b = seed.readPosition()->getParameter("b");
double bRes = 2.0 * b / samples;
double c = seed.readPosition()->getParameter("c");
double cRes = 2.0 * c / samples;
DBGP("a: " << a << "; b: " << b <<"; c: " << c);
for (double i=0.5; i<samples; i+=1.0) {
for(double j=0.5; j<samples; j+=1.0) {
addCartesianSamples(seed, sampling, samples, a, -b+i*bRes, -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a, -b+i*bRes, -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, b , -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes,-b , -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes , c);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes ,-c);
}
}
}
/*! The caller passes it a HandObjectState and a calculator that can be used
to compute the quality (or in annealing terms "energy") of a HandObjectState.
This function computes the next state in the annealing schedule.
See SimAnn::Result declaration for possible return values.
*/
SimAnn::Result
SimAnn::iterate(GraspPlanningState *currentState, SearchEnergy *energyCalculator, GraspPlanningState *targetState)
{
//using different cooling constants for probs and neighbors
double T = cooling(mT0, mParams.YC, mCurrentStep, mParams.YDIMS);
//attempt to compute a neighbor of the current state
GraspPlanningState *newState;
double energy; bool legal = false;
int attempts = 0; int maxAttempts = 10;
DBGP("Ngbr gen loop");
while (!legal && attempts <= maxAttempts) {
newState = stateNeighbor(currentState, T * mParams.NBR_ADJ, targetState);
DBGP("Analyze state...");
energyCalculator->analyzeState(legal, energy, newState);
DBGP("Analysis done.");
if (!legal) { delete newState; }
attempts++;
}
if (!legal) {
DBGP("Failed to compute a legal neighbor");
//we have failed to compute a legal neighbor.
//weather the SimAnn should advance a step and cool down the temperature even when it fails to compute a
//legal neighbor is debatable. Might be more interactive (especially for the online planner) if it does.
//mCurrentStep += 1;
return FAIL;
}
//we have a neighbor. Now we decide if we jump to it.
DBGP("Legal neighbor computed; energy: " << energy)
newState->setEnergy(energy);
newState->setLegal(true);
newState->setItNumber(mCurrentStep);
//using different cooling constants for probs and neighbors
T = cooling(mT0, mParams.HC, mCurrentStep, mParams.HDIMS);
double P = prob(mParams.ERR_ADJ * currentState->getEnergy(), mParams.ERR_ADJ * newState->getEnergy(), T);
double U = ((double)rand()) / RAND_MAX;
Result r = KEEP;
if (P > U) {
DBGP("Jump performed");
currentState->copyFrom(newState);
r = JUMP;
} else {
DBGP("Jump rejected");
}
mCurrentStep += 1; mTotalSteps += 1;
DBGP("Main iteration done.")
delete newState;
if (mWriteResults && mCurrentStep % 2 == 0) {
assert(mFile);
fprintf(mFile, "%ld %d %f %f %f %f\n", mCurrentStep, mTotalSteps, T, currentState->getEnergy(),
currentState->readPosition()->readVariable("Tx"),
targetState->readPosition()->readVariable("Tx"));
//currentState->writeToFile(mFile);
}
return r;
}
void GraspPlanningTask::start()
{
//get the details of the planning task itself
if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) {
DBGA("Failed to get planning record for task");
mStatus = FAILED;
return;
}
World *world = graspitCore->getWorld();
//check if the currently selected hand is the same as the one we need
//if not, load the hand we need
if (world->getCurrentHand() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) {
DBGA("Grasp Planning Task: using currently loaded hand");
mHand = world->getCurrentHand();
} else {
QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str()));
handPath = QString(getenv("GRASPIT")) + handPath;
DBGA("Grasp Planning Task: loading hand from " << handPath.latin1());
mHand = static_cast<Hand*>(world->importRobot(handPath));
if ( !mHand ) {
DBGA("Failed to load hand");
mStatus = FAILED;
return;
}
}
//check for virtual contacts
if (mHand->getNumVirtualContacts()==0) {
DBGA("Specified hand does not have virtual contacts defined");
mStatus = FAILED;
return;
}
//load the object
GraspitDBModel *model = static_cast<GraspitDBModel*>(mPlanningTask.model);
if (model->load(world) != SUCCESS) {
DBGA("Grasp Planning Task: failed to load model");
mStatus = FAILED;
return;
}
mObject = model->getGraspableBody();
mObject->addToIvc();
world->addBody(mObject);
//initialize the planner
GraspPlanningState seed(mHand);
seed.setObject(mObject);
seed.setPositionType(SPACE_AXIS_ANGLE);
seed.setPostureType(POSE_EIGEN);
seed.setRefTran(mObject->getTran());
seed.reset();
mPlanner = new LoopPlanner(mHand);
QObject::connect(mPlanner, SIGNAL(loopUpdate()), this, SLOT(plannerLoopUpdate()));
QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));
mPlanner->setEnergyType(ENERGY_CONTACT);
mPlanner->setContactType(CONTACT_PRESET);
mPlanner->setMaxSteps(65000);
mPlanner->setRepeat(true);
//max time set from database record
if (mPlanningTask.taskTime >= 0){
mPlanner->setMaxTime(mPlanningTask.taskTime);
} else {
mPlanner->setMaxTime(-1);
}
static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed);
if (!mPlanner->resetPlanner()) {
DBGA("Grasp Planning Task: failed to reset planner");
mStatus = FAILED;
return ;
}
//load all already known grasps so that we avoid them in current searches
mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(mHand));
std::vector<db_planner::Grasp*> graspList;
if(!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)){
//for now, we don't know if this means "no grasps found" or "error" so we assume the first
DBGA("No grasps found in database for model " << mPlanningTask.model->ModelName());
}
//and pass them on to the planner
for (size_t i=0; i<graspList.size(); i++) {
GraspPlanningState *state = new GraspPlanningState(static_cast<GraspitDBGrasp*>(graspList[i])
->getFinalGraspPlanningState() );
state->setObject(mObject);
state->setPositionType(SPACE_AXIS_ANGLE, true);
//careful here - is it the same posture in both spaces?
state->setPostureType(POSE_EIGEN, true);
static_cast<LoopPlanner*>(mPlanner)->addToAvoidList(state);
}
while (!graspList.empty()) {
delete graspList.back();
graspList.pop_back();
}
mLastSolution = mPlanner->getListSize();
DBGA("Planner starting off with " << mLastSolution << " solutions");
mPlanner->startPlanner();
mStatus = RUNNING;
}
bool PreGraspCheckTask::checkSetGrasp(db_planner::Grasp *grasp)
{
if (!computePreGrasp(grasp)) {
DBGA("Pre-grasp creation fails");
//delete from database
if (!mDBMgr->DeleteGrasp(grasp)) {
DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database");
return false;
}
return true;
}
//sanity check
if (!mHand->getWorld()->noCollision()) {
DBGA("Collision detected for pre-grasp!");
if (!mDBMgr->DeleteGrasp(grasp)) {
DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database");
return false;
}
return true;
}
//compute distance to object
double clearance = mHand->getWorld()->getDist(mHand, mObject);
grasp->SetClearance(clearance);
//create pre-grasp
GraspPlanningState *newPreGrasp = new GraspPlanningState(mHand);
newPreGrasp->setPostureType(POSE_DOF, false);
newPreGrasp->setPositionType(SPACE_COMPLETE, false);
newPreGrasp->setRefTran(mObject->getTran(), false);
newPreGrasp->saveCurrentHandState();
//set pre-grasp
static_cast<GraspitDBGrasp *>(grasp)->setPreGraspPlanningState(newPreGrasp);
//save pre-grasp along with clearance in database
//for now, we update by deleting and re-inserting
if (!mDBMgr->DeleteGrasp(grasp)) {
DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database");
return false;
}
/*
//move the grasp back by 42mm
transf t(Quaternion::IDENTITY, vec3(-42, 0, 0));
GraspitDBGrasp* dbg = static_cast<GraspitDBGrasp*>(grasp);
GraspPlanningState *movedPreGrasp = new GraspPlanningState(dbg->getPreGraspPlanningState());
movedPreGrasp->getPosition()->setTran( t * movedPreGrasp->getPosition()->getCoreTran() );
dbg->setPreGraspPlanningState(movedPreGrasp);
GraspPlanningState *movedFinalGrasp = new GraspPlanningState(dbg->getFinalGraspPlanningState());
movedFinalGrasp->getPosition()->setTran( t * movedFinalGrasp->getPosition()->getCoreTran() );
dbg->setFinalGraspPlanningState(movedFinalGrasp);
*/
//and save to database
if (!mDBMgr->SaveGrasp(grasp)) {
DBGA("Failed to save new grasp to database");
return false;
}
DBGA("Pre-grasp inserted");
return true;
}
void DBaseBatchPlanner::processSolution(const GraspPlanningState *s)
{
//we will write this solution to the result file
//if it's a poor solution don't even bother
if (s->getEnergy() > energyConstraint){
DBGAF(mLogStream,"Solution with energy to be thrown: " << s->getEnergy());
return;
}
//we need a SearchEnergy calculator in order to do the autoGrasp in the exact same way that the planner does it
static SearchEnergy *se = NULL; //don't create it each time
if (!se) {
//this is the same type used by the loop planner
switch(mType) {
case DEXTEROUS:
se = SearchEnergy::getSearchEnergy(ENERGY_STRICT_AUTOGRASP);
break;
case GRIPPER:
se = SearchEnergy::getSearchEnergy(ENERGY_CONTACT);
se->setContactType(CONTACT_PRESET);
break;
}
}
DBGAF(mLogStream,"Solution with energy: " << s->getEnergy());
//first, copy it to a new one so it's not const and we can modify it
GraspPlanningState *sol = new GraspPlanningState(s);
//convert it's tranform to the Quaternion__Translation format
//make sure you pass it sticky=true, otherwise information is lost in the conversion
sol->setPositionType(SPACE_COMPLETE,true);
//we will want to save exact DOF positions, not eigengrasp values
//again, make sure sticky=true
sol->setPostureType(POSE_DOF,true);
//we can write it to a file
fprintf(mResultFile,"pre-grasp\n");
sol->writeToFile(mResultFile);
//at this point, we are saving each DOF individually, but it should be an eigengrasp posture
if (mType == DEXTEROUS) {
//now close the fingers and perform the autograsp
bool legal; double energy;
//make sure to pass it noChange = false and it will leave the hand in the posture AFTER the autograsp
//otherwise, analyzeState will revert the state to what it was on its entry
se->analyzeState(legal,energy,sol,false);
if (!legal) {
DBGAF(mLogStream,"buru Illegal solution! This should not be!");
}
//store the hand posture resulting from the autograsp
//careful: this only works if sol is of the type SPACE_COMPLETE (as set above)
sol->saveCurrentHandState();
//write this one to the file as well
fprintf(mResultFile,"grasp\n");
sol->writeToFile(mResultFile);
//this posture is probably not in eigengrasp space
//write the contacts to a file
fprintf(mResultFile,"contacts\n");
writeContactsToFile(sol->getHand(), sol->getObject());
}
numOfGrasps++;
printf("\nup to now, %d out of %d grasps have been found\n",numOfGrasps, numOfGraspsGoal);
//add some codes here to limit the grasps
if(numOfGrasps == numOfGraspsGoal)
{
mMaxTime = 10;// this is a trick, maybe not good
printf("begin to leave...");
DBGAF(mLogStream, numOfGraspsGoal);
mPlanner->setMaxTime(mMaxTime);
}
//we are done
delete sol;
}
/*! Every grasp in graspList will be tested and stored in testedGraspList
even if their test scores, epsilon quality and volume quality, are
not positive, which means they are not stable grasps.
*/
bool GraspitDBPlanner::testGrasps(TestType t,
std::vector<db_planner::Grasp *> graspList,
std::vector<db_planner::Grasp *> *testedGraspList) {
mTestedGrasps.clear();
if (testedGraspList) {
testedGraspList->clear();
}
// test each of the grasps in graspList
for (int i = 0; i < ((int)graspList.size()); i ++)
{
GraspitDBGrasp *tempGrasp = new GraspitDBGrasp(*static_cast<GraspitDBGrasp *>(graspList[i]));
// load this grasp into the world
static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->setTran(transf::IDENTITY);
GraspPlanningState *tempState = new GraspPlanningState(tempGrasp->getPreGraspPlanningState());
tempState->setRefTran(transf::IDENTITY);
tempGrasp->setPreGraspPlanningState(tempState);
float elmts[16];
if (mAligner->Align(tempGrasp->SourceModel(), *mObject, elmts)) {
tempGrasp->Transform(elmts);
}
mHand->getGrasp()->setObject(static_cast<GraspitDBModel *>(mObject)->getGraspableBody());
// apply it to the hand
tempGrasp->getPreGraspPlanningState()->execute();
bool testResult;
DynamicCode dynCode;
// test this grasp
if (t == STATIC) {
testResult = testGraspStatic();
dynCode = NO_DYNAMICS;
} else if (t == DYNAMIC) {
testResult = testGraspDynamic(&dynCode);
//DBGA("Result: " << testResult << "; dynamics code: " << dynCode);
}
float eq = -1.0, vq;
// decide whether to record
if (testResult) {
computeQuality(eq, vq);
if (!static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getNumContacts()) {
dynCode = DYNAMIC_OBJECT_EJECTED;
} else if (eq <= 0.0) {
dynCode = DYNAMIC_NO_FC;
}
} else { return false; }
if (eq < 0) { continue; }
//we now save all grasps in mTestedGrasps
GraspPlanningState *newGrasp = new GraspPlanningState(mHand);
newGrasp->setPositionType(SPACE_COMPLETE, false);
newGrasp->setPostureType(POSE_DOF, false);
//it is possible the object moved after dynamics, so we need to re-set the reference posture
newGrasp->setRefTran(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getTran());
newGrasp->saveCurrentHandState();
newGrasp->setEpsilonQuality(eq);
newGrasp->setVolume(vq);
newGrasp->setIndex(i);
//this is a hack; just gives us a field to save the dynamics code
newGrasp->setDistance((double)dynCode);
mTestedGrasps.push_back(newGrasp);
//std::cout << "grasp tested, quality: " << eq << std::endl;
if (!testedGraspList) {
continue;
}
// record this to synthesize the output
db_planner::Grasp *recordGrasp = static_cast<db_planner::Grasp *>(new GraspitDBGrasp(*tempGrasp));
tempState = new GraspPlanningState(static_cast<GraspitDBGrasp *>(recordGrasp)->getPreGraspPlanningState());
tempState->copyFrom(newGrasp);
static_cast<GraspitDBGrasp *>(recordGrasp)->setPreGraspPlanningState(tempState);
testedGraspList->push_back(recordGrasp);
delete tempGrasp;
}
if (mTestedGrasps.size() != 0) {
return true;
}
return false;
}
void
OnLinePlanner::mainLoop()
{
static clock_t lastCheck = clock();
clock_t time = clock();
double secs = (float)(time - lastCheck) / CLOCKS_PER_SEC;
if (secs < 0.2) {
//perform grasp planning all the time
graspLoop();
return;
}
lastCheck = time;
//every 0.2 seconds, perform the management part:
//set as a reference transform for the search the transform of the reference hand (presumably controlled by
//the user via a flock of birds)
mCurrentState->setRefTran( mRefHand->getTran(), false );
//this is to ensure this (potentially) illegal state does not make it into the best list
mCurrentState->setLegal(false);
//re-set the legal search range along the approach direction, so we don't search pointlessly inside the object
if ( mCurrentState->getVariable("dist")) {
Body *obj = mCurrentState->getObject();
double maxDist = 200;
mObjectDistance = mRefHand->getApproachDistance(obj,maxDist);
if (mObjectDistance > maxDist) mObjectDistance = maxDist;
mCurrentState->getPosition()->getVariable("dist")->setRange(-30,mObjectDistance);
//make sure the current value is within range; otherwise simm ann can hang...
mCurrentState->getPosition()->getVariable("dist")->setValue(mObjectDistance/2);
mCurrentState->getPosition()->getVariable("dist")->setJump(0.33);
}
//is the planning part has produced new candidates, send them to the grasp tester
std::list<GraspPlanningState*>::iterator it = mCandidateList.begin();
while (it!=mCandidateList.end()) {
//while there is space
if ( mGraspTester->postCandidate(*it) ) {
DBGP("Candidate posted");
it = mCandidateList.erase(it);
} else {
DBGP("Tester thread buffer is full");
break;
}
}
//retrieve solutions from the tester
GraspPlanningState *s;
while ( (s = mGraspTester->popSolution()) != NULL ) {
//hack - this is not ideal, but so far I don't have a better solution of how to keep track
//of what hand is being used at what time
s->changeHand( mRefHand, true );//CHANGED! to mSolutionClone from mRefHand
mBestList.push_back(s);
//graspItGUI->getIVmgr()->emitAnalyzeGrasp(s);
if (mMarkSolutions) {
mHand->getWorld()->getIVRoot()->addChild( s->getIVRoot() );
}
}
updateSolutionList();
//now shape the real hand.
//s = mInterface->updateHand(&mBestList);//this used to update the current hand to solutions that are found, no longer does this
// if (mBestList.size() != 0) showGrasp(0);//CHANGED! this replaces line above
//CHANGED! stuff below was taken out
// if (s) {
// if (mSolutionClone) s->execute(mSolutionClone);
// if (mMarkSolutions) s->setIVMarkerColor(1,1,0);
// }
DBGP("On-line main loop done");
}
bool GuidedGraspPlanningTask::saveGrasp(const GraspPlanningState *pre_gps, const GraspPlanningState *final_gps)
{
GraspitDBModel* dbModel= mObject->getDBModel();
assert(dbModel);
db_planner::Grasp* grasp = new db_planner::Grasp;
std::vector<double> contacts = grasp->GetContacts();
grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) );
grasp->SetHandName(GraspitDBGrasp::getHandDBName(mHand).toStdString());
grasp->SetEpsilonQuality(final_gps->getEpsilonQuality());
grasp->SetVolumeQuality(final_gps->getVolume());
grasp->SetEnergy(final_gps->getEnergy());
grasp->SetClearance(0.0);
grasp->SetClusterRep(false);
grasp->SetCompliantCopy(false);
grasp->SetSource("EIGENGRASPS");
std::vector<double> tempArray;
//the final grasp
GraspPlanningState *sol = new GraspPlanningState(final_gps);
//make sure you pass it sticky=true
sol->setPositionType(SPACE_COMPLETE,true);
sol->setPostureType(POSE_DOF,true);
tempArray.clear();
for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosture()->readVariable(i));
}
grasp->SetFinalgraspJoints(tempArray);
tempArray.clear();
for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosition()->readVariable(i));
}
grasp->SetFinalgraspPosition(tempArray);
delete sol;
//the pre-grasp
sol = new GraspPlanningState(pre_gps);
sol->setPositionType(SPACE_COMPLETE,true);
sol->setPostureType(POSE_DOF,true);
tempArray.clear();
for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosture()->readVariable(i));
}
grasp->SetPregraspJoints(tempArray);
tempArray.clear();
for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosition()->readVariable(i));
}
grasp->SetPregraspPosition(tempArray);
delete sol;
//contacts
//for some reason, the grasp's contact vector gets initialized to a mess!
tempArray.clear();
grasp->SetContacts(tempArray);
std::vector<db_planner::Grasp*> graspList;
graspList.push_back(grasp);
bool result = mDBMgr->SaveGrasps(graspList);
delete grasp;
return result;
}
int EvalPlugin::mainLoop()
{
//save grasps and reset planner
if(plannerStarted && plannerFinished && (!evaluatingGrasps))
{
std::cout << "FINISHED Planning\n" ;
SearchEnergy *mEnergyCalculator = new SearchEnergy();
mEnergyCalculator->setType(ENERGY_CONTACT_QUALITY);
mEnergyCalculator->setContactType(CONTACT_PRESET);
int num_grasps = mPlanner->getListSize();
std::cout << "Found " << num_grasps << " Grasps. " << std::endl;
for(int i=0; i < num_grasps; i++)
{
GraspPlanningState gps = mPlanner->getGrasp(i);
gps.execute(mHand);
mHand->autoGrasp(render_it, 1.0, false);
bool is_legal;
double new_planned_energy;
mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),is_legal,new_planned_energy,false );
gps.setEnergy(new_planned_energy);
}
//remove completed body
std::cout << "About to Remove Body\n" ;
graspItGUI->getMainWorld()->destroyElement(graspItGUI->getMainWorld()->getGB(0));
std::cout << "Removed Body\n" ;
//add gt body
std::cout << "About to add GT Body: " << gt_mesh_filepath.toStdString().c_str() << "\n" ;
graspItGUI->getMainWorld()->importBody("GraspableBody", gt_mesh_filepath);
graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale);
std::cout << "Added GT Body\n" ;
evaluatingGrasps = true;
std::ofstream outfile;
outfile.open(result_filepath.toStdString().c_str(), std::ios_base::app);
outfile << "Planned Meshfile,";
outfile << "Ground Truth Meshfile,";
outfile << "Grasp Legal,";
outfile << "Planned Quality,";
outfile << "Evaluated Quality,";
outfile << "Planned Joint Values,";
outfile << "Evaluated Joint Values,";
outfile << "Planned Pose Values,";
outfile << "Evaluated Pose Values,";
outfile << "Jaccard" << std::endl;
for(int i=0; i < num_grasps; i++)
{
GraspPlanningState gps = mPlanner->getGrasp(i);
gps.setObject(graspItGUI->getMainWorld()->getGB(0));
double desiredVals [mHand->getNumDOF()];
double desiredSteps [mHand->getNumDOF()];
desiredSteps[0] = 20.0;
desiredSteps[1] = 20.0;
desiredSteps[2] = 20.0;
desiredSteps[3] = 20.0;
bool legal;
double new_energy;
double old_energy = gps.getEnergy();
//mEnergyCalculator->analyzeState(legal,new_energy, &gps);
//disable collisions
graspItGUI->getMainWorld()->toggleAllCollisions(false);
usleep(10000);
gps.execute(mHand);
mHand->getDOFVals(desiredVals);
double initial_joint_values [mHand->getNumJoints()];
mHand->getJointValues(initial_joint_values);
transf initial_tran = mHand->getPalm()->getTran();
if(render_it)
{
graspItGUI->getIVmgr()->getViewer()->render();
usleep(1000000);
}
//mHand->quickOpen();
mHand->autoGrasp(true, -10.0, false);
if(render_it)
{
graspItGUI->getIVmgr()->getViewer()->render();
usleep(1000000);
}
mHand->approachToContact(-200.0);
if(render_it)
{
graspItGUI->getIVmgr()->getViewer()->render();
usleep(1000000);
}
graspItGUI->getMainWorld()->toggleAllCollisions(true);
mHand->approachToContact(200);
if(render_it)
{
graspItGUI->getIVmgr()->getViewer()->render();
usleep(1000000);
}
//gps.execute(mHand);
//possible
//mHand->moveDOFToContacts();
//mHand->checkSetDOFVals();
//moveDOFToContacts
//mHand->jumpDOFToContact();
//mHand->moveDOFToContacts(double *desiredVals, double *desiredSteps, bool stopAtContact, bool renderIt);
mHand->moveDOFToContacts( desiredVals, desiredSteps, false, true);
if(render_it)
{
graspItGUI->getIVmgr()->getViewer()->render();
usleep(1000000);
}
mHand->autoGrasp(render_it, 1.0, false);
double final_joint_values [mHand->getNumJoints()];
mHand->getJointValues(final_joint_values);
transf final_tran = mHand->getPalm()->getTran();
mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),legal,new_energy,false );
// outfile << "Planned Meshfile,";
outfile << completed_mesh_filepath.toStdString().c_str() << ",";
// outfile << "Ground Truth Meshfile,";
outfile << gt_mesh_filepath.toStdString().c_str() << ",";
// outfile << "Grasp Legal,";
outfile << legal << ",";
// outfile << "Planned Quality,";
outfile << old_energy << ",";
// outfile << "Evaluated Quality,";
outfile << new_energy << ",";
// outfile << "Planned Joint Values,";
for (int j =0; j < mHand->getNumJoints(); j++)
{
outfile << initial_joint_values[j] << ";" ;
}
outfile << ",";
// outfile << "Evaluated Joint Values,";
for (int j =0; j < mHand->getNumJoints(); j++)
{
outfile << final_joint_values[j] << ";" ;
}
outfile << ",";
// outfile << "Planned Pose Values,";
outfile << initial_tran.translation().x() << ";";
outfile << initial_tran.translation().y() << ";";
outfile << initial_tran.translation().z() << ";";
outfile << initial_tran.rotation().w << ";";
outfile << initial_tran.rotation().x << ";";
outfile << initial_tran.rotation().y << ";";
outfile << initial_tran.rotation().z << ";";
outfile << ",";
// outfile << "Evaluated Pose Values" << std::endl;
outfile << final_tran.translation().x() << ";";
outfile << final_tran.translation().y() << ";";
outfile << final_tran.translation().z() << ";";
outfile << final_tran.rotation().w << ";";
outfile << final_tran.rotation().x << ";";
outfile << final_tran.rotation().y << ";";
outfile << final_tran.rotation().z << ";";
outfile << ",";
//mEnergyCalculator->analyzeState(l2, new_energy, &gps);
outfile << jaccard;
outfile << std::endl;
}
outfile.close();
assert(false);
}
//start planner
else if (!plannerStarted)
{
std::cout << "Starting Planner\n" ;
graspItGUI->getMainWorld()->importBody("GraspableBody", completed_mesh_filepath);
graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale);
mObject = graspItGUI->getMainWorld()->getGB(0);
mHand = graspItGUI->getMainWorld()->getCurrentHand();
mHand->getGrasp()->setObjectNoUpdate(mObject);
mHand->getGrasp()->setGravity(false);
mHandObjectState = new GraspPlanningState(mHand);
mHandObjectState->setObject(mObject);
mHandObjectState->setPositionType(SPACE_AXIS_ANGLE);
mHandObjectState->setRefTran(mObject->getTran());
mHandObjectState->reset();
mPlanner = new SimAnnPlanner(mHand);
((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState);
mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY);
mPlanner->setContactType(CONTACT_PRESET);
mPlanner->setMaxSteps(num_steps);
mPlanner->resetPlanner();
mPlanner->startPlanner();
plannerStarted = true;
}
//let planner run.
else if( (plannerStarted) && !plannerFinished )
{
if ( mPlanner->getCurrentStep() >= num_steps)
{
mPlanner->stopPlanner();
plannerFinished=true;
}
}
return 0;
}
