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;
}
}
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;
}
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());
}
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 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;
}
