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();
}
}
/*!
Start running the task
First gets the hand and object, then creates a starting position for the planner
Then it generates a loop planner and begins planning.
*/
void GraspPlanningTask::start()
{
//sets mHand
getHand();
if(!mHand)
{
DBGA("Failed to start planner");
return;
}
//sets mObject
getObject();
//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);
mPlanner->setEnergyType(ENERGY_STRICT_AUTOGRASP);
mPlanner->setContactType(CONTACT_PRESET);
// mPlanner->setMaxSteps(65000);
mPlanner->setRepeat(true);
//max time set from database record
if (mRecord.taskTime >= 0){
mPlanner->setMaxTime(mRecord.taskTime);
} else {
mPlanner->setMaxTime(-1);
}
static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed);
startPlanner();
}
void EigenGraspPlannerDlg::plannerStart_clicked()
{
if (!mPlanner->isActive()){
startPlanner();
} else {
stopPlanner();
}
}
void AutoGraspGenerationDlg::plannerStart_clicked()
{
if (!mPlanner->isActive()){
startPlanner();
} else {
stopPlanner();
}
}
void
CompliantPlannerDlg::designTestButtonClicked()
{
if (!mOut) {
DBGA("Set output file first!!!!");
return;
}
mTFrom = tFromEdit->text().toDouble();
mTTo = tToEdit->text().toDouble();
mTStep = tStepEdit->text().toDouble();
mSFrom = sFromEdit->text().toDouble();
mSTo = sToEdit->text().toDouble();
mSStep = sStepEdit->text().toDouble();
DBGA("Starting batch testing");
mBatch = true;
mTR = mTFrom;
mSR = mSFrom;
mPlanner->setEnergyType(ENERGY_COMPLIANT);
mPlanner->setStatStream(NULL);
startPlanner();
}
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();
}
