void DBaseDlg::loadGraspButton_clicked(){
PROF_RESET_ALL;
PROF_START_TIMER(GET_GRASPS);
//get the current hand and check its validity
Hand *hand = graspItGUI->getIVmgr()->getWorld()->getCurrentHand();
if (!hand) {
DBGA("Load and select a hand before viewing grasps!");
return;
}
//check the currently loaded model
if(!mCurrentLoadedModel){
DBGA("Load model first!");
return;
}
//clear the previously loaded grasps
deleteVectorElements<db_planner::Grasp*, GraspitDBGrasp*>(mGraspList);
mGraspList.clear();
mCurrentFrame = 0;
//get new grasps from database manager
PROF_START_TIMER(GET_GRASPS_CALL);
if(!mDBMgr->GetGrasps(*mCurrentLoadedModel,hand->getDBName().toStdString(), &mGraspList)){
DBGA("Load grasps failed");
mGraspList.clear();
return;
}
PROF_STOP_TIMER(GET_GRASPS_CALL);
for(std::vector<db_planner::Grasp*>::iterator it = mGraspList.begin(); it != mGraspList.end(); ){
if( QString((*it)->GetSource().c_str()) == typesComboBox->currentText() ||
typesComboBox->currentText() == "ALL") ++it;
else{
delete (*it);
mGraspList.erase(it);
}
}
//set corresponding indices and show the grasp
QString numTotal, numCurrent;
numTotal.setNum(mGraspList.size());
if(!mGraspList.empty()){
numCurrent.setNum(mCurrentFrame + 1);
graspsGroup->setEnabled(TRUE);
showGrasp(0);
showMarkers();
} else{
numCurrent.setNum(0);
graspsGroup->setEnabled(FALSE);
}
graspIndexLabel->setText(numCurrent + "/" + numTotal);
PROF_STOP_TIMER(GET_GRASPS);
PROF_PRINT_ALL;
}
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();
}
}
int main(int argc, char**argv)
{
PROF_START_TIMER(TOTAL_TIMER);
for (int i=0; i<100000; i++)
{
double a = 5*5;
func();
}
PROF_STOP_TIMER(TOTAL_TIMER);
PROF_PRINT_ALL;
return 0;
}
/*!
Begins the testing process. The list of grasps to test is provided in
\a graspList_in , and \a render_in controls whether the movement of the hand
will be shown during the testing process. It helps to understand what is
going on, but slows down the process.
This saves the current hand configuration, so that after testing is complete
it can be returned to it's original state. However, we may want to create
a separate copy of the main world in which to perform the testing, so we
don't make any changes to the main world.
An Inventor idleSensor is created to call the testing callback whenever the
user is idle. This means that the testing won't interfere with user
interaction like changing the camera viewpoint.
*/
bool
grasp_tester::callTestIt(std::list<plannedGrasp *> &graspList_in, bool render_in)
{
/* check if another testing process is running */
if (idleSensor) { return false; }
/* get global stuff from ivmgr */
updateGlobals();
/* check if at least one quality measure exists */
if (whichQM < 0) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: No quality measure specified. Do that first!" << std::endl;
#endif
return false;
}
/* check if any planned grasps in list */
if (graspList_in.empty()) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: Tester received empty grasp list. Nothing happened." << std::endl;
#endif
return false;
}
nrOfGrasps = graspList_in.size();
actualGraspNr = 0;
graspList = &graspList_in;
render = render_in;
/* Save old hand transformation */
origTran = my_hand->getTran();
dofs = new double[my_hand->getNumDOF()];
for (int i = 0; i < my_hand->getNumDOF(); i++) {
dofs[i] = my_hand->getDOF(i)->getVal();
}
/* set starting iterator for thread */
it_gr = (*graspList).begin();
PROF_RESET_ALL;
PROF_START_TIMER(TOTAL_PLANNER);
/* start thread */
idleSensor = new SoIdleSensor(testItCB, NULL);
idleSensor->schedule();
return true;
}
void
EGPlanner::startPlanner()
{
if ( getState() != READY ) {
DBGA("Planner not ready to start!");
return;
}
if (!mMultiThread) {
mHand->showVirtualContacts(false);
mIdleSensor = new SoIdleSensor(sensorCB, this);
mIdleSensor->schedule();
}
PROF_RESET_ALL;
PROF_START_TIMER(EG_PLANNER);
mStartTime = clock();
setState( RUNNING );
}
bool SqlDatabaseManager::GetGrasps(const Model& model,
const string& hand_name,
vector<Grasp*>* grasp_list) const {
if (grasp_list == NULL) return false;
Table results;
PROF_START_TIMER(GET_GRASPS_SQL);
if (!database_.Query("SELECT * FROM get_grasps('" + model.ModelName() +
"','" + hand_name + "');", &results))
return false;
PROF_STOP_TIMER(GET_GRASPS_SQL);
// Get the column indices for the columns we care about.
int pregrasp_joints_column, grasp_joints_column,
pregrasp_position_column, grasp_position_column,
grasp_id_column, epsilon_quality_column, volume_quality_column,
grasp_contacts_column, grasp_source_name_column;
PROF_START_TIMER(GET_GRASPS_GETCOLUMN);
if (!results.GetColumnIndex("grasp_pregrasp_joints", &pregrasp_joints_column) ||
!results.GetColumnIndex("grasp_grasp_joints", &grasp_joints_column) ||
!results.GetColumnIndex("grasp_pregrasp_position",
&pregrasp_position_column) ||
!results.GetColumnIndex("grasp_grasp_position", &grasp_position_column) ||
!results.GetColumnIndex("grasp_id", &grasp_id_column) ||
!results.GetColumnIndex("grasp_epsilon_quality", &epsilon_quality_column) ||
!results.GetColumnIndex("grasp_volume_quality", &volume_quality_column) ||
!results.GetColumnIndex("grasp_contacts", &grasp_contacts_column) ||
!results.GetColumnIndex("grasp_source_name", &grasp_source_name_column))
return false;
PROF_STOP_TIMER(GET_GRASPS_GETCOLUMN);
// Turn each database row into a Grasp.
vector<double> pregrasp_joints, grasp_joints,
pregrasp_position, grasp_position, grasp_contacts;
int grasp_id;
string grasp_source_name;
double epsilon_quality, volume_quality;
const int num_rows = results.NumRows();
grasp_list->reserve(num_rows);
for (int row = 0; row < num_rows; ++row) {
grasp_list->push_back(grasp_allocator_->Get());
Grasp& grasp = *(grasp_list->back());
pregrasp_joints.clear();
pregrasp_position.clear();
grasp_joints.clear();
grasp_position.clear();
PROF_START_TIMER(GET_GRASPS_GETFIELD);
if (!results.GetField(pregrasp_joints_column, row, &pregrasp_joints) ||
!results.GetField(grasp_joints_column, row, &grasp_joints) ||
!results.GetField(pregrasp_position_column, row, &pregrasp_position) ||
!results.GetField(grasp_position_column, row, &grasp_position) ||
!results.GetField(grasp_id_column, row, &grasp_id) ||
!results.GetField(epsilon_quality_column, row, &epsilon_quality) ||
!results.GetField(volume_quality_column, row, &volume_quality) ||
!results.GetField(grasp_contacts_column, row, &grasp_contacts) ||
!results.GetField(grasp_source_name_column, row, &grasp_source_name))
return false;
PROF_STOP_TIMER(GET_GRASPS_GETFIELD);
grasp.SetSourceModel(model);
grasp.SetHandName(hand_name);
grasp.SetEpsilonQuality(epsilon_quality);
grasp.SetVolumeQuality(volume_quality);
grasp.SetGraspId(grasp_id);
pregrasp_joints.erase(pregrasp_joints.begin());
pregrasp_position.erase(pregrasp_position.begin());
grasp_joints.erase(grasp_joints.begin());
grasp_position.erase(grasp_position.begin());
grasp.SetGraspParameters(pregrasp_joints,
pregrasp_position,
grasp_joints,
grasp_position);
grasp.SetContacts(grasp_contacts);
grasp.SetPregraspJoints(pregrasp_joints);
grasp.SetPregraspPosition(pregrasp_position);
grasp.SetFinalgraspJoints(grasp_joints);
grasp.SetFinalgraspPosition(grasp_position);
grasp.SetSource(grasp_source_name);
}
return true;
}
