void
CompliantPlannerDlg::testButtonClicked()
{
if (energyTypeBox->currentText() == "Quasistatic") {
mPlanner->setEnergyType(ENERGY_COMPLIANT);
} else if (energyTypeBox->currentText() == "Dynamic") {
mPlanner->setEnergyType(ENERGY_DYNAMIC);
} else {
assert(0);
}
if (mPlanner->isActive()) {
DBGA("Pause:");
mPlanner->pausePlanner();
} else {
PROF_RESET(QS_TOTAL);
PROF_START_TIMER(QS_TOTAL);
mBatch = false;
if (mOut) {
mPlanner->setStatStream(mOut);
} else {
mPlanner->setStatStream(&std::cerr);
}
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();
}