MAT planner example

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@442 042f3d55-54a8-47e9-b7fb-15903f145c44

GraspPlanning/CMakeLists.txt

@@ -51,6 +51,56 @@ Visualization/ConvexHullVisualization.h
 ${GRASPSTUDIO_SimoxDir}/VirtualRobot/definesVR.h
 )
 
+IF(BUILD_powercrust)
+	# Adding MATGraspPlanner
+	SET(SOURCES
+		${SOURCES}
+		GraspPlanner/MATPlanner/CandidateGrasp.cpp
+		GraspPlanner/MATPlanner/CandidateGraspGenerator.cpp
+		GraspPlanner/MATPlanner/Converter.cpp
+		GraspPlanner/MATPlanner/GraspPlannerConfiguration.cpp
+		GraspPlanner/MATPlanner/GridOfMedialSpheres.cpp
+		GraspPlanner/MATPlanner/GridParameters.cpp
+		GraspPlanner/MATPlanner/LocalNeighborhood.cpp
+		GraspPlanner/MATPlanner/MatGraspPlanner.cpp
+		GraspPlanner/MATPlanner/MedialSphere.cpp
+		GraspPlanner/MATPlanner/MeshConverter.cpp
+		GraspPlanner/MATPlanner/SphereHelpers.cpp
+		GraspPlanner/MATPlanner/StrOutHelpers.cpp
+		GraspPlanner/MATPlanner/TestCases.cpp
+		GraspPlanner/MATPlanner/CandidateGraspTester.cpp
+	)
+
+	SET(INCLUDES
+		${INCLUDES}
+		GraspPlanner/MATPlanner/CandidateGrasp.h
+		GraspPlanner/MATPlanner/CandidateGraspGenerator.h
+		GraspPlanner/MATPlanner/Converter.h
+		GraspPlanner/MATPlanner/GraspPlannerConfiguration.h
+		GraspPlanner/MATPlanner/GridOfMedialSpheres.h
+		GraspPlanner/MATPlanner/GridParameters.h
+		GraspPlanner/MATPlanner/LocalNeighborhood.h
+		GraspPlanner/MATPlanner/MatGraspPlanner.h
+		GraspPlanner/MATPlanner/MedialSphere.h
+		GraspPlanner/MATPlanner/MeshConverter.h
+		GraspPlanner/MATPlanner/SphereHelpers.h
+		GraspPlanner/MATPlanner/StrOutHelpers.h
+		GraspPlanner/MATPlanner/TestCases.h
+		GraspPlanner/MATPlanner/CandidateGraspTester.h
+	)
+
+	if (Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
+		SET(SOURCES
+			${SOURCES}
+			GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.cpp
+		)
+		SET(INCLUDES
+			${INCLUDES}
+			GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h
+		)
+	endif()
+ENDIF()
+
 if (Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
 	SET(SOURCES
 	${SOURCES}

GraspPlanning/examples/CMakeLists.txt

@@ -2,4 +2,6 @@
 
 ADD_SUBDIRECTORY(GraspQuality)
 ADD_SUBDIRECTORY(GraspPlanner)
-
+IF(BUILD_powercrust)
+	ADD_SUBDIRECTORY(MATGraspPlanner)
+ENDIF()
\ No newline at end of file

GraspPlanning/examples/MATGraspPlanner/CMakeLists.txt

+PROJECT ( MatGraspPlanner )
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4)
+
+CMAKE_POLICY(VERSION 2.6)
+	
+IF(Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
+
+	# the variable "demo_SRCS" contains all .cpp files of this project
+	FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/MatGraspPlannerApp.cpp ${PROJECT_SOURCE_DIR}/MatGraspPlannerWindow.cpp)
+	FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/MatGraspPlannerWindow.h)
+	
+    set(GUI_MOC_HDRS
+        ${PROJECT_SOURCE_DIR}/MatGraspPlannerWindow.h
+    )
+    set(GUI_UIS
+        ${PROJECT_SOURCE_DIR}/MatGraspPlanner.ui
+    )
+    
+    # create the executable
+    SimoxQtApplication(${PROJECT_NAME} "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}")
+    
+    SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
+    SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${GRASPSTUDIO_BIN_DIR})
+    
+     # install
+    INSTALL(TARGETS ${PROJECT_NAME}
+        DESTINATION ${GRASPSTUDIO_INSTALL_BIN_DIR}
+	COMPONENT Applications
+	)
+        
+    MESSAGE( STATUS ${PROJECT_NAME} " will be placed into " ${GRASPSTUDIO_BIN_DIR})
+    MESSAGE( STATUS ${PROJECT_NAME} " will be installed into " ${GRASPSTUDIO_INSTALL_BIN_DIR})
+ENDIF()

GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerApp.cpp

+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    GraspStudio
+* @author     Markus Przybylski
+* @copyright  2013 H2T,KIT
+*             GNU Lesser General Public License
+*
+*/
+#include <GraspPlanning/GraspStudio.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+
+#include <string>
+#include <iostream>
+
+using std::cout;
+using std::endl;
+using namespace VirtualRobot;
+using namespace GraspStudio;
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "MatGraspPlannerWindow.h"
+
+
+int main(int argc, char *argv[])
+{
+	SoDB::init();
+	SoQt::init(argc,argv,"MatGraspPlanner");
+	cout << " --- START --- " << endl;
+
+    /*std::string basepath(DEMO_BASE_DIR);
+    cout << "Base path:" << basepath << endl;
+    VirtualRobot::RuntimeEnvironment::addDataPath(basepath);*/
+
+	// --robot robots/iCub/iCub.xml --endeffector "Left Hand" --preshape "Grasp Preshape"
+	std::string robot("robots/ArmarIII/ArmarIII.xml");
+	VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robot);
+	std::string eef("Hand R");
+	//std::string object("objects/wok.xml");
+	std::string object("objects/riceBox.xml");
+    //std::string object("objects/WaterBottleSmall.xml");
+    //std::string object("../objects/361.xml");
+    VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(object);
+	std::string preshape("");
+
+	VirtualRobot::RuntimeEnvironment::considerKey("robot");
+	VirtualRobot::RuntimeEnvironment::considerKey("object");
+	VirtualRobot::RuntimeEnvironment::considerKey("endeffector");
+	VirtualRobot::RuntimeEnvironment::considerKey("preshape");
+	VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
+	VirtualRobot::RuntimeEnvironment::print();
+
+	std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
+	if (!robFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
+		robot = robFile;
+
+	std::string objFile = VirtualRobot::RuntimeEnvironment::getValue("object");
+	if (!objFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(objFile))
+		object = objFile;
+
+	std::string eefname = VirtualRobot::RuntimeEnvironment::getValue("endeffector");
+	if (!eefname.empty())
+		eef = eefname;
+
+	std::string ps = VirtualRobot::RuntimeEnvironment::getValue("preshape");
+	if (!ps.empty())
+		preshape = ps;
+
+
+	cout << "Using robot from " << robot << endl;
+	cout << "End effector:" << eef << ", preshape:" << preshape << endl;
+	cout << "Using object from " << object << endl;
+
+    MatGraspPlannerWindow rw(robot,eef,preshape,object);
+
+	rw.main();
+
+	return 0;
+}

GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h

+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    GraspStudio
+* @author     Markus Przybylski
+* @copyright  2013 H2T,KIT
+*             GNU Lesser General Public License
+*
+*/
+#ifndef __MatGraspPlanner_WINDOW_H_
+#define __MatGraspPlanner_WINDOW_H_
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/SceneIO.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Obstacle.h>
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/Obstacle.h>
+
+#include <string.h>
+#include <QtCore/QtGlobal>
+#include <QtGui/QtGui>
+#include <QtCore/QtCore>
+
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <vector>
+
+#include "ui_MatGraspPlanner.h"
+
+#include "GraspPlanner/MATPlanner/TestCases.h"
+#include "GraspPlanner/MATPlanner/StrOutHelpers.h"
+#include "GraspPlanning/GraspStudio.h"
+#include "GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h"
+#include "GraspPlanner/MATPlanner/MatGraspPlanner.h"  
+#include "GraspPlanner/MATPlanner/MeshConverter.h"
+#include "GraspPlanner/MATPlanner/GraspPlannerConfiguration.h"
+
+class MatGraspPlannerWindow : public QMainWindow
+{
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    Q_OBJECT
+public:
+    MatGraspPlannerWindow(std::string &robotFile, std::string &eefName, std::string &preshape, std::string &objectFile, Qt::WFlags flags = 0);
+    ~MatGraspPlannerWindow();
+
+	/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
+    int main();
+
+public slots:
+	/*! Closes the window and exits SoQt runloop. */
+	void quit();
+
+	/*!< Overriding the close event, so we know when the window was closed by the user. */
+	void closeEvent(QCloseEvent *event);
+
+	void resetSceneryAll();
+
+	void colModel();
+	void frictionConeVisu();
+	void showGrasps();
+
+    void showNextGrasp();
+
+	void buildVisu();
+
+	void plan();
+	void save();
+
+	void initPlanner();
+
+    void removeStuff();
+
+    //Helpers
+    void drawCandidateGrasp(GraspStudio::CandidateGraspPtr cg, float drawScaleCandidate);
+	void drawCandidateGrasps(std::vector<GraspStudio::CandidateGraspPtr>& cg, float drawScaleCandidates);
+	void drawLocalNeighborhood(GraspStudio::LocalNeighborhoodPtr nbhds,
+		GraspStudio::GraspPlannerConfigurationPtr gpConfig,
+                                float drawScaleNeighborhoods);
+	void drawLocalNeighborhoods(std::vector<GraspStudio::LocalNeighborhoodPtr>& nbhds,
+		GraspStudio::GraspPlannerConfigurationPtr gpConfig,
+                                float drawScaleNeighborhoods);
+    void drawSurfacePointCloud(float pointSize);
+	void drawMedialAxisPointCloud(std::vector<GraspStudio::MedialSpherePtr>& spheres,
+                                  float pointSize);
+	void drawMedialSpheres(std::vector<GraspStudio::MedialSpherePtr>& spheres, float maxRadius);
+	void drawMedialAxisPointCloudFiltered(std::vector<GraspStudio::MedialSpherePtr>& spheres,
+                                  float pointSize);
+	void drawMedialSpheresFiltered(std::vector<GraspStudio::MedialSpherePtr>& spheres, float maxRadius);
+	void drawSearchRadius(GraspStudio::LocalNeighborhoodPtr neighborhood);
+	void drawSearchRadii(std::vector<GraspStudio::LocalNeighborhoodPtr>& neighborhood);
+
+
+    void testStuff();
+
+    void testAllCandidates(); //GUI
+
+
+    void updateGraspPlannerConfigurationFromGui();
+
+    void selectObject();
+
+protected:
+
+    std::vector<Eigen::Vector3f> getSurfacePoints(float scaling = 1.0f, bool checkForDoubledEntries = false);
+    
+    void loadRobot();
+	void loadObject();
+    void loadObjectFromFile(std::string objectFilename);
+
+	void simulateOneGrasp(GraspStudio::CandidateGraspPtr candidate);
+	
+	void setupUI();
+
+	static void timerCB(void * data, SoSensor * sensor);
+    Ui::MatGraspPlanner UI;
+	SoQtExaminerViewer *viewer; /*!< Viewer to display the 3D model of the robot and the environment. */
+		
+	SoSeparator *sceneSep;
+	SoSeparator *eefClonedSep;
+	SoSeparator *objectSep;
+	SoSeparator *frictionConeSep;
+	SoSeparator *graspsSep;
+
+    SoSeparator *drawStuffSep;
+    SoSeparator *drawStuffGuiSep;
+    SoSeparator *medialAxisPointCloudSep;
+    SoSeparator *medialSpheresSep;
+    SoSeparator *medialAxisPointCloudFilteredSep;
+    SoSeparator *medialSpheresFilteredSep;
+    SoSeparator *neighborhoodSep;
+    SoSeparator *searchRadiusSep;
+    SoSeparator *candidateGraspsSep;
+
+	VirtualRobot::RobotPtr robot;
+	VirtualRobot::RobotPtr eefCloned;
+	VirtualRobot::ObstaclePtr object;
+	VirtualRobot::EndEffectorPtr eef;
+	    
+	VirtualRobot::GraspSetPtr grasps;
+
+
+	VirtualRobot::EndEffector::ContactInfoVector contacts;
+
+	
+	std::string robotFile;
+	std::string objectFile;
+	std::string eefName;
+	std::string preshape;
+
+	SoSeparator *eefVisu;
+
+	GraspStudio::GraspQualityMeasureWrenchSpacePtr qualityMeasure;
+	GraspStudio::MatGraspPlannerPtr planner;
+
+    boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationEEFCloned;
+    boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject;
+
+	GraspStudio::GraspPlannerConfigurationPtr gpConfig;
+	int candidateTestCounter;
+	std::vector<GraspStudio::CandidateGraspPtr> candidateGrasps;
+
+};
+
+#endif // __MatGraspPlanner_WINDOW_H_