/*!
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 GuidedGraspPlanningTask::plannerUpdate()
{
while (mLastSolution +1 < mPlanner->getListSize() )
{
DBGA("New solution!");
if (mLastSolution + 2 > mPlanner->getListSize())
{
DBGA("Error, expected even number of solutions");
mStatus = ERROR;
return;
}
if ( !saveGrasp(mPlanner->getGrasp(mLastSolution), mPlanner->getGrasp(mLastSolution+1)) )
{
DBGA("Error saving grasp");
mStatus = ERROR;
return;
}
mLastSolution += 2;
}
}
/*!
Moves the hand a distance of backStepDist back along the grasp approach
vector, and tries the grasp again. If the grasp is of minumum quality,
it saves it and exits with TRUE. Otherwise it continues stepping the hand
back and evaluating grasps until the hand collides with another object or
the fingers no longer touch the object, or the max number of back steps,
maxItStepNr , is reached. If no grasp has a quality greater than 0, then
FALSE is returned.
*/
bool
grasp_tester::iteration(plannedGrasp &pg)
{
double backStepDist = backStepSize;
transf actTran;
vec3 toVec;
for (int step = 0; step < maxItStepNr; step++) {
/* get actual tran */
actTran = my_hand->getTran();
toVec = actTran.translation() - backStepDist * pg.get_graspDirection().get_dir() / pg.get_graspDirection().get_dir().len();
/* take a step back */
transf to = coordinate_transf(position(toVec.x(), toVec.y(), toVec.z()),
(- pg.get_fixedFingerDirection()) *
pg.get_graspDirection().get_dir(),
- pg.get_fixedFingerDirection());
if (my_hand->setTran(to)) { return false; }
/* preshape again */
preshapeIt(pg.get_preshape(), render);
if (render) {
myViewer->render();
}
if (handCollision()) { return false; }
/* dont move; close fingers directly */
my_hand->autoGrasp(render, 1);
// my_world->findAllContacts();
my_world->updateGrasps();
// if (render)
// myViewer->render();
/* if distance is already too big and there are no more contacts,
the joints have closed to max and more steps won't help */
if (!checkContactToHand(pg.get_graspableBody())) {
if (my_hand->getName().startsWith("Barrett")) {
/* if joints have closed to max */
int allClosed = 0;
for (int i = 1; i < 4; i++) {
if (my_hand->getDOF(i)->getVal() == my_hand->getDOF(i)->getMax()) {
allClosed++;
}
}
if (allClosed >= 2) {
return false;
}
}
else { return false; }
}
/* Evaluate grasp */
pg.set_quality(my_grasp->getQM(whichQM)->evaluate());
if (saveToFile) { saveGrasp(pg.get_quality()); }
/* evaluate, if stable save break */
if (pg.get_quality() > QUALITY_MIN_THRESHOLD) {
return true;
}
}
/* not successful */
return false;
}
/*!
This is the main testing function. It is called whenever the user is idle,
and it tests the next grasp in the graspList. It follows the testing
process defined above. When the last grasp is reached, the hand is
returned to its position before the testing began, the graspList is sorted
in quality order, and the testingComplete signal is emitted.
*/
void
grasp_tester::testIt()
{
bool do_iteration;
bool do_save;
/* Show status bar */
#ifdef GRASPITDBG
std::cout << "PL_OUT: Testing grasp no " << actualGraspNr++ << " out of " <<
nrOfGrasps << std::endl;
#endif
/* Loop over all planned grasps to test them */
if (it_gr != (*graspList).end()) {
do_iteration = false;
do_save = false;
/* Update visualization */
#ifdef GRASPITDBG
std::cout << "PL_OUT: vor change color" << std::endl;
#endif
(*it_gr)->get_graspRepresentation()->changeColor(1., 0., 0.);
if (render) {
myViewer->render();
projectionViewer->render(); // this doesn't work!!!!!!
}
#ifdef GRASPITDBG
std::cout << "PL_OUT: Put hand in position" << std::endl;
#endif
/* First, put hand to starting point outside the object */
if (putIt(*it_gr, render) == SUCCESS) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: set preshape" << std::endl;
#endif
/* Use given preshape */
preshapeIt((*it_gr)->get_preshape(), render);
#ifdef GRASPITDBG
std::cout << "PL_OUT: test for collisions" << std::endl;
#endif
/* check if hand collides already with any obstacle (like the table) */
if (!handCollision()) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: move hand towards object" << std::endl;
#endif
/* Now, move the hand in the specified direction */
if (moveIt((*it_gr)->get_graspDirection(), render)) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: check contacts" << std::endl;
#endif
/* Check if contact exists between hand and the right object */
if (checkContactToHand((*it_gr)->get_graspableBody())) {
/* Then close the fingers */
my_hand->autoGrasp(render, 1);
// my_world->findAllContacts();
my_world->updateGrasps();
// if (render) myViewer->render();
/* Evaluate grasp */
(*it_gr)->set_quality(my_grasp->getQM(whichQM)->evaluate());
#ifdef GRASPITDBG
std::cout << "PL_OUT: !!!! whichQM: " << whichQM << " quality: " << (*it_gr)->get_quality() << std::endl;
#endif
if (saveToFile) { saveGrasp((*it_gr)->get_quality()); }
/* save final position to grasp class */
if ((*it_gr)->get_quality() > QUALITY_MIN_THRESHOLD && (*it_gr)->get_quality() <= 1.0) {
do_save = true;
}
}
else {
do_iteration = true;
}
/* iteration if:
- grasp not stable
- wrong contacts */
if (do_iteration ||
((*it_gr)->get_quality() <= QUALITY_MIN_THRESHOLD)) {
if (iteration(**it_gr)) {
/* save final position to grasp class */
do_save = true;
}
}
}
#ifdef GRASPITDBG
else { std::cout << "PL_OUT: MoveIt failed." << std::endl; }
#endif
}
}
#ifdef GRASPITDBG
else {
std::cout << "PL_OUT: putIt failed. Stepping to next grasp." << std::endl;
}
#endif
if (do_save) {
/* change radius in vis window according to quality */
(*it_gr)->get_graspRepresentation()->changeRadius((*it_gr)->get_quality());
/* save final position to grasp class */
savePosition(**it_gr);
}
else {
(*it_gr)->remove_graspRepresentation();
}
/* reset color */
//(*it_gr)->get_graspRepresentation()->resetColor();
/* increment iterator for next step */
if (it_gr != (*graspList).end()) {
it_gr++;
}
}
/* last grasp reached */
else {
#ifdef GRASPITDBG
std::cout << "PL_OUT: Last grasp reached" << std::endl;
#endif
/* Order List and remove bad grasps */
orderGraspListByQuality(*graspList);
if (saveToFile) {graspFile.close(); saveToFile = false;}
/* we are ready; kill idleSensor */
if (idleSensor != NULL) {
delete idleSensor;
}
idleSensor = NULL;
if (render) {
/* put the hand back to starting position */
my_hand->setTran(origTran);
my_hand->forceDOFVals(dofs);
}
PROF_STOP_TIMER(TOTAL_PLANNER);
PROF_PRINT_ALL;
Q_EMIT testingComplete();
}
if (idleSensor != NULL) {
idleSensor->schedule();
}
if (!render) {
/* put the hand back to starting position */
my_hand->setTran(origTran);
my_hand->forceDOFVals(dofs);
}
}
