From 414fc1905d778f31c78cfc073c96c545935d75a6 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Thu, 28 Nov 2013 12:53:16 +0000
Subject: [PATCH] MAT planner example
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@442 042f3d55-54a8-47e9-b7fb-15903f145c44
---
GraspPlanning/CMakeLists.txt | 50 +
.../ExternalDependencies/powercrust/io.cpp | 984 +++++++++---------
GraspPlanning/examples/CMakeLists.txt | 4 +-
.../examples/MATGraspPlanner/CMakeLists.txt | 34 +
.../MATGraspPlanner/MatGraspPlanner.ui | 713 +++++++++++++
.../MATGraspPlanner/MatGraspPlannerApp.cpp | 95 ++
.../MATGraspPlanner/MatGraspPlannerWindow.cpp | 973 +++++++++++++++++
.../MATGraspPlanner/MatGraspPlannerWindow.h | 186 ++++
8 files changed, 2546 insertions(+), 493 deletions(-)
create mode 100644 GraspPlanning/examples/MATGraspPlanner/CMakeLists.txt
create mode 100644 GraspPlanning/examples/MATGraspPlanner/MatGraspPlanner.ui
create mode 100644 GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerApp.cpp
create mode 100644 GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
create mode 100644 GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt
index 59730008b..edc705c81 100644
--- a/GraspPlanning/CMakeLists.txt
+++ b/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}
diff --git a/GraspPlanning/ExternalDependencies/powercrust/io.cpp b/GraspPlanning/ExternalDependencies/powercrust/io.cpp
index d7552b206..888da3231 100644
--- a/GraspPlanning/ExternalDependencies/powercrust/io.cpp
+++ b/GraspPlanning/ExternalDependencies/powercrust/io.cpp
@@ -1,493 +1,493 @@
-/* io.c : input-output */
-
-
-/*
- * Ken Clarkson wrote this. Copyright (c) 1995 by AT&T..
- * Permission to use, copy, modify, and distribute this software for any
- * purpose without fee is hereby granted, provided that this entire notice
- * is included in all copies of any software which is or includes a copy
- * or modification of this software and in all copies of the supporting
- * documentation for such software.
- * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
- * WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
- * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
- * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdarg.h>
-#include <assert.h>
-#include <float.h>
-#include <math.h>
-
-#include "hull.h"
-#include "powercrust.h"
-
-#ifdef WIN32
-#define popen _popen
-#define pclose _pclose
+/* io.c : input-output */
+
+
+/*
+ * Ken Clarkson wrote this. Copyright (c) 1995 by AT&T..
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose without fee is hereby granted, provided that this entire notice
+ * is included in all copies of any software which is or includes a copy
+ * or modification of this software and in all copies of the supporting
+ * documentation for such software.
+ * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
+ * WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
+ * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
+ * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <assert.h>
+#include <float.h>
+#include <math.h>
+
+#include "hull.h"
+#include "powercrust.h"
+
+#ifdef WIN32
+#define popen _popen
+#define pclose _pclose
#pragma warning(push)
-#pragma warning(disable: 4996) //4996 for _CRT_SECURE_NO_WARNINGS equivalent
-#endif
-
-namespace GraspStudio
-{
-
-namespace PowerCrust
-{
-
-void PowerCrustHull::panic(char *fmt, ...) {
- va_list args;
-
- va_start(args, fmt);
- vfprintf(DFILE, fmt, args);
- fflush(DFILE);
- va_end(args);
-
- exit(1);
-}
-
-/*
-FILE * popen(char*, char*);
-void pclose(FILE*);
-*/
-
-
-FILE* PowerCrustHull::efopen(char *file, char *mode) {
- FILE* fp;
- if ((fp = fopen(file, mode))!=NULL) return fp;
- fprintf(DFILE, "couldn't open file %s mode %s\n",file,mode);
- exit(1);
- return NULL;
-}
-
-void PowerCrustHull::efclose(FILE* file) {
- fclose(file);
- file = NULL;
-}
-
-
-FILE* PowerCrustHull::epopen(char *com, char *mode) {
- FILE* fp;
- if ((fp = popen(com, mode))!=NULL) return fp;
- fprintf(stderr, "couldn't open stream %s mode %s\n",com,mode);
- exit(1);
- return 0;
-}
-
-
-
-void PowerCrustHull::print_neighbor_snum(FILE* F, neighbor *n){
-
- //assert(pc->site_numm!=NULL);
- if (n->vert)
- fprintf(F, "%ld ", pc->site_numm(n->vert));
- else
- fprintf(F, "NULL vert ");
- fflush(stdout);
-}
-
-void PowerCrustHull::print_basis(FILE *F, basis_s* b) {
- if (!b) {fprintf(F, "NULL basis ");fflush(stdout);return;}
- if (b->lscale<0) {fprintf(F, "\nbasis computed");return;}
- fprintf(F, "\n%p %d \n b=",(void*)b,b->lscale);
- print_point(F, rdim, b->vecs);
- fprintf(F, "\n a= ");
- print_point_int(F, rdim, b->vecs+rdim); fprintf(F, " ");
- fflush(F);
-}
-
-void PowerCrustHull::print_simplex_num(FILE *F, simplex *s) {
- fprintf(F, "simplex ");
- if(!s) fprintf(F, "NULL ");
- else fprintf(F, "%p ", (void*)s);
-}
-
-void PowerCrustHull::print_neighbor_full(FILE *F, neighbor *n){
-
- if (!n) {fprintf(F, "null neighbor\n");return;}
-
- print_simplex_num(F, n->simp);
- print_neighbor_snum(F, n);fprintf(F, ": ");fflush(F);
- if (n->vert) {
- /* if (n->basis && n->basis->lscale <0) fprintf(F, "trans ");*/
- /* else */ print_point(F, pdim,n->vert);
- fflush(F);
- }
- print_basis(F, n->basis);
- fflush(F);
- fprintf(F, "\n");
-}
-
-void *PowerCrustHull::print_facet(FILE *F, simplex *s, print_neighbor_f pnfin) {
- int i;
- neighbor *sn = s->neigh;
-
- /* fprintf(F, "%d ", s->mark);*/
- for (i=0; i<cdim;i++,sn++) (*this.*pnfin)(F, sn);
- fprintf(F, "\n");
- fflush(F);
- return NULL;
-}
-
-
-
-
-void *PowerCrustHull::print_simplex_f(simplex *s, FILE *F, print_neighbor_f pnfin)
-{
- if (pnfin) {
- if (!s) return NULL;
- }
-
- print_simplex_num(F, s);
- fprintf(F, "\n");
- if (!s) return NULL;
- fprintf(F, "normal ="); print_basis(F, s->normal); fprintf(F, "\n");
- fprintf(F, "peak ="); (*this.*pnfin)(F, &(s->peak));
- fprintf (F, "facet =\n");fflush(F);
- return print_facet(F, s, pnfin);
-}
-
-void *PowerCrustHull::print_simplex(simplex *s, void *Fin) {
- static FILE *F;
-
- if (Fin) {F=(FILE*)Fin; if (!s) return NULL;}
-
- return print_simplex_f(s, F, 0);
-
-}
-
-
-void PowerCrustHull::print_triang(simplex *root, FILE *F, print_neighbor_f pnf) {
- print_simplex(0,F);
- print_simplex_f(0,0,pnf);
- visit_triang(root, &GraspStudio::PowerCrust::PowerCrustHull::print_simplex);
-}
-
-void *PowerCrustHull::p_peak_test(simplex *s) {return (s->peak.vert==pc->p) ? (void*)s : (void*)NULL;}
-
-
-
-void *PowerCrustHull::check_simplex(simplex *s, void *dum){
-
- int i,j,k,l;
- neighbor *sn, *snn, *sn2;
- simplex *sns;
- site vn;
-
- for (i=-1,sn=s->neigh-1;i<cdim;i++,sn++) {
- sns = sn->simp;
- if (!sns) {
- fprintf(DFILE, "check_triang; bad simplex\n");
- print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- fprintf(DFILE, "site_num(p)=%ld\n", pc->site_numm(pc->p));
- return s;
- }
- if (!s->peak.vert && sns->peak.vert && i!=-1) {
- fprintf(DFILE, "huh?\n");
- print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- exit(1);
- }
- for (j=-1,snn=sns->neigh-1; j<cdim && snn->simp!=s; j++,snn++);
- if (j==cdim) {
- fprintf(DFILE, "adjacency failure:\n");
- //DEBEXP(-1,site_num(p))
- print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- exit(1);
- }
- for (k=-1,snn=sns->neigh-1; k<cdim; k++,snn++){
- vn = snn->vert;
- if (k!=j) {
- for (l=-1,sn2 = s->neigh-1;
- l<cdim && sn2->vert != vn;
- l++,sn2++);
- if (l==cdim) {
- fprintf(DFILE, "cdim=%d\n",cdim);
- fprintf(DFILE, "error: neighboring simplices with incompatible vertices:\n");
- print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- exit(1);
- }
- }
- }
- }
- return NULL;
-}
-
-int PowerCrustHull::p_neight(simplex *s, int i, void *dum) {return s->neigh[i].vert !=pc->p;}
-
-void PowerCrustHull::check_triang(simplex *root){visit_triang(root, &GraspStudio::PowerCrust::PowerCrustHull::check_simplex);}
-
-void PowerCrustHull::check_new_triangs(simplex *s){visit_triang_gen(s, &GraspStudio::PowerCrust::PowerCrustHull::check_simplex, &GraspStudio::PowerCrust::PowerCrustHull::p_neight);}
-
-
-/* outfuncs: given a list of points, output in a given format */
-
-void PowerCrustHull::vv_out(point *v, int vdim, FILE *Fin, int amble) {
- /* sunghee */
- static FILE *F;
- int i,j;
-
- if (Fin) {F=Fin; if (!v) return;}
- for (j=0;j<vdim;j++) {
- for (i=0;i<3;i++) {
- fprintf(F, "%G ", v[j][i]/pc->mult_up);
- }
- fprintf(F, " | ");
- }
- fprintf(F, "\n");
- return;
-}
-
-void PowerCrustHull::vlist_out(point *v, int vdim, FILE *Fin, int amble) {
-
- static FILE *F;
- int j;
-
- if (Fin) {F=Fin; if (!v) return;}
-
- for (j=0;j<vdim;j++) fprintf(F, "%ld ", (pc->site_numm)(v[j]));
- fprintf(F,"\n");
-
- return;
-}
-
-void PowerCrustHull::off_out(point *v, int vdim, FILE *Fin, int amble) {
-
- static FILE *F, *G;
- static FILE *OFFFILE;
- //static char offfilenam[L_tmpnam]; //MP
- static char offfilenam[10000]; //MP
- char comst[100], buf[200];
- int j,i;
-
- if (Fin) {F=Fin;}
-
- if (pdim!=3) { VR_WARNING << "off apparently for 3d points only" << endl; return;}
-
- if (amble==0) {
- for (i=0;i<vdim;i++) if (v[i]==infinity) return;
- fprintf(OFFFILE, "%d ", vdim);
- for (j=0;j<vdim;j++) fprintf(OFFFILE, "%ld ", (pc->site_numm)(v[j]));
- fprintf(OFFFILE,"\n");
- } else if (amble==-1) {
- OFFFILE = efopen(tmpnam(offfilenam), "w");
- } else {
- efclose(OFFFILE);
-
- fprintf(F, " OFF\n");
-
- sprintf(comst, "wc %s", tmpfilenam);
- G = epopen(comst, "r");
- fscanf(G, "%d", &i);
- fprintf(F, " %d", i);
- pclose(G);
-
- sprintf(comst, "wc %s", offfilenam);
- G = epopen(comst, "r");
- fscanf(G, "%d", &i);
- fprintf(F, " %d", i);
- pclose(G);
-
- fprintf (F, " 0\n");
-
- G = efopen(tmpfilenam, "r");
- while (fgets(buf, sizeof(buf), G)) fprintf(F, "%s", buf);
- efclose(G);
-
- G = efopen(offfilenam, "r");
-
-
- while (fgets(buf, sizeof(buf), G)) fprintf(F, "%s", buf);
- efclose(G);
- }
-
-}
-
-
-
-void PowerCrustHull::mp_out(point *v, int vdim, FILE *Fin, int amble) {
-
-
- /* should fix scaling */
-
- static int figno=1;
- static FILE *F;
-
- if (Fin) {F=Fin;}
-
- if (pdim!=2) { VR_WARNING << " mp for planar points only" << endl; return;}
- if (amble==0) {
- int i;
- if (!v) return;
- for (i=0;i<vdim;i++) if (v[i]==infinity) {
- point t=v[i];
- v[i]=v[vdim-1];
- v[vdim-1] = t;
- vdim--;
- break;
- }
- fprintf(F, "draw ");
- for (i=0;i<vdim;i++)
- fprintf(F,
- (i+1<vdim) ? "(%Gu,%Gu)--" : "(%Gu,%Gu);\n",
- v[i][0]/pc->mult_up,v[i][1]/pc->mult_up
- );
- } else if (amble==-1) {
- if (figno==1) fprintf(F, "u=1pt;\n");
- fprintf(F , "beginfig(%d);\n",figno++);
- } else if (amble==1) {
- fprintf(F , "endfig;\n");
- }
-}
-
-
-void PowerCrustHull::ps_out(point *v, int vdim, FILE *Fin, int amble) {
-
- static FILE *F;
- static double scaler;
-
- if (Fin) {F=Fin;}
-
- if (pdim!=2) { VR_WARNING << "ps for planar points only" << endl; return;}
-
- if (amble==0) {
- int i;
- if (!v) return;
- for (i=0;i<vdim;i++) if (v[i]==infinity) {
- point t=v[i];
- v[i]=v[vdim-1];
- v[vdim-1] = t;
- vdim--;
- break;
- }
- fprintf(F,
- "newpath %G %G moveto\n",
- v[0][0]*scaler,v[0][1]*scaler);
- for (i=1;i<vdim;i++)
- fprintf(F,
- "%G %G lineto\n",
- v[i][0]*scaler,v[i][1]*scaler
- );
- fprintf(F, "stroke\n");
- } else if (amble==-1) {
- float len[2], maxlen;
- fprintf(F, "%%!PS\n");
- len[0] = (float)(pc->maxs[0]-pc->mins[0]);
- len[1] = (float)(pc->maxs[1]-pc->mins[1]);
- maxlen = (len[0]>len[1]) ? len[0] : len[1];
- scaler = 216/maxlen;
-
- fprintf(F, "%%%%BoundingBox: %G %G %G %G \n",
- pc->mins[0]*scaler,
- pc->mins[1]*scaler,
- pc->maxs[0]*scaler,
- pc->maxs[1]*scaler);
- fprintf(F, "%%%%Creator: hull program\n");
- fprintf(F, "%%%%Pages: 1\n");
- fprintf(F, "%%%%EndProlog\n");
- fprintf(F, "%%%%Page: 1 1\n");
- fprintf(F, " 0.5 setlinewidth [] 0 setdash\n");
- fprintf(F, " 1 setlinecap 1 setlinejoin 10 setmiterlimit\n");
- } else if (amble==1) {
- fprintf(F , "showpage\n %%%%EOF\n");
- }
-}
-
-void PowerCrustHull::cpr_out(point *v, int vdim, FILE *Fin, int amble) {
-
- static FILE *F;
- int i;
-
- if (Fin) {F=Fin; if (!v) return;}
-
- if (pdim!=3) { VR_WARNING << "cpr for 3d points only" << endl; return;}
-
- for (i=0;i<vdim;i++) if (v[i]==infinity) return;
-
- fprintf(F, "t %G %G %G %G %G %G %G %G %G 3 128\n",
- v[0][0]/pc->mult_up,v[0][1]/pc->mult_up,v[0][2]/pc->mult_up,
- v[1][0]/pc->mult_up,v[1][1]/pc->mult_up,v[1][2]/pc->mult_up,
- v[2][0]/pc->mult_up,v[2][1]/pc->mult_up,v[2][2]/pc->mult_up
- );
-}
-
-
-/* vist_funcs for different kinds of output: facets, alpha shapes, etc. */
-
-
-
-void *PowerCrustHull::facets_print(simplex *s, void *p) {
-
- static out_func *out_func_here;
- point v[MAXDIM];
- int j;
-
- if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
-
- for (j=0;j<cdim;j++) v[j] = s->neigh[j].vert;
-
- (*this.**out_func_here)(v,cdim,0,0);
-
- return NULL;
-}
-
-void *PowerCrustHull::ridges_print(simplex *s, void *p) {
-
- static out_func *out_func_here;
- point v[MAXDIM];
- int j,k,vnum;
-
- if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
-
- for (j=0;j<cdim;j++) {
- vnum=0;
- for (k=0;k<cdim;k++) {
- if (k==j) continue;
- v[vnum++] = (s->neigh[k].vert);
- }
- (*this.**out_func_here)(v,cdim-1,0,0);
- }
- return NULL;
-}
-
-
-void *PowerCrustHull::afacets_print(simplex *s, void *p) {
-
- static out_func *out_func_here;
- point v[MAXDIM];
- int j,k,vnum;
-
- if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
-
- for (j=0;j<cdim;j++) { /* check for ashape consistency */
- for (k=0;k<cdim;k++) if (s->neigh[j].simp->neigh[k].simp==s) break;
- if (alph_test(s,j,0)!=alph_test(s->neigh[j].simp,k,0)) {
- //DEB(-10,alpha-shape not consistent)
- // DEBTR(-10)
- print_simplex_f(s,DFILE,&GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- print_simplex_f(s->neigh[j].simp,DFILE,&GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
- fflush(DFILE);
- exit(1);
- }
- }
- for (j=0;j<cdim;j++) {
- vnum=0;
- if (alph_test(s,j,0)) continue;
- for (k=0;k<cdim;k++) {
- if (k==j) continue;
- v[vnum++] = s->neigh[k].vert;
- }
- (*this.**out_func_here)(v,cdim-1,0,0);
- }
- return NULL;
-}
-
-}
-}
-#ifdef WIN32
-#pragma warning(pop)
-#endif
+#pragma warning(disable: 4996) //4996 for _CRT_SECURE_NO_WARNINGS equivalent
+#endif
+
+namespace GraspStudio
+{
+
+namespace PowerCrust
+{
+
+void PowerCrustHull::panic(char *fmt, ...) {
+ va_list args;
+
+ va_start(args, fmt);
+ vfprintf(DFILE, fmt, args);
+ fflush(DFILE);
+ va_end(args);
+
+ exit(1);
+}
+
+/*
+FILE * popen(char*, char*);
+void pclose(FILE*);
+*/
+
+
+FILE* PowerCrustHull::efopen(char *file, char *mode) {
+ FILE* fp;
+ if ((fp = fopen(file, mode))!=NULL) return fp;
+ fprintf(DFILE, "couldn't open file %s mode %s\n",file,mode);
+ exit(1);
+ return NULL;
+}
+
+void PowerCrustHull::efclose(FILE* file) {
+ fclose(file);
+ file = NULL;
+}
+
+
+FILE* PowerCrustHull::epopen(char *com, char *mode) {
+ FILE* fp;
+ if ((fp = popen(com, mode))!=NULL) return fp;
+ fprintf(stderr, "couldn't open stream %s mode %s\n",com,mode);
+ exit(1);
+ return 0;
+}
+
+
+
+void PowerCrustHull::print_neighbor_snum(FILE* F, neighbor *n){
+
+ //assert(pc->site_numm!=NULL);
+ if (n->vert)
+ fprintf(F, "%ld ", pc->site_numm(n->vert));
+ else
+ fprintf(F, "NULL vert ");
+ fflush(stdout);
+}
+
+void PowerCrustHull::print_basis(FILE *F, basis_s* b) {
+ if (!b) {fprintf(F, "NULL basis ");fflush(stdout);return;}
+ if (b->lscale<0) {fprintf(F, "\nbasis computed");return;}
+ fprintf(F, "\n%p %d \n b=",(void*)b,b->lscale);
+ print_point(F, rdim, b->vecs);
+ fprintf(F, "\n a= ");
+ print_point_int(F, rdim, b->vecs+rdim); fprintf(F, " ");
+ fflush(F);
+}
+
+void PowerCrustHull::print_simplex_num(FILE *F, simplex *s) {
+ fprintf(F, "simplex ");
+ if(!s) fprintf(F, "NULL ");
+ else fprintf(F, "%p ", (void*)s);
+}
+
+void PowerCrustHull::print_neighbor_full(FILE *F, neighbor *n){
+
+ if (!n) {fprintf(F, "null neighbor\n");return;}
+
+ print_simplex_num(F, n->simp);
+ print_neighbor_snum(F, n);fprintf(F, ": ");fflush(F);
+ if (n->vert) {
+ /* if (n->basis && n->basis->lscale <0) fprintf(F, "trans ");*/
+ /* else */ print_point(F, pdim,n->vert);
+ fflush(F);
+ }
+ print_basis(F, n->basis);
+ fflush(F);
+ fprintf(F, "\n");
+}
+
+void *PowerCrustHull::print_facet(FILE *F, simplex *s, print_neighbor_f pnfin) {
+ int i;
+ neighbor *sn = s->neigh;
+
+ /* fprintf(F, "%d ", s->mark);*/
+ for (i=0; i<cdim;i++,sn++) (*this.*pnfin)(F, sn);
+ fprintf(F, "\n");
+ fflush(F);
+ return NULL;
+}
+
+
+
+
+void *PowerCrustHull::print_simplex_f(simplex *s, FILE *F, print_neighbor_f pnfin)
+{
+ if (pnfin) {
+ if (!s) return NULL;
+ }
+
+ print_simplex_num(F, s);
+ fprintf(F, "\n");
+ if (!s) return NULL;
+ fprintf(F, "normal ="); print_basis(F, s->normal); fprintf(F, "\n");
+ fprintf(F, "peak ="); (*this.*pnfin)(F, &(s->peak));
+ fprintf (F, "facet =\n");fflush(F);
+ return print_facet(F, s, pnfin);
+}
+
+void *PowerCrustHull::print_simplex(simplex *s, void *Fin) {
+ static FILE *F;
+
+ if (Fin) {F=(FILE*)Fin; if (!s) return NULL;}
+
+ return print_simplex_f(s, F, 0);
+
+}
+
+
+void PowerCrustHull::print_triang(simplex *root, FILE *F, print_neighbor_f pnf) {
+ print_simplex(0,F);
+ print_simplex_f(0,0,pnf);
+ visit_triang(root, &GraspStudio::PowerCrust::PowerCrustHull::print_simplex);
+}
+
+void *PowerCrustHull::p_peak_test(simplex *s) {return (s->peak.vert==pc->p) ? (void*)s : (void*)NULL;}
+
+
+
+void *PowerCrustHull::check_simplex(simplex *s, void *dum){
+
+ int i,j,k,l;
+ neighbor *sn, *snn, *sn2;
+ simplex *sns;
+ site vn;
+
+ for (i=-1,sn=s->neigh-1;i<cdim;i++,sn++) {
+ sns = sn->simp;
+ if (!sns) {
+ fprintf(DFILE, "check_triang; bad simplex\n");
+ print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ fprintf(DFILE, "site_num(p)=%ld\n", pc->site_numm(pc->p));
+ return s;
+ }
+ if (!s->peak.vert && sns->peak.vert && i!=-1) {
+ fprintf(DFILE, "huh?\n");
+ print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ exit(1);
+ }
+ for (j=-1,snn=sns->neigh-1; j<cdim && snn->simp!=s; j++,snn++);
+ if (j==cdim) {
+ fprintf(DFILE, "adjacency failure:\n");
+ //DEBEXP(-1,site_num(p))
+ print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ exit(1);
+ }
+ for (k=-1,snn=sns->neigh-1; k<cdim; k++,snn++){
+ vn = snn->vert;
+ if (k!=j) {
+ for (l=-1,sn2 = s->neigh-1;
+ l<cdim && sn2->vert != vn;
+ l++,sn2++);
+ if (l==cdim) {
+ fprintf(DFILE, "cdim=%d\n",cdim);
+ fprintf(DFILE, "error: neighboring simplices with incompatible vertices:\n");
+ print_simplex_f(sns, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ print_simplex_f(s, DFILE, &GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ exit(1);
+ }
+ }
+ }
+ }
+ return NULL;
+}
+
+int PowerCrustHull::p_neight(simplex *s, int i, void *dum) {return s->neigh[i].vert !=pc->p;}
+
+void PowerCrustHull::check_triang(simplex *root){visit_triang(root, &GraspStudio::PowerCrust::PowerCrustHull::check_simplex);}
+
+void PowerCrustHull::check_new_triangs(simplex *s){visit_triang_gen(s, &GraspStudio::PowerCrust::PowerCrustHull::check_simplex, &GraspStudio::PowerCrust::PowerCrustHull::p_neight);}
+
+
+/* outfuncs: given a list of points, output in a given format */
+
+void PowerCrustHull::vv_out(point *v, int vdim, FILE *Fin, int amble) {
+ /* sunghee */
+ static FILE *F;
+ int i,j;
+
+ if (Fin) {F=Fin; if (!v) return;}
+ for (j=0;j<vdim;j++) {
+ for (i=0;i<3;i++) {
+ fprintf(F, "%G ", v[j][i]/pc->mult_up);
+ }
+ fprintf(F, " | ");
+ }
+ fprintf(F, "\n");
+ return;
+}
+
+void PowerCrustHull::vlist_out(point *v, int vdim, FILE *Fin, int amble) {
+
+ static FILE *F;
+ int j;
+
+ if (Fin) {F=Fin; if (!v) return;}
+
+ for (j=0;j<vdim;j++) fprintf(F, "%ld ", (pc->site_numm)(v[j]));
+ fprintf(F,"\n");
+
+ return;
+}
+
+void PowerCrustHull::off_out(point *v, int vdim, FILE *Fin, int amble) {
+
+ static FILE *F, *G;
+ static FILE *OFFFILE;
+ //static char offfilenam[L_tmpnam]; //MP
+ static char offfilenam[10000]; //MP
+ char comst[100], buf[200];
+ int j,i;
+
+ if (Fin) {F=Fin;}
+
+ if (pdim!=3) { VR_WARNING << "off apparently for 3d points only" << endl; return;}
+
+ if (amble==0) {
+ for (i=0;i<vdim;i++) if (v[i]==infinity) return;
+ fprintf(OFFFILE, "%d ", vdim);
+ for (j=0;j<vdim;j++) fprintf(OFFFILE, "%ld ", (pc->site_numm)(v[j]));
+ fprintf(OFFFILE,"\n");
+ } else if (amble==-1) {
+ OFFFILE = efopen(tmpnam(offfilenam), "w");
+ } else {
+ efclose(OFFFILE);
+
+ fprintf(F, " OFF\n");
+
+ sprintf(comst, "wc %s", tmpfilenam);
+ G = epopen(comst, "r");
+ fscanf(G, "%d", &i);
+ fprintf(F, " %d", i);
+ pclose(G);
+
+ sprintf(comst, "wc %s", offfilenam);
+ G = epopen(comst, "r");
+ fscanf(G, "%d", &i);
+ fprintf(F, " %d", i);
+ pclose(G);
+
+ fprintf (F, " 0\n");
+
+ G = efopen(tmpfilenam, "r");
+ while (fgets(buf, sizeof(buf), G)) fprintf(F, "%s", buf);
+ efclose(G);
+
+ G = efopen(offfilenam, "r");
+
+
+ while (fgets(buf, sizeof(buf), G)) fprintf(F, "%s", buf);
+ efclose(G);
+ }
+
+}
+
+
+
+void PowerCrustHull::mp_out(point *v, int vdim, FILE *Fin, int amble) {
+
+
+ /* should fix scaling */
+
+ static int figno=1;
+ static FILE *F;
+
+ if (Fin) {F=Fin;}
+
+ if (pdim!=2) { VR_WARNING << " mp for planar points only" << endl; return;}
+ if (amble==0) {
+ int i;
+ if (!v) return;
+ for (i=0;i<vdim;i++) if (v[i]==infinity) {
+ point t=v[i];
+ v[i]=v[vdim-1];
+ v[vdim-1] = t;
+ vdim--;
+ break;
+ }
+ fprintf(F, "draw ");
+ for (i=0;i<vdim;i++)
+ fprintf(F,
+ (i+1<vdim) ? "(%Gu,%Gu)--" : "(%Gu,%Gu);\n",
+ v[i][0]/pc->mult_up,v[i][1]/pc->mult_up
+ );
+ } else if (amble==-1) {
+ if (figno==1) fprintf(F, "u=1pt;\n");
+ fprintf(F , "beginfig(%d);\n",figno++);
+ } else if (amble==1) {
+ fprintf(F , "endfig;\n");
+ }
+}
+
+
+void PowerCrustHull::ps_out(point *v, int vdim, FILE *Fin, int amble) {
+
+ static FILE *F;
+ static double scaler;
+
+ if (Fin) {F=Fin;}
+
+ if (pdim!=2) { VR_WARNING << "ps for planar points only" << endl; return;}
+
+ if (amble==0) {
+ int i;
+ if (!v) return;
+ for (i=0;i<vdim;i++) if (v[i]==infinity) {
+ point t=v[i];
+ v[i]=v[vdim-1];
+ v[vdim-1] = t;
+ vdim--;
+ break;
+ }
+ fprintf(F,
+ "newpath %G %G moveto\n",
+ v[0][0]*scaler,v[0][1]*scaler);
+ for (i=1;i<vdim;i++)
+ fprintf(F,
+ "%G %G lineto\n",
+ v[i][0]*scaler,v[i][1]*scaler
+ );
+ fprintf(F, "stroke\n");
+ } else if (amble==-1) {
+ float len[2], maxlen;
+ fprintf(F, "%%!PS\n");
+ len[0] = (float)(pc->maxs[0]-pc->mins[0]);
+ len[1] = (float)(pc->maxs[1]-pc->mins[1]);
+ maxlen = (len[0]>len[1]) ? len[0] : len[1];
+ scaler = 216/maxlen;
+
+ fprintf(F, "%%%%BoundingBox: %G %G %G %G \n",
+ pc->mins[0]*scaler,
+ pc->mins[1]*scaler,
+ pc->maxs[0]*scaler,
+ pc->maxs[1]*scaler);
+ fprintf(F, "%%%%Creator: hull program\n");
+ fprintf(F, "%%%%Pages: 1\n");
+ fprintf(F, "%%%%EndProlog\n");
+ fprintf(F, "%%%%Page: 1 1\n");
+ fprintf(F, " 0.5 setlinewidth [] 0 setdash\n");
+ fprintf(F, " 1 setlinecap 1 setlinejoin 10 setmiterlimit\n");
+ } else if (amble==1) {
+ fprintf(F , "showpage\n %%%%EOF\n");
+ }
+}
+
+void PowerCrustHull::cpr_out(point *v, int vdim, FILE *Fin, int amble) {
+
+ static FILE *F;
+ int i;
+
+ if (Fin) {F=Fin; if (!v) return;}
+
+ if (pdim!=3) { VR_WARNING << "cpr for 3d points only" << endl; return;}
+
+ for (i=0;i<vdim;i++) if (v[i]==infinity) return;
+
+ fprintf(F, "t %G %G %G %G %G %G %G %G %G 3 128\n",
+ v[0][0]/pc->mult_up,v[0][1]/pc->mult_up,v[0][2]/pc->mult_up,
+ v[1][0]/pc->mult_up,v[1][1]/pc->mult_up,v[1][2]/pc->mult_up,
+ v[2][0]/pc->mult_up,v[2][1]/pc->mult_up,v[2][2]/pc->mult_up
+ );
+}
+
+
+/* vist_funcs for different kinds of output: facets, alpha shapes, etc. */
+
+
+
+void *PowerCrustHull::facets_print(simplex *s, void *p) {
+
+ static out_func *out_func_here;
+ point v[MAXDIM];
+ int j;
+
+ if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
+
+ for (j=0;j<cdim;j++) v[j] = s->neigh[j].vert;
+
+ (*this.**out_func_here)(v,cdim,0,0);
+
+ return NULL;
+}
+
+void *PowerCrustHull::ridges_print(simplex *s, void *p) {
+
+ static out_func *out_func_here;
+ point v[MAXDIM];
+ int j,k,vnum;
+
+ if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
+
+ for (j=0;j<cdim;j++) {
+ vnum=0;
+ for (k=0;k<cdim;k++) {
+ if (k==j) continue;
+ v[vnum++] = (s->neigh[k].vert);
+ }
+ (*this.**out_func_here)(v,cdim-1,0,0);
+ }
+ return NULL;
+}
+
+
+void *PowerCrustHull::afacets_print(simplex *s, void *p) {
+
+ static out_func *out_func_here;
+ point v[MAXDIM];
+ int j,k,vnum;
+
+ if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}
+
+ for (j=0;j<cdim;j++) { /* check for ashape consistency */
+ for (k=0;k<cdim;k++) if (s->neigh[j].simp->neigh[k].simp==s) break;
+ if (alph_test(s,j,0)!=alph_test(s->neigh[j].simp,k,0)) {
+ //DEB(-10,alpha-shape not consistent)
+ // DEBTR(-10)
+ print_simplex_f(s,DFILE,&GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ print_simplex_f(s->neigh[j].simp,DFILE,&GraspStudio::PowerCrust::PowerCrustHull::print_neighbor_full);
+ fflush(DFILE);
+ exit(1);
+ }
+ }
+ for (j=0;j<cdim;j++) {
+ vnum=0;
+ if (alph_test(s,j,0)) continue;
+ for (k=0;k<cdim;k++) {
+ if (k==j) continue;
+ v[vnum++] = s->neigh[k].vert;
+ }
+ (*this.**out_func_here)(v,cdim-1,0,0);
+ }
+ return NULL;
+}
+
+}
+}
+#ifdef WIN32
+#pragma warning(pop)
+#endif
diff --git a/GraspPlanning/examples/CMakeLists.txt b/GraspPlanning/examples/CMakeLists.txt
index c1abfa61c..8367cb774 100644
--- a/GraspPlanning/examples/CMakeLists.txt
+++ b/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
diff --git a/GraspPlanning/examples/MATGraspPlanner/CMakeLists.txt b/GraspPlanning/examples/MATGraspPlanner/CMakeLists.txt
new file mode 100644
index 000000000..26fb7857a
--- /dev/null
+++ b/GraspPlanning/examples/MATGraspPlanner/CMakeLists.txt
@@ -0,0 +1,34 @@
+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()
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlanner.ui b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlanner.ui
new file mode 100644
index 000000000..7fcf383ca
--- /dev/null
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlanner.ui
@@ -0,0 +1,713 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MatGraspPlanner</class>
+ <widget class="QMainWindow" name="MatGraspPlanner">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1242</width>
+ <height>819</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>MatGraspPlanner - based on the Medial Axis Transform (MAT)</string>
+ </property>
+ <widget class="QWidget" name="centralwidget">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="1">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="maximumSize">
+ <size>
+ <width>400</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="title">
+ <string/>
+ </property>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>60</y>
+ <width>381</width>
+ <height>341</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Grasp Planning Options</string>
+ </property>
+ <widget class="QLabel" name="label_11">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>70</y>
+ <width>361</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Minimum sphere radius (relative to biggest sphere)</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_13">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>20</y>
+ <width>331</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Search radius (size of local neighborhoods)</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxSearchRadius">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>40</y>
+ <width>111</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMinimumSphereRadiusRelative">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>90</y>
+ <width>111</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>2</number>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_12">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>120</y>
+ <width>351</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Number of approach directions for local symmetry axis</string>
+ </property>
+ </widget>
+ <widget class="QSpinBox" name="spinBoxNumberOfApproachDirectionsForLocalSymmetryAxis">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>140</y>
+ <width>61</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>4</number>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_14">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>180</y>
+ <width>191</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Fraction of spheres to analyze</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxFractionOfSpheresToAnalyze">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>200</y>
+ <width>111</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>2</number>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_15">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>230</y>
+ <width>351</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Stop after analyzing this number of spheres</string>
+ </property>
+ </widget>
+ <widget class="QSpinBox" name="spinBoxStopAfterAnalyzingThisNumberOfSpheres">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>250</y>
+ <width>111</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="minimum">
+ <number>-1</number>
+ </property>
+ <property name="maximum">
+ <number>10000</number>
+ </property>
+ <property name="value">
+ <number>-1</number>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_16">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>280</y>
+ <width>351</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Maximum grasp diameter of current robot hand</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxGraspDiameterOfCurrentRobotHand">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>300</y>
+ <width>111</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>-1.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.094000000000000</double>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_18">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>300</y>
+ <width>201</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>(-1,0: no limit; 0,094: ARMAR-III)</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_19">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>200</y>
+ <width>191</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>(1,0: analyze all spheres)</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_20">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>250</y>
+ <width>191</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>(-1: analyze all spheres)</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_21">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>90</y>
+ <width>191</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>(0,0: use all sphere radii)</string>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QPushButton" name="pushButtonSave">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>710</y>
+ <width>91</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Save</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonInit">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>660</y>
+ <width>121</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Init MAT Planner</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonRemoveStuff">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>660</y>
+ <width>91</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>RemoveStuff</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonShowGrasp">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>700</y>
+ <width>121</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>TestNextCandidate</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonTestStuff">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>680</y>
+ <width>91</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>TestStuff</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonTestAllCandidates">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>720</y>
+ <width>121</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>TestAllCandidates</string>
+ </property>
+ </widget>
+ <widget class="QGroupBox" name="groupBoxVisualization">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>420</y>
+ <width>381</width>
+ <height>201</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Visualization Options</string>
+ </property>
+ <widget class="QCheckBox" name="checkBoxHand">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>20</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Show Hand</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxObject">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>40</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Object</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxColModel">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>60</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>200</x>
+ <y>20</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>200</x>
+ <y>40</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Show grasps</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxHighlight">
+ <property name="geometry">
+ <rect>
+ <x>200</x>
+ <y>60</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Highlight</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxMedialAxisComplete">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>100</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>MedialAxis complete</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxMedialAxisFiltered">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>120</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>MedialAxis filtered</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxMedialSpheres">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>140</y>
+ <width>181</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>MedialSpheres complete</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxMedialSpheresFiltered">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>160</y>
+ <width>181</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>MedialSpheres filtered</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxLocalNeighborhoods">
+ <property name="geometry">
+ <rect>
+ <x>200</x>
+ <y>100</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>LocalNeighborhoods</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxSearchRadius">
+ <property name="geometry">
+ <rect>
+ <x>200</x>
+ <y>120</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>SearchRadius</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxCandidateGrasps">
+ <property name="geometry">
+ <rect>
+ <x>200</x>
+ <y>140</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>CandidateGrasps</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_17">
+ <property name="geometry">
+ <rect>
+ <x>200</x>
+ <y>170</y>
+ <width>171</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>DrawScale</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDrawScale">
+ <property name="geometry">
+ <rect>
+ <x>270</x>
+ <y>170</y>
+ <width>101</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="maximum">
+ <double>100000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.003000000000000</double>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QLabel" name="labelInfo">
+ <property name="geometry">
+ <rect>
+ <x>240</x>
+ <y>660</y>
+ <width>151</width>
+ <height>71</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="QPushButton" name="pushButtonSelectObject">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>20</y>
+ <width>101</width>
+ <height>28</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Select object</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxRefineMesh">
+ <property name="geometry">
+ <rect>
+ <x>120</x>
+ <y>20</y>
+ <width>191</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Refine mesh with max dist:</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxFractionOfSpheresToAnalyze_2">
+ <property name="geometry">
+ <rect>
+ <x>310</x>
+ <y>20</y>
+ <width>81</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>2</number>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>4.000000000000000</double>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxVerbose">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>630</y>
+ <width>191</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Verbose</string>
+ </property>
+ </widget>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QFrame" name="frameViewer">
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QMenuBar" name="menubar">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1242</width>
+ <height>21</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerApp.cpp b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerApp.cpp
new file mode 100644
index 000000000..0ac245b32
--- /dev/null
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerApp.cpp
@@ -0,0 +1,95 @@
+/**
+* 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;
+}
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
new file mode 100644
index 000000000..8f01a14b8
--- /dev/null
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
@@ -0,0 +1,973 @@
+/**
+* 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 "MatGraspPlannerWindow.h"
+#include "GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h"
+#include "GraspPlanning/ContactConeGenerator.h"
+#include "GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h"
+#include "VirtualRobot/EndEffector/EndEffector.h"
+#include "VirtualRobot/Workspace/Reachability.h"
+#include "VirtualRobot/ManipulationObject.h"
+#include "VirtualRobot/Grasping/Grasp.h"
+#include "VirtualRobot/IK/GenericIKSolver.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
+#include "VirtualRobot/CollisionDetection/CDManager.h"
+#include "VirtualRobot/XML/ObjectIO.h"
+#include "VirtualRobot/XML/RobotIO.h"
+#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
+#include "VirtualRobot/Visualization/TriMeshModel.h"
+#include <QFileDialog>
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <VirtualRobot/XML/FileIO.h>
+
+#include <stdlib.h>
+
+#include <time.h>
+#include <vector>
+#include <iostream>
+#include <cmath>
+
+#include "Inventor/actions/SoLineHighlightRenderAction.h"
+#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoLightModel.h>
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/nodes/SoMatrixTransform.h>
+#include <Inventor/nodes/SoScale.h>
+
+#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/nodes/SoUnits.h>
+#include <Inventor/nodes/SoSphere.h>
+#include <sstream>
+using namespace std;
+using namespace VirtualRobot;
+using namespace GraspStudio;
+
+float TIMER_MS = 30.0f;
+
+MatGraspPlannerWindow::MatGraspPlannerWindow(std::string &robFile, std::string &eefName, std::string &preshape, std::string &objFile, Qt::WFlags flags)
+:QMainWindow(NULL)
+{
+ VR_INFO << " start " << endl;
+
+ // init the random number generator
+ srand(time(NULL));
+
+ this->robotFile = robFile;
+ this->objectFile = objFile;
+ this->eefName = eefName;
+ this->preshape = preshape;
+ //eefVisu = new SoSeparator;
+ //eefVisu->ref();
+ sceneSep = new SoSeparator;
+ sceneSep->ref();
+ eefClonedSep = new SoSeparator;
+ objectSep = new SoSeparator;
+ frictionConeSep = new SoSeparator;
+ graspsSep = new SoSeparator;
+ graspsSep->ref();
+
+ //MP 2013-06-14
+ drawStuffSep = new SoSeparator;
+
+ drawStuffGuiSep = new SoSeparator;
+ drawStuffGuiSep->ref();
+ medialAxisPointCloudSep = new SoSeparator;
+ medialAxisPointCloudSep->ref();
+
+ medialSpheresSep = new SoSeparator;
+ medialSpheresSep->ref();
+
+ medialAxisPointCloudFilteredSep = new SoSeparator;
+ medialAxisPointCloudFilteredSep->ref();
+ medialSpheresFilteredSep = new SoSeparator;
+ medialSpheresFilteredSep->ref();
+
+ neighborhoodSep = new SoSeparator;
+ neighborhoodSep->ref();
+ searchRadiusSep = new SoSeparator;
+ searchRadiusSep->ref();
+ candidateGraspsSep = new SoSeparator;
+ candidateGraspsSep->ref();
+
+
+#if 0
+ SoSeparator *s = CoinVisualizationFactory::CreateCoordSystemVisualization();
+ sceneSep->addChild(s);
+#endif
+ sceneSep->addChild(eefClonedSep);
+ sceneSep->addChild(objectSep);
+ sceneSep->addChild(frictionConeSep);
+ //sceneSep->addChild(eefVisu);
+ //sceneSep->addChild(graspsSep);
+
+ //MP 2013-06-14
+ sceneSep->addChild(drawStuffSep);
+
+ sceneSep->addChild(drawStuffGuiSep);
+ drawStuffGuiSep->addChild(medialAxisPointCloudSep);
+ drawStuffGuiSep->addChild(medialSpheresSep);
+ drawStuffGuiSep->addChild(neighborhoodSep);
+ drawStuffGuiSep->addChild(searchRadiusSep);
+ drawStuffGuiSep->addChild(candidateGraspsSep);
+
+ drawStuffGuiSep->addChild(medialAxisPointCloudFilteredSep);
+ drawStuffGuiSep->addChild(medialSpheresFilteredSep);
+
+ setupUI();
+
+ loadRobot();
+ loadObject();
+
+ buildVisu();
+
+
+
+ viewer->viewAll();
+
+ candidateTestCounter = 0;
+
+ //initializing the configuration
+ gpConfig = GraspPlannerConfigurationPtr(new GraspPlannerConfiguration);
+
+}
+
+
+MatGraspPlannerWindow::~MatGraspPlannerWindow()
+{
+ sceneSep->unref();
+ graspsSep->unref();
+ //eefVisu->unref();
+}
+
+
+
+void MatGraspPlannerWindow::setupUI()
+{
+ UI.setupUi(this);
+ viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
+
+ // setup
+ viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
+ viewer->setAccumulationBuffer(true);
+#ifdef WIN32
+ viewer->setAntialiasing(true, 8);
+#endif
+ viewer->setGLRenderAction(new SoLineHighlightRenderAction);
+ viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
+ viewer->setFeedbackVisibility(true);
+ viewer->setSceneGraph(sceneSep);
+ viewer->viewAll();
+
+// connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll()));
+ connect(UI.pushButtonSelectObject, SIGNAL(clicked()), this, SLOT(selectObject()));
+// connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan()));
+ connect(UI.pushButtonSave, SIGNAL(clicked()), this, SLOT(save()));
+// connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openEEF()));
+// connect(UI.pushButtonClose, SIGNAL(clicked()), this, SLOT(closeEEF()));
+ connect(UI.pushButtonShowGrasp, SIGNAL(clicked()), this, SLOT(showNextGrasp()));
+ connect(UI.pushButtonTestAllCandidates, SIGNAL(clicked()), this, SLOT(testAllCandidates()));
+ connect(UI.pushButtonTestStuff, SIGNAL(clicked()), this, SLOT(testStuff()));
+
+ connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxCones, SIGNAL(clicked()), this, SLOT(frictionConeVisu()));
+ connect(UI.checkBoxGrasps, SIGNAL(clicked()), this, SLOT(showGrasps()));
+ connect(UI.checkBoxHand, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxObject, SIGNAL(clicked()), this, SLOT(colModel()));
+
+ // MP 2013-06-14 first tests
+ connect(UI.pushButtonInit, SIGNAL(clicked()), this, SLOT(initPlanner()));
+ //connect(UI.pushButtonDrawStuff, SIGNAL(clicked()), this, SLOT(drawStuff()));
+ connect(UI.pushButtonRemoveStuff, SIGNAL(clicked()), this, SLOT(removeStuff()));
+
+ //MP 2013-09-25
+ connect(UI.checkBoxMedialAxisComplete, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxMedialSpheres, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxMedialAxisFiltered, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxMedialSpheresFiltered, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxLocalNeighborhoods, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxSearchRadius, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.checkBoxCandidateGrasps, SIGNAL(clicked()), this, SLOT(colModel()));
+}
+
+
+void MatGraspPlannerWindow::resetSceneryAll()
+{
+ grasps->removeAllGrasps();
+ graspsSep->removeAllChildren();
+
+ //if (rns)
+ // rns->setJointValues(startConfig);
+}
+
+
+void MatGraspPlannerWindow::closeEvent(QCloseEvent *event)
+{
+ quit();
+ QMainWindow::closeEvent(event);
+}
+
+
+void MatGraspPlannerWindow::buildVisu()
+{
+
+ eefClonedSep->removeAllChildren();
+ SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full;
+
+ if (eefCloned && UI.checkBoxHand->isChecked())
+ {
+ visualizationEEFCloned = eefCloned->getVisualization<CoinVisualization>(colModel);
+ SoNode* visualisationNode = visualizationEEFCloned->getCoinVisualization();
+ if (visualisationNode)
+ {
+ eefClonedSep->addChild(visualisationNode);
+ visualizationEEFCloned->highlight(UI.checkBoxHighlight->isChecked());
+ }
+ }
+ /*
+ if (robot)
+ {
+ visualizationRobot = robot->getVisualization<CoinVisualization>(colModel);
+ SoNode* visualisationNode = visualizationRobot->getCoinVisualization();
+ if (visualisationNode)
+ {
+ robotSep->addChild(visualisationNode);
+ //visualizationRobot->highlight(true);
+ }
+ }
+ */
+ objectSep->removeAllChildren();
+ if (object && UI.checkBoxObject->isChecked())
+ {
+ SceneObject::VisualizationType colModel2 = (UI.checkBoxColModel->isChecked())?SceneObject::CollisionData:SceneObject::Full;
+ SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(object,colModel2);
+ if (visualisationNode)
+ objectSep->addChild(visualisationNode);
+
+ /*SoNode *s = CoinVisualizationFactory::getCoinVisualization(object->getCollisionModel()->getTriMeshModel(),true);
+ if (s)
+ objectSep->addChild(s); */
+ }
+
+ frictionConeSep->removeAllChildren();
+ bool fc = (UI.checkBoxCones->isChecked());
+ if (fc && contacts.size()>0 && qualityMeasure)
+ {
+ ContactConeGeneratorPtr cg = qualityMeasure->getConeGenerator();
+ float radius = cg->getConeRadius();
+ float height = cg->getConeHeight();
+ float scaling = 30.0f;
+ SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(contacts,height*scaling,radius*scaling,true);
+ if (visualisationNode)
+ frictionConeSep->addChild(visualisationNode);
+
+ // add approach dir visu
+ for (size_t i=0;i<contacts.size();i++)
+ {
+ SoSeparator *s = new SoSeparator;
+ Eigen::Matrix4f ma;
+ ma.setIdentity();
+ ma.block(0,3,3,1) = contacts[i].contactPointFingerGlobal;
+ SoMatrixTransform *m = CoinVisualizationFactory::getMatrixTransform(ma);
+ s->addChild(m);
+ s->addChild(CoinVisualizationFactory::CreateArrow(contacts[i].approachDirectionGlobal,10.0f,1.0f));
+ frictionConeSep->addChild(s);
+ }
+ }
+
+ if (UI.checkBoxGrasps->isChecked() && sceneSep->findChild(graspsSep)<0)
+ sceneSep->addChild(graspsSep);
+ if (!UI.checkBoxGrasps->isChecked() && sceneSep->findChild(graspsSep)>=0)
+ sceneSep->removeChild(graspsSep);
+
+ //MP 2013-09-25 draw more stuff using checkboxes
+ //drawStuffGuiSep->removeAllChildren();
+
+ // ---- medial axis
+ if (UI.checkBoxMedialAxisComplete->isChecked()
+ && drawStuffGuiSep->findChild(medialAxisPointCloudSep)<0)
+ {
+ drawStuffGuiSep->addChild(medialAxisPointCloudSep);
+ }
+ if (!UI.checkBoxMedialAxisComplete->isChecked()
+ && drawStuffGuiSep->findChild(medialAxisPointCloudSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(medialAxisPointCloudSep);
+ }
+
+ if (UI.checkBoxMedialAxisFiltered->isChecked()
+ && drawStuffGuiSep->findChild(medialAxisPointCloudFilteredSep)<0)
+ {
+ drawStuffGuiSep->addChild(medialAxisPointCloudFilteredSep);
+ }
+ if (!UI.checkBoxMedialAxisFiltered->isChecked()
+ && drawStuffGuiSep->findChild(medialAxisPointCloudFilteredSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(medialAxisPointCloudFilteredSep);
+ }
+
+ // ---- medial spheres
+ if (UI.checkBoxMedialSpheres->isChecked()
+ && drawStuffGuiSep->findChild(medialSpheresSep)<0)
+ {
+ drawStuffGuiSep->addChild(medialSpheresSep);
+ }
+ if (!UI.checkBoxMedialSpheres->isChecked()
+ && drawStuffGuiSep->findChild(medialSpheresSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(medialSpheresSep);
+ }
+
+ if (UI.checkBoxMedialSpheresFiltered->isChecked()
+ && drawStuffGuiSep->findChild(medialSpheresFilteredSep)<0)
+ {
+ drawStuffGuiSep->addChild(medialSpheresFilteredSep);
+ }
+ if (!UI.checkBoxMedialSpheresFiltered->isChecked()
+ && drawStuffGuiSep->findChild(medialSpheresFilteredSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(medialSpheresFilteredSep);
+ }
+
+ // ---- local neighborhoods
+ if (UI.checkBoxLocalNeighborhoods->isChecked()
+ && drawStuffGuiSep->findChild(neighborhoodSep)<0)
+ {
+ drawStuffGuiSep->addChild(neighborhoodSep);
+ }
+ if (!UI.checkBoxLocalNeighborhoods->isChecked()
+ && drawStuffGuiSep->findChild(neighborhoodSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(neighborhoodSep);
+ }
+
+ // ---- search radius
+ if (UI.checkBoxSearchRadius->isChecked()
+ && drawStuffGuiSep->findChild(searchRadiusSep)<0)
+ {
+ drawStuffGuiSep->addChild(searchRadiusSep);
+ }
+ if (!UI.checkBoxSearchRadius->isChecked()
+ && drawStuffGuiSep->findChild(searchRadiusSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(searchRadiusSep);
+ }
+
+ // ---- candidate grasps
+ if (UI.checkBoxCandidateGrasps->isChecked()
+ && drawStuffGuiSep->findChild(candidateGraspsSep)<0)
+ {
+ drawStuffGuiSep->addChild(candidateGraspsSep);
+ }
+ if (!UI.checkBoxCandidateGrasps->isChecked()
+ && drawStuffGuiSep->findChild(candidateGraspsSep)>=0)
+ {
+ drawStuffGuiSep->removeChild(candidateGraspsSep);
+ }
+
+
+
+
+ viewer->scheduleRedraw();
+}
+
+int MatGraspPlannerWindow::main()
+{
+ SoQt::show(this);
+ SoQt::mainLoop();
+ return 0;
+}
+
+
+void MatGraspPlannerWindow::quit()
+{
+ std::cout << "MatGraspPlannerWindow: Closing" << std::endl;
+ this->close();
+ SoQt::exitMainLoop();
+}
+
+void MatGraspPlannerWindow::loadObject()
+{
+ if (!eef)
+ {
+ VR_ERROR << "No end effector loaded..." << endl;
+ return;
+ }
+
+ if (!object)
+ {
+ object = Obstacle::createBox(100.0f,100.0f,40.0f);
+ }
+ object = MeshConverter::refineObjectSurface(object,4.0f);
+
+
+ qualityMeasure.reset(new GraspStudio::GraspQualityMeasureWrenchSpace(object));
+ //qualityMeasure->setVerbose(true);
+ qualityMeasure->calculateObjectProperties();
+}
+
+void MatGraspPlannerWindow::loadObjectFromFile(string objectFilename)
+{
+
+ object = ObjectIO::loadManipulationObject(objectFilename);
+
+ if (UI.checkBoxRefineMesh->isChecked())
+ {
+ cout << "Refining surface mesh..." << endl;
+ object = MeshConverter::refineObjectSurface(object,4.0f);
+ cout << "Refining surface mesh: Done." << endl;
+ }
+
+}
+
+void MatGraspPlannerWindow::loadRobot()
+{
+ robot.reset();
+ robot = RobotIO::loadRobot(robotFile);
+ if (!robot)
+ {
+ VR_ERROR << " no robot at " << robotFile << endl;
+ return;
+ }
+ VR_INFO << "Searching EEF with name " << eefName << endl;
+ eef = robot->getEndEffector(eefName);
+ if (eef)
+ cout << "...OK" << endl;
+ else
+ {
+ cout << "...FAILED" << endl;
+ return;
+ }
+
+ eefCloned = eef->createEefRobot(eef->getName(), eef->getName());
+
+ eef = eefCloned->getEndEffector(eef->getName());
+ eef->getGCP()->showCoordinateSystem(true);
+ if (!preshape.empty())
+ {
+ eef->setPreshape(preshape);
+ }
+ /*eefVisu->removeAllChildren();
+ eefVisu->addChild(CoinVisualizationFactory::CreateEndEffectorVisualization(eef));*/
+}
+
+void MatGraspPlannerWindow::plan()
+{
+ //(MP 2013-09-30) Auskommentierung weg... (temp)
+
+
+// float timeout = UI.spinBoxTimeOut->value() * 1000.0f;
+// bool forceClosure = UI.checkBoxFoceClosure->isChecked();
+// float quality = (float)UI.doubleSpinBoxQuality->value();
+// int nrGrasps = UI.spinBoxGraspNumber->value();
+
+// //MP create grasp planner
+// //planner.reset(new GraspStudio::GenericGraspPlanner(grasps,qualityMeasure,approach,quality,forceClosure));
+// planner.reset(new MatGraspPlanner(grasps,qualityMeasure,approach,quality,forceClosure));
+
+// //MP plan grasps
+// int nr = planner->plan(nrGrasps,timeout);
+// VR_INFO << " Grasp planned:" << nr << endl;
+// int start = (int)grasps->getSize()-nrGrasps-1;
+// if (start<0)
+// start = 0;
+// grasps->setPreshape(preshape);
+
+// //MP draw resulting grasps
+// for (int i=start; i<(int)grasps->getSize()-1; i++)
+// {
+// //MP Tcp pose in world frame
+// Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
+
+// SoSeparator *sep1 = new SoSeparator();
+// SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransform(m);
+// sep1->addChild(mt);
+// sep1->addChild(eefVisu);
+// graspsSep->addChild(sep1);
+// }
+// // set to last valid grasp
+// if (grasps->getSize()>0 && eefCloned && eefCloned->getEndEffector(eefName))
+// {
+// Eigen::Matrix4f mGrasp = grasps->getGrasp(grasps->getSize()-1)->getTcpPoseGlobal(object->getGlobalPose());
+// eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getTcp(),mGrasp);
+// }
+// if (nrGrasps>0)
+// {
+// openEEF();
+// closeEEF();
+// }
+
+}
+
+/*
+void MatGraspPlannerWindow::closeEEF(CandidateGraspPtr candidate, float positionScaleFactor)
+{
+#if 1
+ static int pp=0;
+ static Eigen::Vector3f approachDir;
+
+// object->getCollisionModel()->getTriMeshModel()->print();
+
+// if (pp==0)
+// {
+// Eigen::Vector3f position;
+// approach->getPositionOnObject(position,approachDir);
+
+// // set new pose
+// approach->setEEFToApproachPose(position,approachDir);
+// //eefCloned->getEndEffector(eefName)->getGCP()->showCoordinateSystem(true);
+
+
+// } else
+// {
+// //approach->moveEEFAway(approachDir,3.0f,5);
+// approach->moveEEFAway(candidate->handApproachDirection,3.0f,5); //MP 2013-10-01
+// }
+// pp = (pp+1) % 5;
+
+
+
+
+ //=== MP 2013-10-01
+ Eigen::Matrix4f poseGCP = candidate->toMatrix4f(positionScaleFactor);
+ setEEF(poseGCP);
+
+ cout << "todo: moveaway..." << endl;
+ //approach->moveEEFAway(candidate->handApproachDirection,3.0f,5); //MP 2013-10-01
+ //=== MP 2013-10-01
+
+
+// return;
+#endif
+ contacts.clear();
+
+ float qual;
+ if (eefCloned && eefCloned->getEndEffector(eefName))
+ {
+ contacts = eefCloned->getEndEffector(eefName)->closeActors(object);
+ qualityMeasure->setContactPoints(contacts);
+
+ qual = qualityMeasure->getGraspQuality();
+ bool isFC = qualityMeasure->isGraspForceClosure();
+ std::stringstream ss;
+ ss << std::setprecision(3);
+ ss << "Grasp Nr " << grasps->getSize() << "\nQuality (wrench space): "
+ << qual << "\nForce closure: ";
+ if (isFC)
+ ss << "yes";
+ else
+ ss << "no";
+ UI.labelInfo->setText(QString (ss.str().c_str()));
+ }
+ buildVisu();
+
+ candidate->tested = true;
+ candidate->qualityMindist = qual;
+ //candidate->finalHandPose = eef->getGCP()->getGlobalPose(); //is that the correct pose?
+}
+*/
+
+/*
+void MatGraspPlannerWindow::openEEF()
+{
+ contacts.clear();
+ if (eefCloned && eefCloned->getEndEffector(eefName))
+ {
+ eefCloned->getEndEffector(eefName)->openActors();
+ }
+ buildVisu();
+}
+*/
+
+void MatGraspPlannerWindow::frictionConeVisu()
+{
+ buildVisu();
+}
+
+void MatGraspPlannerWindow::colModel()
+{
+ buildVisu();
+}
+
+void MatGraspPlannerWindow::showGrasps()
+{
+ buildVisu();
+}
+
+
+void MatGraspPlannerWindow::save()
+{
+ if (!object)
+ return;
+
+ 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.toAscii());
+ bool ok = false;
+ try
+ {
+ ok = ObjectIO::saveManipulationObject(objectM, objectFile);
+ }
+ catch (VirtualRobotException &e)
+ {
+ cout << " ERROR while saving object" << endl;
+ cout << e.what();
+ return;
+ }
+
+ if (!ok)
+ {
+ cout << " ERROR while saving object" << endl;
+ return;
+ }
+}
+
+void MatGraspPlannerWindow::initPlanner()
+{
+ VR_INFO << "-------- Init MAT planner." << endl;
+
+ if (!robot || !eef || !object || !qualityMeasure)
+ {
+ VR_ERROR << "no robot " << endl;
+ return;
+ }
+ candidateTestCounter = 0;
+ updateGraspPlannerConfigurationFromGui();
+
+ std::string name = "MAT Grasp Planner - ";
+ name += eef->getName();
+ grasps.reset(new GraspSet(name,robot->getType(),eefName));
+
+ planner.reset(new MatGraspPlanner(grasps,robot,eef,qualityMeasure,
+ gpConfig,0.0f,true,
+ UI.checkBoxVerbose->isChecked()));
+
+ planner->init();
+
+ std::vector<MedialSpherePtr> medialSpheres = planner->getMedialSpheres();
+ float maxSphereRadiusForColorComputation = SphereHelpers::findMaxSphereRadius(medialSpheres);
+
+ drawMedialAxisPointCloud(medialSpheres, 0.5*gpConfig->drawPointSize);
+ drawMedialSpheres(medialSpheres, maxSphereRadiusForColorComputation);
+
+ std::vector<MedialSpherePtr> medialSpheresFiltered = planner->getMedialSpheresFiltered();
+ drawMedialAxisPointCloudFiltered(medialSpheresFiltered, 0.5*gpConfig->drawPointSize);
+ drawMedialSpheresFiltered(medialSpheresFiltered, maxSphereRadiusForColorComputation);
+
+ std::vector<LocalNeighborhoodPtr> localNeighborhoods = planner->getLocalNeighborhoods();
+ drawLocalNeighborhoods(localNeighborhoods, gpConfig, gpConfig->drawScale);
+ drawSearchRadii(localNeighborhoods);
+ std::vector<CandidateGraspPtr> candidateGraspsTemp = planner->getCandidateGrasps();
+ drawCandidateGrasps(candidateGraspsTemp, 20*gpConfig->drawScale);
+
+ cout << "Generated " << planner->getCandidateGrasps().size() << " candidate grasps." << endl;
+ candidateGrasps = planner->getCandidateGrasps();
+ cout << "Everything done." << std::endl;
+}
+
+void MatGraspPlannerWindow::showNextGrasp()
+{
+ cout << "\nButton ShowGrasp pushed. Candidate number: "
+ << candidateTestCounter << endl;
+
+ if (candidateTestCounter>=(int)candidateGrasps.size())
+ {
+ cout << "No grasps available..." << endl;
+ return;
+ }
+
+ CandidateGraspPtr currentCandidate = candidateGrasps.at(candidateTestCounter);
+
+ simulateOneGrasp(currentCandidate);
+ eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getGCP(), currentCandidate->finalHandPose);
+ eefCloned->setConfig(currentCandidate->finalJointAngles);
+
+ candidateTestCounter++;
+ if (candidateTestCounter >= (int)candidateGrasps.size())
+ candidateTestCounter = 0;
+}
+
+void MatGraspPlannerWindow::testAllCandidates()
+{
+ //planner->cgTester->simulateAllGrasps(candidateGrasps);
+}
+
+/*
+void MatGraspPlannerWindow::simulateAllGrasps(vector<CandidateGraspPtr> candidates)
+{
+ for (size_t i=0; i<candidates.size(); i++)
+ {
+ if (i % 10 == 0)
+ cout << "Testing candidate " << i << " of " << candidates.size() << endl;
+ simulateOneGrasp(candidates.at(i));
+ }
+
+ cout << "simulateAllGrasps: Done." << endl;
+}
+*/
+
+
+void MatGraspPlannerWindow::simulateOneGrasp(CandidateGraspPtr candidate)
+{
+ cout << "candidate grasp BEFORE testing: " << endl;
+ candidate->printDebug();
+
+ // --- for visualization
+ //int drawScaleCandidate = 40*gpConfig->drawScale;
+ float drawScaleCandidate = 0.4f; //ACHTUNG: als Argument...!
+ drawStuffSep->removeAllChildren();
+ SoSeparator* sepCandidate = DrawHelpers::DrawCandidateGrasp(candidate, drawScaleCandidate);
+ drawStuffSep->addChild(sepCandidate);
+
+ planner->testCandidate(candidate);
+
+ cout << "candidate grasp AFTER testing: " << endl;
+ candidate->printDebug();
+
+}
+
+
+
+void MatGraspPlannerWindow::removeStuff()
+{
+ printf("Button RemoveStuff pressed.\n");
+ //drawStuffSep->removeChild(sep1);
+ drawStuffSep->removeAllChildren();
+}
+
+void MatGraspPlannerWindow::testStuff()
+{
+ //temporary 2013-09-29 other stuff
+ //TestCases::testSingleLinkedList();
+ //TestCases::testTree();
+ //TestCases::testFunStuff();
+}
+
+
+
+
+void MatGraspPlannerWindow::drawCandidateGrasp(CandidateGraspPtr cg, float drawScaleCandidate)
+{
+ SoSeparator* sepCandidate = DrawHelpers::DrawCandidateGrasp(cg, drawScaleCandidate);
+ drawStuffSep->addChild(sepCandidate);
+}
+
+void MatGraspPlannerWindow::drawCandidateGrasps(std::vector<CandidateGraspPtr>& cg, float drawScaleCandidates)
+{
+ // cout << "candidate no. " << j << endl;
+ // candidates.at(j).printDebug();
+ // cout << "Now draw it..." << endl;
+ SoSeparator* sepCandidates = DrawHelpers::DrawCandidateGrasps(cg,
+ drawScaleCandidates);
+ //drawStuffSep->addChild(sepCandidates);
+
+ candidateGraspsSep->removeAllChildren();
+ candidateGraspsSep->addChild(sepCandidates);
+}
+
+void MatGraspPlannerWindow::drawSearchRadius(LocalNeighborhoodPtr neighborhood)
+{
+ SoSeparator* sepSearchRadius = DrawHelpers::DrawSearchRadius(neighborhood);
+
+ searchRadiusSep->addChild(sepSearchRadius);
+}
+
+void MatGraspPlannerWindow::drawSearchRadii(std::vector<LocalNeighborhoodPtr>& neighborhoods)
+{
+ SoSeparator* sepSearchRadii = DrawHelpers::DrawSearchRadii(neighborhoods);
+
+ searchRadiusSep->removeAllChildren();
+ searchRadiusSep->addChild(sepSearchRadii);
+}
+
+void MatGraspPlannerWindow::drawLocalNeighborhood(LocalNeighborhoodPtr neighborhood, GraspPlannerConfigurationPtr gpConfig, float drawScaleNeighborhood)
+{
+ //cout << "MatGraspPlannerWindow::drawLocalNeighborhood() called. "<< endl;
+ //gpConfig->printDebug();
+
+ SoSeparator* sepNeighborhood = DrawHelpers::DrawLocalNeighborhood(
+ neighborhood,
+ gpConfig->drawNeighborhoodEigenvectors,
+ gpConfig->drawNeighborhoodCenterOfGravity,
+ gpConfig->drawNeighborhoodSearchRadius,
+ gpConfig->drawPointSize,
+ drawScaleNeighborhood);
+ //drawStuffSep->addChild(sepNeighborhood);
+
+ //do not call removeChildren before, because we draw the neighborhoods incrementally,
+ //only one at a time!
+ neighborhoodSep->addChild(sepNeighborhood);
+}
+
+
+void MatGraspPlannerWindow::drawLocalNeighborhoods(std::vector<LocalNeighborhoodPtr>& neighborhoods, GraspPlannerConfigurationPtr gpConfig, float drawScaleNeighborhoods)
+{
+ SoSeparator* sepNeighborhoods = DrawHelpers::DrawLocalNeighborhoods(
+ neighborhoods,
+ gpConfig->drawNeighborhoodEigenvectors,
+ gpConfig->drawNeighborhoodCenterOfGravity,
+ gpConfig->drawNeighborhoodSearchRadius,
+ gpConfig->drawPointSize,
+ drawScaleNeighborhoods);
+ //drawStuffSep->addChild(sepNeighborhoods);
+ neighborhoodSep->removeAllChildren();
+ neighborhoodSep->addChild(sepNeighborhoods);
+
+}
+
+void MatGraspPlannerWindow::drawSurfacePointCloud(float pointSize)
+{
+ float colorR = 0.0;
+ float colorG = 0.0;
+ float colorB = 0.0;
+ float transparency = 0.0;
+ std::vector<Eigen::Vector3f> surfacePoints = planner->getSurfacePoints();
+ SoSeparator *sepSurfacePoints = DrawHelpers::DrawPointCloud(surfacePoints,
+ colorR, colorG, colorB, transparency, pointSize);
+ drawStuffSep->addChild(sepSurfacePoints);
+}
+
+void MatGraspPlannerWindow::drawMedialAxisPointCloud(
+ std::vector<MedialSpherePtr>& spheres, float pointSize)
+{
+ //cout << "MatGraspPlannerWindow::drawMedialAxisPointCloud() called()." << endl;
+
+ float colorR = 1.0;
+ float colorG = 0.0;
+ float colorB = 1.0;
+ float transparency = 0.0;
+ SoSeparator *sepMAPoints = DrawHelpers::DrawPointCloud(spheres,
+ colorR, colorG, colorB, transparency, pointSize);
+ //drawStuffSep->addChild(sepMAPoints);
+
+ medialAxisPointCloudSep->removeAllChildren(); //segfault -> warum?
+ medialAxisPointCloudSep->addChild(sepMAPoints);
+}
+
+void MatGraspPlannerWindow::drawMedialAxisPointCloudFiltered(
+ std::vector<MedialSpherePtr>& spheres, float pointSize)
+{
+ //cout << "MatGraspPlannerWindow::drawMedialAxisPointCloudFiltered() called()." << endl;
+
+ float colorR = 1.0;
+ float colorG = 0.0;
+ float colorB = 1.0;
+ float transparency = 0.0;
+ SoSeparator *sepMAPoints = DrawHelpers::DrawPointCloud(spheres,
+ colorR, colorG, colorB, transparency, pointSize);
+ //drawStuffSep->addChild(sepMAPoints);
+
+ medialAxisPointCloudFilteredSep->removeAllChildren(); //segfault -> warum?
+ medialAxisPointCloudFilteredSep->addChild(sepMAPoints);
+}
+
+void MatGraspPlannerWindow::drawMedialSpheres(std::vector<MedialSpherePtr>& spheres,
+ float maxRadius)
+{
+ float transparency = 0.0;
+ SoSeparator *sepMedialSpheres = DrawHelpers::DrawVectorOfSpheres(spheres,
+ maxRadius, transparency);
+ //drawStuffSep->addChild(sepMedialSpheres);
+ medialSpheresSep->removeAllChildren();
+ medialSpheresSep->addChild(sepMedialSpheres);
+}
+
+void MatGraspPlannerWindow::drawMedialSpheresFiltered(std::vector<MedialSpherePtr>& spheres,
+ float maxRadius)
+{
+ float transparency = 0.0;
+ SoSeparator *sepMedialSpheres = DrawHelpers::DrawVectorOfSpheres(spheres,
+ maxRadius, transparency);
+ //drawStuffSep->addChild(sepMedialSpheres);
+ medialSpheresFilteredSep->removeAllChildren();
+ medialSpheresFilteredSep->addChild(sepMedialSpheres);
+}
+
+/*
+void MatGraspPlannerWindow::setEEF(Eigen::Matrix4f &poseGCP)
+{
+ if (!eefCloned)
+ return;
+ eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getGCP(),poseGCP);
+}
+*/
+
+void MatGraspPlannerWindow::updateGraspPlannerConfigurationFromGui()
+{
+ gpConfig->drawScale = UI.doubleSpinBoxDrawScale->value();
+ gpConfig->drawPointSize = 0.3*gpConfig->drawScale;
+
+ gpConfig->neighborhoodSearchRadius = UI.doubleSpinBoxSearchRadius->value();
+
+ gpConfig->minimumSphereRadiusRelative =
+ UI.doubleSpinBoxMinimumSphereRadiusRelative->value();
+
+ gpConfig->numberOfApproachDirectionsForLocalSymmetryAxis =
+ UI.spinBoxNumberOfApproachDirectionsForLocalSymmetryAxis->value();
+ gpConfig->maxGraspDiameterOfCurrentRobotHand =
+ UI.doubleSpinBoxMaxGraspDiameterOfCurrentRobotHand->value();
+
+ gpConfig->fractionOfSpheresToAnalyze =
+ UI.doubleSpinBoxFractionOfSpheresToAnalyze->value();
+ gpConfig->stopAfterAnalyzingThisNumberOfSpheres =
+ UI.spinBoxStopAfterAnalyzingThisNumberOfSpheres->value();
+
+ gpConfig->printDebug();
+}
+/*
+void MatGraspPlannerWindow::resetDataStructures()
+{
+ medialSpheres.clear();
+ medialSpheresFiltered.clear();
+ localNeighborhoods.clear();
+ candidateGrasps.clear();
+ candidateTestCounter = 0;
+}*/
+
+void MatGraspPlannerWindow::selectObject()
+{
+ //cout << "selectObject() called. " << endl;
+
+ QString fi = QFileDialog::getOpenFileName(this, tr("Open Object File"), QString(), tr("XML Files (*.xml)"));
+ //std::string s = m_sRobotFilename = std::string(fi.toAscii());
+ std::string s = std::string(fi.toAscii());
+ if (!s.empty())
+ {
+ if (UI.checkBoxVerbose->isChecked())
+ cout << "selectObject() Filename: " << s << endl;
+ loadObjectFromFile(s);
+
+ }
+ else
+ if (UI.checkBoxVerbose->isChecked())
+ cout << "selectObject(): empty filenmame!" << endl;
+}
+
+
+
+
+
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
new file mode 100644
index 000000000..8456a7167
--- /dev/null
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
@@ -0,0 +1,186 @@
+/**
+* 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_
--
GitLab