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