00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "contactExaminerDlg.h"
00027
00028 #include <QFileDialog>
00029
00030 #include "world.h"
00031 #include "robot.h"
00032 #include "contact.h"
00033 #include "grasp.h"
00034 #include "quality.h"
00035 #include "graspitGUI.h"
00036 #include "ivmgr.h"
00037 #include "body.h"
00038 #include "mainWindow.h"
00039
00040 #include "debug.h"
00041
00042 void ContactExaminerDlg::init()
00043 {
00044 mWorld = graspItGUI->getIVmgr()->getWorld();
00045
00046 if(mWorld->getCurrentHand()){
00047 mHand = mWorld->getCurrentHand();
00048 handRadioButton->setEnabled(TRUE);
00049 }
00050
00051 if(mWorld->getNumGB() > 0){
00052 mObject = mWorld->getGB(0);
00053 objectRadioButton->setEnabled(TRUE);
00054 }
00055 if (mHand) {
00056 handRadioButton->setChecked(true);
00057 } else if (mObject) {
00058 objectRadioButton->setChecked(true);
00059 }
00060 modeSelected();
00061 }
00062
00063 void ContactExaminerDlg::markButton_clicked()
00064 {
00065 if(objectRadioButton->isChecked()){
00066 collectObjectContacts();
00067 } else if(handRadioButton->isChecked()) {
00068 collectHandContacts();
00069 } else {
00070 return;
00071 }
00072 markedLabel->setNum( (int)mMarkedContacts.size() );
00073 }
00074
00075 void ContactExaminerDlg::collectHandContacts()
00076 {
00077 int f,l;
00078 std::list<Contact *>::iterator cp;
00079 std::list<Contact *> contactList;
00080 Contact *newContact;
00081
00082
00083 mHand->showVirtualContacts(true);
00084 contactList = mHand->getPalm()->getContacts();
00085 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00086 if ( (*cp)->getMate() != (*cp) ) {
00087 newContact = new VirtualContact(-1, 0, *cp);
00088 ((VirtualContact*)newContact)->setBody( mHand->getPalm() );
00089 mMarkedContacts.push_back(newContact);
00090 mHand->getPalm()->addVirtualContact((VirtualContact*)newContact);
00091 }
00092 }
00093
00094 for(f=0;f<mHand->getNumFingers();f++) {
00095 for (l=0;l<mHand->getFinger(f)->getNumLinks();l++) {
00096 contactList = mHand->getFinger(f)->getLink(l)->getContacts();
00097 for (cp=contactList.begin();cp!=contactList.end();cp++){
00098 if ( (*cp)->getMate() != (*cp) ) {
00099 newContact = new VirtualContact(f, l, *cp);
00100 ((VirtualContact*)newContact)->setBody( mHand->getFinger(f)->getLink(l) );
00101 mMarkedContacts.push_back(newContact);
00102 mHand->getFinger(f)->getLink(l)->addVirtualContact((VirtualContact*)newContact);
00103 }
00104 }
00105 }
00106 }
00107 }
00108
00109 void ContactExaminerDlg::collectObjectContacts()
00110 {
00111 std::list<Contact *>::iterator cp;
00112 std::list<Contact *> contactList;
00113 Contact *newContact;
00114
00115 contactList = mObject->getContacts();
00116 for(cp = contactList.begin(); cp != contactList.end(); ++cp){
00117 newContact = new VirtualContact(-1, 0, *cp);
00118 ((VirtualContact*)newContact)->setBody(mObject);
00119 mMarkedContacts.push_back(newContact);
00120 mObject->addVirtualContact((VirtualContact*)newContact);
00121 }
00122 }
00123
00124 void ContactExaminerDlg::loadButton_clicked()
00125 {
00126 QString fn = QFileDialog::getOpenFileName( this, "Select virtual contact files to load",
00127 QString(getenv("GRASPIT"))+QString("/models/virtual"),"Virtual Contacts (*.vgr)" );
00128 QStringList::iterator it;
00129 if(fn.count() == 0)
00130 return;
00131
00132 clearButton_clicked();
00133
00134 if (objectRadioButton->isChecked()) {
00135 mObject->loadContactData(fn);
00136 } else if (handRadioButton->isChecked()){
00137 mHand->loadContactData(fn);
00138 mHand->showVirtualContacts(true);
00139 }
00140 }
00141
00142 void ContactExaminerDlg::saveButton_clicked()
00143 {
00144 QString fn = QFileDialog::getSaveFileName( this, "Select filename",
00145 QString(getenv("GRASPIT"))+QString("/models/virtual"),"Virtual Grasp Files (*.vgr)" );
00146 if ( !fn.isEmpty() ) {
00147 if (fn.section('.',1).isEmpty())
00148 fn.append(".vgr");
00149 } else {
00150 return;
00151 }
00152
00153 FILE *fp = fopen(fn.latin1(), "w");
00154 if (!fp) {
00155 fprintf(stderr,"Failed to open file for writing\n");
00156 }
00157
00158 if(handRadioButton->isChecked()){
00159 fprintf(fp,"%s\n",mHand->getName().latin1());
00160 fprintf(fp,"%d\n",(int)mMarkedContacts.size());
00161 for (int i=0; i<(int)mMarkedContacts.size(); i++) {
00162 ((VirtualContact*)mMarkedContacts[i])->writeToFile(fp);
00163 }
00164 } else if (objectRadioButton->isChecked()){
00165 fprintf(fp,"%d\n",(int)mMarkedContacts.size());
00166 for (int i=0; i<(int)mMarkedContacts.size(); i++) {
00167 ((VirtualContactOnObject*)mMarkedContacts[i])->writeToFile(fp);
00168 }
00169 }
00170 fclose(fp);
00171 }
00172
00173 void ContactExaminerDlg::exitButton_clicked()
00174 {
00175 QDialog::accept();
00176 }
00177
00178 void ContactExaminerDlg::updateButtons()
00179 {
00180 if (handRadioButton->isChecked()) {
00181 contactCollectionGroupBox->setTitle(QString("Contact Collection: ") + mHand->getName());
00182 } else if (objectRadioButton->isChecked()) {
00183 contactCollectionGroupBox->setTitle(QString("Contact Collection: ") + mObject->getName());
00184 }
00185 }
00186
00187 void ContactExaminerDlg::updateQualityButton_clicked()
00188 {
00189 showQuality();
00190 }
00191
00192 void ContactExaminerDlg::showQuality()
00193 {
00194 double q;
00195 if (!mQual || !mGrasp) {
00196 q = 0.0;
00197 } else {
00198 mGrasp->update();
00199 DBGA("Evaluating quality");
00200 q = mQual->evaluate();
00201 }
00202 if (q < 0) q = 0;
00203 QString qs;
00204 qs.setNum(q);
00205 qs.truncate(5);
00206 qualityLabel->setText(qs);
00207 }
00208
00209 void ContactExaminerDlg::clearButton_clicked()
00210 {
00211 if(objectRadioButton->isChecked() && mObject->getNumVirtualContacts()>0){
00212 mObject->breakVirtualContacts();
00213 } else if(handRadioButton->isChecked()) {
00214 mHand->getPalm()->breakVirtualContacts();
00215 for(int i = 0; i < mHand->getNumFingers(); ++i){
00216 for(int j = 0; j < mHand->getFinger(i)->getNumLinks(); ++j){
00217 mHand->getFinger(i)->getLink(j)->breakVirtualContacts();
00218 }
00219 }
00220 }
00221 mMarkedContacts.clear();
00222 markedLabel->setText("0");
00223 qualityLabel->setText("0");
00224 }
00225
00226 void ContactExaminerDlg::destroy()
00227 {
00228 if (mQual) delete mQual;
00229 if (objectRadioButton->isChecked()) {
00230 delete mGrasp;
00231 }
00232 }
00233
00234 void ContactExaminerDlg::showGWSButton_clicked()
00235 {
00236 mGrasp->update();
00237 graspItGUI->getMainWindow()->graspCreateProjection(mGrasp);
00238 }
00239
00240 void ContactExaminerDlg::modeSelected()
00241 {
00242
00243
00244
00245 static int mode = 0;
00246 int newMode;
00247 if (handRadioButton->isChecked()) {
00248 newMode = 1;
00249 } else if (objectRadioButton->isChecked()) {
00250 newMode = 2;
00251 } else {
00252 newMode = 0;
00253 }
00254
00255 if (mode!=newMode) {
00256 mMarkedContacts.clear();
00257 markedLabel->setText("0");
00258
00259 delete mQual;
00260 mQual = NULL;
00261 if (mode!=1) {
00262 delete mGrasp;
00263 mGrasp = NULL;
00264 }
00265 if (newMode == 1) {
00266 mHand->getGrasp()->setObject(NULL);
00267 mGrasp = mHand->getGrasp();
00268 } else if (newMode == 2){
00269 mGrasp = new Grasp(NULL);
00270 mGrasp->setObject(mObject);
00271 }
00272 mQual = new QualEpsilon( mGrasp, QString("Virtual_grasp_qm"),"L1 Norm");
00273 mode = newMode;
00274 }
00275 updateButtons();
00276 }