void
CompliantPlannerDlg::plannerFinished()
{
PROF_STOP_TIMER(QS_TOTAL);
PROF_PRINT(QS_TOTAL);
if (!mBatch) return;
//write results to file
DBGA("Test done: " << mTR << " and " << mSR);
if (mOut) {
*mOut << mTR << " " << mSR;
*mOut << std::endl;
}
mSR += mSStep;
if (mSR > mSTo + 1.0e-2) {
mSR = mSFrom;
mTR += mTStep;
if (mTR > mTTo + 1.0e-2) {
mBatch = false;
return;
}
}
mPlanner->resetPlanner();
startPlanner();
}
double CompliantEnergy::energy() const
{
PROF_RESET(QS);
PROF_TIMER_FUNC(QS);
//approach the object until contact; go really far if needed
mHand->findInitialContact(200);
//check if we've actually touched the object
if (!mHand->getNumContacts(mObject)) { return 1; }
//close the hand, but do additional processing when each new contact happens
mCompUnbalanced = false;
mMaxUnbalancedForce.set(0.0, 0.0, 0.0);
QObject::connect(mHand, SIGNAL(moveDOFStepTaken(int, bool &)),
this, SLOT(autoGraspStep(int, bool &)));
mHand->autoGrasp(!mDisableRendering, 1.0, false);
QObject::disconnect(mHand, SIGNAL(moveDOFStepTaken(int, bool &)),
this, SLOT(autoGraspStep(int, bool &)));
if (mCompUnbalanced || mMaxUnbalancedForce.len() > unbalancedForceThreshold) {
//the equivalent of an unstable grasp
}
//check if we've actually grasped the object
if (mHand->getNumContacts(mObject) < 2) { return 1; }
PRINT_STAT(mOut, "unbal: " << mMaxUnbalancedForce);
//compute unbalanced force again. Is it zero?
//but compute it for all the force that the dofs will apply
//a big hack for now. It is questionable if the hand should even allow
//this kind of intrusion into its dofs.
for (int d = 0; d < mHand->getNumDOF(); d++) {
mHand->getDOF(d)->setForce(mHand->getDOF(d)->getMaxForce());
}
mHand->getWorld()->resetDynamicWrenches();
//passing true means the set dof force will be used in computations
Matrix tau(mHand->staticJointTorques(true));
int result = mHand->getGrasp()->computeQuasistaticForces(tau);
if (result) {
if (result > 0) {
PRINT_STAT(mOut, "Final_unbalanced");
} else {
PRINT_STAT(mOut, "Final_ERROR");
}
return 1.0;
}
double *extWrench = static_cast<DynamicBody *>(mObject)->getExtWrenchAcc();
vec3 force(extWrench[0], extWrench[1], extWrench[2]);
vec3 torque(extWrench[3], extWrench[4], extWrench[5]);
//perform traditional f-c check
mHand->getGrasp()->collectContacts();
mHand->getGrasp()->updateWrenchSpaces();
double epsQual = mEpsQual->evaluate();
PRINT_STAT(mOut, "eps: " << epsQual);
if (epsQual < 0.05) { return 1.0; }
PROF_PRINT(QS);
PRINT_STAT(mOut, "torque: " << torque << " " << torque.len());
PRINT_STAT(mOut, "force: " << force << " " << force.len());
return -200.0 + force.len();// + torque.len();
}
