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