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