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;
}
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;
}
void
EGPlanner::pausePlanner()
{
if (getState() != RUNNING) return;
mRunningTime = getRunningTime();
if (!mMultiThread) {
if (mIdleSensor) delete mIdleSensor;
mIdleSensor = NULL;
mHand->showVirtualContacts(true);
}
setState(READY);
PROF_STOP_TIMER(EG_PLANNER);
PROF_PRINT_ALL;
if (!mMultiThread) emit complete();
}
void
EGPlanner::pausePlanner()
{
if (getState() != RUNNING) return;
mProfileInstance->stopTimer();
if (!mMultiThread) {
if (mIdleSensor) delete mIdleSensor;
mIdleSensor = NULL;
mHand->showVirtualContacts(true);
}
setState(READY);
PROF_STOP_TIMER(EG_PLANNER);
PROF_PRINT_ALL;
if (!mMultiThread) Q_EMIT complete();
}
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();
}
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;
}
/*!
This is the main testing function. It is called whenever the user is idle,
and it tests the next grasp in the graspList. It follows the testing
process defined above. When the last grasp is reached, the hand is
returned to its position before the testing began, the graspList is sorted
in quality order, and the testingComplete signal is emitted.
*/
void
grasp_tester::testIt()
{
bool do_iteration;
bool do_save;
/* Show status bar */
#ifdef GRASPITDBG
std::cout << "PL_OUT: Testing grasp no " << actualGraspNr++ << " out of " <<
nrOfGrasps << std::endl;
#endif
/* Loop over all planned grasps to test them */
if (it_gr != (*graspList).end()) {
do_iteration = false;
do_save = false;
/* Update visualization */
#ifdef GRASPITDBG
std::cout << "PL_OUT: vor change color" << std::endl;
#endif
(*it_gr)->get_graspRepresentation()->changeColor(1., 0., 0.);
if (render) {
myViewer->render();
projectionViewer->render(); // this doesn't work!!!!!!
}
#ifdef GRASPITDBG
std::cout << "PL_OUT: Put hand in position" << std::endl;
#endif
/* First, put hand to starting point outside the object */
if (putIt(*it_gr, render) == SUCCESS) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: set preshape" << std::endl;
#endif
/* Use given preshape */
preshapeIt((*it_gr)->get_preshape(), render);
#ifdef GRASPITDBG
std::cout << "PL_OUT: test for collisions" << std::endl;
#endif
/* check if hand collides already with any obstacle (like the table) */
if (!handCollision()) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: move hand towards object" << std::endl;
#endif
/* Now, move the hand in the specified direction */
if (moveIt((*it_gr)->get_graspDirection(), render)) {
#ifdef GRASPITDBG
std::cout << "PL_OUT: check contacts" << std::endl;
#endif
/* Check if contact exists between hand and the right object */
if (checkContactToHand((*it_gr)->get_graspableBody())) {
/* Then close the fingers */
my_hand->autoGrasp(render, 1);
// my_world->findAllContacts();
my_world->updateGrasps();
// if (render) myViewer->render();
/* Evaluate grasp */
(*it_gr)->set_quality(my_grasp->getQM(whichQM)->evaluate());
#ifdef GRASPITDBG
std::cout << "PL_OUT: !!!! whichQM: " << whichQM << " quality: " << (*it_gr)->get_quality() << std::endl;
#endif
if (saveToFile) { saveGrasp((*it_gr)->get_quality()); }
/* save final position to grasp class */
if ((*it_gr)->get_quality() > QUALITY_MIN_THRESHOLD && (*it_gr)->get_quality() <= 1.0) {
do_save = true;
}
}
else {
do_iteration = true;
}
/* iteration if:
- grasp not stable
- wrong contacts */
if (do_iteration ||
((*it_gr)->get_quality() <= QUALITY_MIN_THRESHOLD)) {
if (iteration(**it_gr)) {
/* save final position to grasp class */
do_save = true;
}
}
}
#ifdef GRASPITDBG
else { std::cout << "PL_OUT: MoveIt failed." << std::endl; }
#endif
}
}
#ifdef GRASPITDBG
else {
std::cout << "PL_OUT: putIt failed. Stepping to next grasp." << std::endl;
}
#endif
if (do_save) {
/* change radius in vis window according to quality */
(*it_gr)->get_graspRepresentation()->changeRadius((*it_gr)->get_quality());
/* save final position to grasp class */
savePosition(**it_gr);
}
else {
(*it_gr)->remove_graspRepresentation();
}
/* reset color */
//(*it_gr)->get_graspRepresentation()->resetColor();
/* increment iterator for next step */
if (it_gr != (*graspList).end()) {
it_gr++;
}
}
/* last grasp reached */
else {
#ifdef GRASPITDBG
std::cout << "PL_OUT: Last grasp reached" << std::endl;
#endif
/* Order List and remove bad grasps */
orderGraspListByQuality(*graspList);
if (saveToFile) {graspFile.close(); saveToFile = false;}
/* we are ready; kill idleSensor */
if (idleSensor != NULL) {
delete idleSensor;
}
idleSensor = NULL;
if (render) {
/* put the hand back to starting position */
my_hand->setTran(origTran);
my_hand->forceDOFVals(dofs);
}
PROF_STOP_TIMER(TOTAL_PLANNER);
PROF_PRINT_ALL;
Q_EMIT testingComplete();
}
if (idleSensor != NULL) {
idleSensor->schedule();
}
if (!render) {
/* put the hand back to starting position */
my_hand->setTran(origTran);
my_hand->forceDOFVals(dofs);
}
}
