コード例 #1
0
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();
	}
}
コード例 #2
0
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();
}