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