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 tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
GraspPlanning/examples/GraspPlanner/GraspPlanner.ui
+ 201
− 303
View file @ d7e0ad25
......@@ -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>
......
GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+ 47
− 32
View file @ d7e0ad25
......@@ -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();
}
GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
+ 5
− 4
View file @ d7e0ad25
......@@ -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();
};
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment