Skip to content
Snippets Groups Projects
Commit d7e0ad25 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Add a load object button to GraspPlannerWindow (and add layout top ui file)

parent ee741356
No related branches found
No related merge requests found
...@@ -36,309 +36,207 @@ ...@@ -36,309 +36,207 @@
<property name="title"> <property name="title">
<string/> <string/>
</property> </property>
<widget class="QPushButton" name="pushButtonReset"> <layout class="QGridLayout" name="gridLayout_2">
<property name="geometry"> <item row="0" column="0" colspan="2">
<rect> <widget class="QPushButton" name="pushButtonReset">
<x>20</x> <property name="text">
<y>20</y> <string>Reset</string>
<width>171</width> </property>
<height>28</height> </widget>
</rect> </item>
</property> <item row="9" column="0" colspan="2">
<property name="text"> <widget class="QCheckBox" name="checkBoxGrasps">
<string>Reset</string> <property name="text">
</property> <string>Show grasps</string>
</widget> </property>
<widget class="QLabel" name="label_10"> </widget>
<property name="geometry"> </item>
<rect> <item row="4" column="0">
<x>10</x> <widget class="QPushButton" name="pushButtonClose">
<y>450</y> <property name="text">
<width>121</width> <string>Close</string>
<height>16</height> </property>
</rect> </widget>
</property> </item>
<property name="font"> <item row="4" column="1">
<font> <widget class="QPushButton" name="pushButtonOpen">
<weight>75</weight> <property name="text">
<bold>true</bold> <string>Open</string>
</font> </property>
</property> </widget>
<property name="text"> </item>
<string>Visualization</string> <item row="6" column="0" colspan="2">
</property> <widget class="QLabel" name="label_10">
</widget> <property name="font">
<widget class="QCheckBox" name="checkBoxColModel"> <font>
<property name="geometry"> <weight>75</weight>
<rect> <bold>true</bold>
<x>20</x> </font>
<y>480</y> </property>
<width>141</width> <property name="text">
<height>17</height> <string>Visualization</string>
</rect> </property>
</property> </widget>
<property name="text"> </item>
<string>Collision Model</string> <item row="7" column="0" colspan="2">
</property> <widget class="QCheckBox" name="checkBoxColModel">
</widget> <property name="text">
<widget class="QCheckBox" name="checkBoxCones"> <string>Collision Model</string>
<property name="geometry"> </property>
<rect> </widget>
<x>20</x> </item>
<y>500</y> <item row="3" column="1">
<width>141</width> <widget class="QPushButton" name="pushButtonPlanBatch">
<height>17</height> <property name="text">
</rect> <string>Plan batch</string>
</property> </property>
<property name="text"> </widget>
<string>Friction Cones</string> </item>
</property> <item row="5" column="0" colspan="2">
</widget> <widget class="QPushButton" name="pushButtonSave">
<widget class="QCheckBox" name="checkBoxGrasps"> <property name="text">
<property name="geometry"> <string>Save</string>
<rect> </property>
<x>20</x> </widget>
<y>520</y> </item>
<width>141</width> <item row="2" column="0" colspan="2">
<height>17</height> <widget class="QGroupBox" name="groupBox_2">
</rect> <property name="title">
</property> <string>Grasp Planning Options</string>
<property name="text"> </property>
<string>Show grasps</string> <layout class="QGridLayout" name="gridLayout_3">
</property> <item row="0" column="0">
</widget> <widget class="QLabel" name="label_11">
<widget class="QPushButton" name="pushButtonPlan"> <property name="text">
<property name="geometry"> <string>Number of Grasps</string>
<rect> </property>
<x>10</x> </widget>
<y>300</y> </item>
<width>91</width> <item row="1" column="0">
<height>31</height> <widget class="QSpinBox" name="spinBoxGraspNumber">
</rect> <property name="minimum">
</property> <number>1</number>
<property name="text"> </property>
<string>Plan grasp(s)</string> <property name="maximum">
</property> <number>10000000</number>
</widget> </property>
<widget class="QGroupBox" name="groupBox_2"> </widget>
<property name="geometry"> </item>
<rect> <item row="2" column="0">
<x>10</x> <widget class="QLabel" name="label_12">
<y>70</y> <property name="text">
<width>181</width> <string>Timeout, seconds</string>
<height>221</height> </property>
</rect> </widget>
</property> </item>
<property name="title"> <item row="3" column="0">
<string>Grasp Planning Options</string> <widget class="QLabel" name="label">
</property> <property name="text">
<widget class="QCheckBox" name="checkBoxFoceClosure"> <string> (0=endless)</string>
<property name="geometry"> </property>
<rect> </widget>
<x>30</x> </item>
<y>190</y> <item row="4" column="0">
<width>131</width> <widget class="QSpinBox" name="spinBoxTimeOut">
<height>17</height> <property name="minimum">
</rect> <number>0</number>
</property> </property>
<property name="text"> <property name="maximum">
<string>Force closure</string> <number>10000000</number>
</property> </property>
<property name="checked"> <property name="value">
<bool>true</bool> <number>30</number>
</property> </property>
</widget> </widget>
<widget class="QSpinBox" name="spinBoxGraspNumber"> </item>
<property name="geometry"> <item row="5" column="0">
<rect> <widget class="QLabel" name="label_13">
<x>30</x> <property name="text">
<y>40</y> <string>Min quality</string>
<width>111</width> </property>
<height>22</height> </widget>
</rect> </item>
</property> <item row="6" column="0">
<property name="minimum"> <widget class="QDoubleSpinBox" name="doubleSpinBoxQuality">
<number>1</number> <property name="decimals">
</property> <number>3</number>
<property name="maximum"> </property>
<number>10000000</number> <property name="maximum">
</property> <double>1.000000000000000</double>
</widget> </property>
<widget class="QLabel" name="label_11"> <property name="singleStep">
<property name="geometry"> <double>0.001000000000000</double>
<rect> </property>
<x>10</x> <property name="value">
<y>20</y> <double>0.010000000000000</double>
<width>121</width> </property>
<height>16</height> </widget>
</rect> </item>
</property> <item row="7" column="0">
<property name="text"> <widget class="QCheckBox" name="checkBoxFoceClosure">
<string>Number of Grasps</string> <property name="text">
</property> <string>Force closure</string>
</widget> </property>
<widget class="QLabel" name="label_12"> <property name="checked">
<property name="geometry"> <bool>true</bool>
<rect> </property>
<x>10</x> </widget>
<y>70</y> </item>
<width>181</width> </layout>
<height>16</height> </widget>
</rect> </item>
</property> <item row="3" column="0">
<property name="text"> <widget class="QPushButton" name="pushButtonPlan">
<string>Timeout, seconds</string> <property name="text">
</property> <string>Plan grasp(s)</string>
</widget> </property>
<widget class="QSpinBox" name="spinBoxTimeOut"> </widget>
<property name="geometry"> </item>
<rect> <item row="8" column="0" colspan="2">
<x>30</x> <widget class="QCheckBox" name="checkBoxCones">
<y>110</y> <property name="text">
<width>111</width> <string>Friction Cones</string>
<height>22</height> </property>
</rect> </widget>
</property> </item>
<property name="minimum"> <item row="10" column="0" colspan="2">
<number>0</number> <widget class="QCheckBox" name="checkBoxHighlight">
</property> <property name="text">
<property name="maximum"> <string>Highlight</string>
<number>10000000</number> </property>
</property> </widget>
<property name="value"> </item>
<number>30</number> <item row="11" column="0" colspan="2">
</property> <widget class="QLabel" name="labelInfo">
</widget> <property name="text">
<widget class="QLabel" name="label_13"> <string>Grasps: 0</string>
<property name="geometry"> </property>
<rect> <property name="alignment">
<x>10</x> <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
<y>140</y> </property>
<width>171</width> </widget>
<height>16</height> </item>
</rect> <item row="12" column="0">
</property> <spacer name="verticalSpacer">
<property name="text"> <property name="orientation">
<string>Min quality</string> <enum>Qt::Vertical</enum>
</property> </property>
</widget> <property name="sizeHint" stdset="0">
<widget class="QDoubleSpinBox" name="doubleSpinBoxQuality"> <size>
<property name="geometry"> <width>20</width>
<rect> <height>40</height>
<x>30</x> </size>
<y>160</y> </property>
<width>111</width> </spacer>
<height>22</height> </item>
</rect> <item row="1" column="0" colspan="2">
</property> <widget class="QPushButton" name="pushButtonLoadObject">
<property name="decimals"> <property name="text">
<number>3</number> <string>Load Object</string>
</property> </property>
<property name="maximum"> </widget>
<double>1.000000000000000</double> </item>
</property> </layout>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>10</x>
<y>90</y>
<width>131</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string> (0=endless)</string>
</property>
</widget>
</widget>
<widget class="QPushButton" name="pushButtonSave">
<property name="geometry">
<rect>
<x>30</x>
<y>400</y>
<width>141</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Save</string>
</property>
</widget>
<widget class="QPushButton" name="pushButtonClose">
<property name="geometry">
<rect>
<x>30</x>
<y>340</y>
<width>71</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Close</string>
</property>
</widget>
<widget class="QPushButton" name="pushButtonOpen">
<property name="geometry">
<rect>
<x>110</x>
<y>340</y>
<width>71</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Open</string>
</property>
</widget>
<widget class="QLabel" name="labelInfo">
<property name="geometry">
<rect>
<x>20</x>
<y>560</y>
<width>161</width>
<height>171</height>
</rect>
</property>
<property name="text">
<string>Grasps: 0</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
</property>
</widget>
<widget class="QCheckBox" name="checkBoxHighlight">
<property name="geometry">
<rect>
<x>20</x>
<y>540</y>
<width>141</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Highlight</string>
</property>
</widget>
<widget class="QPushButton" name="pushButtonPlanBatch">
<property name="geometry">
<rect>
<x>110</x>
<y>300</y>
<width>81</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Plan batch</string>
</property>
</widget>
</widget> </widget>
</item> </item>
</layout> </layout>
......
...@@ -49,7 +49,6 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam ...@@ -49,7 +49,6 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam
// init the random number generator // init the random number generator
this->robotFile = robFile; this->robotFile = robFile;
this->objectFile = objFile;
this->eefName = eefName; this->eefName = eefName;
this->preshape = preshape; this->preshape = preshape;
eefVisu = nullptr; eefVisu = nullptr;
...@@ -75,7 +74,7 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam ...@@ -75,7 +74,7 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam
loadRobot(); loadRobot();
loadObject(); loadObject(objFile);
buildVisu(); buildVisu();
...@@ -262,7 +261,7 @@ void GraspPlannerWindow::buildVisu() ...@@ -262,7 +261,7 @@ void GraspPlannerWindow::buildVisu()
} }
// add approach dir visu // add approach dir visu
for (auto & contact : contacts) for (auto& contact : contacts)
{ {
SoSeparator* s = new SoSeparator; SoSeparator* s = new SoSeparator;
Eigen::Matrix4f ma; Eigen::Matrix4f ma;
...@@ -303,11 +302,11 @@ void GraspPlannerWindow::quit() ...@@ -303,11 +302,11 @@ void GraspPlannerWindow::quit()
SoQt::exitMainLoop(); SoQt::exitMainLoop();
} }
void GraspPlannerWindow::loadObject() void GraspPlannerWindow::loadObject(const string& objFile)
{ {
if (!objectFile.empty()) if (!objFile.empty())
{ {
object = ObjectIO::loadManipulationObject(objectFile); object = ObjectIO::loadManipulationObject(objFile);
} }
if (!object) if (!object)
...@@ -371,9 +370,13 @@ void GraspPlannerWindow::plan() ...@@ -371,9 +370,13 @@ void GraspPlannerWindow::plan()
float quality = (float)UI.doubleSpinBoxQuality->value(); float quality = (float)UI.doubleSpinBoxQuality->value();
int nrGrasps = UI.spinBoxGraspNumber->value(); int nrGrasps = UI.spinBoxGraspNumber->value();
if (planner) if (planner)
{
planner->setParameters(quality, forceClosure); planner->setParameters(quality, forceClosure);
}
else else
{
planner.reset(new GraspStudio::GenericGraspPlanner(grasps, qualityMeasure, approach, quality, forceClosure)); planner.reset(new GraspStudio::GenericGraspPlanner(grasps, qualityMeasure, approach, quality, forceClosure));
}
int nr = planner->plan(nrGrasps, timeout); int nr = planner->plan(nrGrasps, timeout);
VR_INFO << " Grasp planned:" << nr << endl; VR_INFO << " Grasp planned:" << nr << endl;
...@@ -475,7 +478,8 @@ void GraspPlannerWindow::openEEF() ...@@ -475,7 +478,8 @@ void GraspPlannerWindow::openEEF()
if (!preshape.empty()) if (!preshape.empty())
{ {
eefCloned->getEndEffector(eefName)->setPreshape(preshape); eefCloned->getEndEffector(eefName)->setPreshape(preshape);
} else }
else
{ {
eefCloned->getEndEffector(eefName)->openActors(); eefCloned->getEndEffector(eefName)->openActors();
} }
...@@ -509,12 +513,12 @@ void GraspPlannerWindow::save() ...@@ -509,12 +513,12 @@ void GraspPlannerWindow::save()
ManipulationObjectPtr objectM(new ManipulationObject(object->getName(), object->getVisualization()->clone(), object->getCollisionModel()->clone())); ManipulationObjectPtr objectM(new ManipulationObject(object->getName(), object->getVisualization()->clone(), object->getCollisionModel()->clone()));
objectM->addGraspSet(grasps); objectM->addGraspSet(grasps);
QString fi = QFileDialog::getSaveFileName(this, tr("Save ManipulationObject"), QString(), tr("XML Files (*.xml)")); QString fi = QFileDialog::getSaveFileName(this, tr("Save ManipulationObject"), QString(), tr("XML Files (*.xml)"));
objectFile = std::string(fi.toLatin1()); const std::string objFile(fi.toLatin1());
bool ok = false; bool ok = false;
try try
{ {
ok = ObjectIO::saveManipulationObject(objectM, objectFile); ok = ObjectIO::saveManipulationObject(objectM, objFile);
} }
catch (VirtualRobotException& e) catch (VirtualRobotException& e)
{ {
...@@ -576,45 +580,46 @@ void GraspPlannerWindow::planObjectBatch() ...@@ -576,45 +580,46 @@ void GraspPlannerWindow::planObjectBatch()
} }
fs << std::endl; fs << std::endl;
for(auto& path : paths) for (auto& path : paths)
{ {
try try
{ {
//resetSceneryAll(); //resetSceneryAll();
objectFile = path.toStdString(); loadObject(path.toStdString());
loadObject();
{ {
//planner->setRetreatOnLowContacts(false); //planner->setRetreatOnLowContacts(false);
plan(); plan();
// save(boost::filesystem::path(path.toStdString()).replace_extension(".moxml").string()); // save(boost::filesystem::path(path.toStdString()).replace_extension(".moxml").string());
float avgRate = 0; float avgRate = 0;
float avgForceClosureRate = 0; float avgForceClosureRate = 0;
float avgRateCol = 0; float avgRateCol = 0;
float avgForceClosureRateCol = 0; float avgForceClosureRateCol = 0;
size_t graspSum = 0; size_t graspSum = 0;
std::vector<double> histogramFC(bins,0.0); std::vector<double> histogramFC(bins, 0.0);
std::vector<double> histogramFCWithCollisions(bins,0.0); std::vector<double> histogramFCWithCollisions(bins, 0.0);
// if(planner->getPlannedGrasps().empty()) // if(planner->getPlannedGrasps().empty())
// { // {
// ARMARX_INFO << "No grasps found for" // ARMARX_INFO << "No grasps found for"
// } // }
for(VirtualRobot::GraspPtr& g : planner->getPlannedGrasps()) for (VirtualRobot::GraspPtr& g : planner->getPlannedGrasps())
{ {
GraspEvaluationPoseUncertainty::PoseEvalResults result; GraspEvaluationPoseUncertainty::PoseEvalResults result;
if (!evaluateGrasp(g, eefCloned, eefCloned->getEndEffector(eefName), 100, result)) if (!evaluateGrasp(g, eefCloned, eefCloned->getEndEffector(eefName), 100, result))
{
continue; continue;
}
VR_INFO << "Grasp " << graspSum << "/" << planner->getPlannedGrasps().size() << std::endl; VR_INFO << "Grasp " << graspSum << "/" << planner->getPlannedGrasps().size() << std::endl;
histogramFC.at(std::min<int>((int)(result.forceClosureRate * bins), bins-1))++; histogramFC.at(std::min<int>((int)(result.forceClosureRate * bins), bins - 1))++;
histogramFCWithCollisions.at(std::min<int>((int)((double)(result.numForceClosurePoses)/result.numPosesTested * bins), bins-1))++; histogramFCWithCollisions.at(std::min<int>((int)((double)(result.numForceClosurePoses) / result.numPosesTested * bins), bins - 1))++;
avgRate += result.avgQuality; avgRate += result.avgQuality;
avgForceClosureRate += result.forceClosureRate; avgForceClosureRate += result.forceClosureRate;
avgRateCol += result.avgQualityCol; avgRateCol += result.avgQualityCol;
avgForceClosureRateCol += result.forceClosureRateCol; avgForceClosureRateCol += result.forceClosureRateCol;
graspSum++; graspSum++;
// if(graspSum > 10) // if(graspSum > 10)
// break; // break;
} }
if (graspSum>0) if (graspSum > 0)
{ {
avgRate /= planner->getPlannedGrasps().size(); avgRate /= planner->getPlannedGrasps().size();
avgForceClosureRate /= planner->getPlannedGrasps().size(); avgForceClosureRate /= planner->getPlannedGrasps().size();
...@@ -623,35 +628,39 @@ void GraspPlannerWindow::planObjectBatch() ...@@ -623,35 +628,39 @@ void GraspPlannerWindow::planObjectBatch()
} }
fs << object->getName() << "," << planner->getEvaluation().toCSVString() << "," << avgRate << "," << avgForceClosureRate << "," << avgRateCol << "," << avgForceClosureRateCol; fs << object->getName() << "," << planner->getEvaluation().toCSVString() << "," << avgRate << "," << avgForceClosureRate << "," << avgRateCol << "," << avgForceClosureRateCol;
int i = 0; int i = 0;
for(auto bin : histogramFC) for (auto bin : histogramFC)
{ {
fs << ", " << (double)(bin)/graspSum; fs << ", " << (double)(bin) / graspSum;
cout << i << ": " << bin << ", " << graspSum << ", " << (double)(bin)/graspSum << std::endl; cout << i << ": " << bin << ", " << graspSum << ", " << (double)(bin) / graspSum << std::endl;
i++; i++;
} }
for(auto bin : histogramFCWithCollisions) for (auto bin : histogramFCWithCollisions)
{ {
fs << ", " << (double)(bin)/graspSum; fs << ", " << (double)(bin) / graspSum;
} }
fs << std::endl; fs << std::endl;
} }
} }
catch(std::exception & e) catch (std::exception& e)
{ {
VR_ERROR << "Failed to plan for " << path.toStdString() << "\nReason: \n" << e.what() << std::endl; VR_ERROR << "Failed to plan for " << path.toStdString() << "\nReason: \n" << e.what() << std::endl;
} }
progress.setValue(++i); progress.setValue(++i);
qApp->processEvents(); qApp->processEvents();
if (progress.wasCanceled()) if (progress.wasCanceled())
{
break; break;
}
} }
VR_INFO << "Saving CSV results to " << resultsCSVPath.string() << std::endl; VR_INFO << "Saving CSV results to " << resultsCSVPath.string() << std::endl;
} }
bool GraspPlannerWindow::evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::RobotPtr eefRobot, VirtualRobot::EndEffectorPtr eef, int nrEvalLoops, GraspEvaluationPoseUncertainty::PoseEvalResults &results) bool GraspPlannerWindow::evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::RobotPtr eefRobot, VirtualRobot::EndEffectorPtr eef, int nrEvalLoops, GraspEvaluationPoseUncertainty::PoseEvalResults& results)
{ {
if (!g || !eefRobot || !eef) if (!g || !eefRobot || !eef)
{
return false; return false;
}
GraspEvaluationPoseUncertaintyPtr eval(new GraspEvaluationPoseUncertainty(GraspEvaluationPoseUncertainty::PoseUncertaintyConfig())); GraspEvaluationPoseUncertaintyPtr eval(new GraspEvaluationPoseUncertainty(GraspEvaluationPoseUncertainty::PoseUncertaintyConfig()));
...@@ -660,3 +669,9 @@ bool GraspPlannerWindow::evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::R ...@@ -660,3 +669,9 @@ bool GraspPlannerWindow::evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::R
return true; return true;
} }
void GraspPlannerWindow::on_pushButtonLoadObject_clicked()
{
loadObject(QFileDialog::getOpenFileName(this, tr("Open Object File"), "", tr("XML Files (*.xml)")).toStdString());
buildVisu();
}
...@@ -38,7 +38,7 @@ class GraspPlannerWindow : public QMainWindow ...@@ -38,7 +38,7 @@ class GraspPlannerWindow : public QMainWindow
{ {
Q_OBJECT Q_OBJECT
public: public:
GraspPlannerWindow(std::string& robotFile, std::string& eefName, std::string& preshape, std::string& objectFile); GraspPlannerWindow(std::string& robotFile, std::string& eefName, std::string& preshape, std::string& objFile);
~GraspPlannerWindow() override; ~GraspPlannerWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
...@@ -67,10 +67,10 @@ public slots: ...@@ -67,10 +67,10 @@ public slots:
void planObjectBatch(); void planObjectBatch();
protected: protected:
bool evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::RobotPtr eefRobot, VirtualRobot::EndEffectorPtr eef, int nrEvalLoops, GraspStudio::GraspEvaluationPoseUncertainty::PoseEvalResults &results); bool evaluateGrasp(VirtualRobot::GraspPtr g, VirtualRobot::RobotPtr eefRobot, VirtualRobot::EndEffectorPtr eef, int nrEvalLoops, GraspStudio::GraspEvaluationPoseUncertainty::PoseEvalResults& results);
void loadRobot(); void loadRobot();
void loadObject(); void loadObject(const std::string& objFile);
void setupUI(); void setupUI();
...@@ -96,7 +96,6 @@ protected: ...@@ -96,7 +96,6 @@ protected:
std::string robotFile; std::string robotFile;
std::string objectFile;
std::string eefName; std::string eefName;
std::string preshape; std::string preshape;
...@@ -108,5 +107,7 @@ protected: ...@@ -108,5 +107,7 @@ protected:
boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot; boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot;
boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject; boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject;
private slots:
void on_pushButtonLoadObject_clicked();
}; };
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment