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; }
}
void
ListPlanner::showState(int index)
{
GraspPlanningState *state = getState(index);
if (!state) return;
state->execute();
}
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);
}
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);
}
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;
}
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);
mBestList.push_back(s);
if (mMarkSolutions) {
mHand->getWorld()->getIVRoot()->addChild(s->getIVRoot());
}
}
updateSolutionList();
//now shape the real hand.
s = mInterface->updateHand(&mBestList);
if (s) {
if (mSolutionClone) { s->execute(mSolutionClone); }
if (mMarkSolutions) { s->setIVMarkerColor(1, 1, 0); }
}
DBGP("On-line main loop done");
}
