grasps.h5 file generator

src/Core/h5.cpp

@@ -12,6 +12,8 @@
 
 #include <H5Cpp.h>
 
+namespace rai {
+
 //===========================================================================
 
 template<class T> H5::DataType get_h5type();
@@ -45,6 +47,7 @@ template<class T> void H5_Writer::add(const char* name, const rai::Array<T>& x)
 //===========================================================================
 
 H5_Reader::H5_Reader(const char* filename) {
+  CHECK(FileToken(filename).exists(), "file '" <<filename <<"' does not exist");
   file = make_shared<H5::H5File>(filename, H5F_ACC_RDONLY);
 }
 
@@ -142,3 +145,5 @@ template rai::Array<int16_t> H5_Reader::read<int16_t>(const char* name, bool ifE
 template rai::Array<uint16_t> H5_Reader::read<uint16_t>(const char* name, bool ifExists);
 template rai::Array<char> H5_Reader::read<char>(const char* name, bool ifExists);
 template rai::Array<byte> H5_Reader::read<byte>(const char* name, bool ifExists);
+
+}//namespace

src/Core/h5.h

@@ -13,6 +13,8 @@
 
 namespace H5 { class H5File; }
 
+namespace rai {
+
 //===========================================================================
 
 struct H5_Writer {
@@ -37,3 +39,4 @@ struct H5_Reader {
   bool exists(const char* name);
 };
 
+} //namespace

src/Core/util.cpp

@@ -775,6 +775,12 @@ void rai::String::replace(uint i, uint n, const char* xp, uint xN) {
   }
 }
 
+void rai::String::removePostfix(const char* postfix){
+  uint n = strlen(postfix);
+  CHECK(!strcmp(p+N-n, postfix), "no match between postfix '" <<postfix <<"' and end '" <<p+N-n <<"'");
+  resize(N-n, true);
+}
+
 rai::String& rai::String::setRandom() {
   resize(rnd(2, 6), false);
   for(uint i=0; i<N; i++) operator()(i)=rnd('a', 'z');

src/Core/util.h

@@ -187,6 +187,7 @@ struct String : public std::iostream {
   void prepend(const String& s);
   void replace(uint i, uint n, const char* xp, uint xN);
   void removePrefix(const char* prefix);
+  void removePostfix(const char* postfix);
 
   String& setRandom();
 

src/DataGen/shapenetGrasps.cpp

@@ -23,30 +23,33 @@ void ShapenetGrasps::clearScene(){
   ref->setColor({1.,1.,0.});
 }
 
-void ShapenetGrasps::addSceneGripper(){
-  C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaFloatingGripper.g"));
+void ShapenetGrasps::resetObjectPose(int idx, bool rndOrientation){
+  rai::Frame *obj = C.getFrame(STRING("obj"<<idx));
+  if(rndOrientation) obj->set_X()->rot.setRandom();
+  obj->setPosition({double(idx),0.,1.});
 }
 
-bool ShapenetGrasps::addSceneObject(const char* file, int idx, bool rndPose, bool visual){
+bool ShapenetGrasps::addSceneObject(const char* file, int idx, bool rndOri, bool visual){
   LOG(0) <<"loading shapenet object " <<file;
-  H5_Reader H(file);
+#if 1
+  rai::Frame *obj = C.addH5Object(STRING("obj"<<idx), file, 2);
+  if(!obj) return false;
+  obj->inertia->scaleTo(.1);
+#else
+  rai::H5_Reader H(file);
 
-  rai::Frame *ref = C.getFrame("ref");
   rai::Frame *obj = C.addFrame(STRING("obj"<<idx));
 
   {
     arr pts = H.read<double>("points/vertices");
     arr normals = H.read<double>("points/normals");
 
-    rai::Frame *objPts = C.addFrame(STRING("objPts"<<idx));
+    rai::Frame *objPts = C.addFrame(obj->name+"_pts");
     objPts->setParent(obj);
     objPts->setPointCloud(pts, {}, normals);
 //    objPts->setMesh(pts);
     objPts->setContact(0);
     objPts->setColor({1., 0., 0., .9});
-
-    if(ref->parent) ref->unLink();
-    ref->setParent(objPts, false);
   }
 
   if(!visual){
@@ -55,25 +58,25 @@ bool ShapenetGrasps::addSceneObject(const char* file, int idx, bool rndPose, boo
     byteA colors = H.read<byte>("decomp/colors");
     uintA parts = H.read<uint>("decomp/parts");
 
-    rai::Frame *objMeshes = C.addFrame(STRING("objMeshes"<<idx));
-    objMeshes->setParent(obj);
-    objMeshes->setMesh(pts, faces, colors, parts);
-    objMeshes->setContact(1);
-    objMeshes->setMass(.1);
+    rai::Frame *objDecomp = C.addFrame(obj->name+"_decomp");
+    objDecomp ->setParent(obj);
+    objDecomp ->setMesh(pts, faces, colors, parts);
+    objDecomp ->setContact(1);
+    objDecomp ->setMass(.1);
 
     obj->computeCompoundInertia();
     obj->transformToDiagInertia();
 
-    objMeshes->convertDecomposedShapeToChildFrames();
-    if(!objMeshes->children.N){
+    objDecomp ->convertDecomposedShapeToChildFrames();
+    if(!objDecomp ->children.N){
       //there are no collision shapes
       return false;
     }
   }else{
     arr pts = H.read<double>("mesh/vertices");
     uintA faces = H.read<uint>("mesh/faces");
 
-    rai::Frame *objMesh = C.addFrame(STRING("objMesh"<<idx));
+    rai::Frame *objMesh = C.addFrame(obj->name+"_mesh");
     objMesh->setParent(obj);
     objMesh->setMesh(pts, faces);
   }
@@ -82,15 +85,17 @@ bool ShapenetGrasps::addSceneObject(const char* file, int idx, bool rndPose, boo
     //obj transform is buggy (typically singular inertia)
     return false;
   }
+#endif
 
-  if(rndPose) obj->set_X()->setRandom();
-  obj->setPosition({double(idx),0.,1.});
+  // obj->setShape(rai::ST_marker, {.5});
+  resetObjectPose(idx, rndOri);
+  cout <<"loaded object inertia: " <<*obj->inertia <<endl;
 
   return true;
 }
 
 arr ShapenetGrasps::sampleGraspPose(){
-    ::sampleGraspCandidate(C, "objPts0", "ref", opt.pregraspNormalSdv, opt.verbose);
+    return ::sampleGraspCandidate(C, "obj0_pts", "ref", opt.pregraspNormalSdv, opt.verbose);
 }
 
 arr sampleGraspCandidate(rai::Configuration& C, const char *ptsFrame, const char* refFrame, double pregraspNormalSdv, int verbose){
@@ -178,7 +183,9 @@ arr sampleGraspCandidate(rai::Configuration& C, const char *ptsFrame, const char
         cout <<komo.pathConfig.reportForces() <<endl;
         //    komo.view(true);
       }
+      //C.get_viewer()->nonThreaded=true;
       C.view(verbose>2, "fine tuned pregrasp");
+      //C.get_viewer()->savePng();
       if(verbose>1) rai::wait(.1);
     }
 
@@ -281,7 +288,11 @@ arr ShapenetGrasps::evaluateGrasp(){
       //display
       if(opt.verbose>0){
         if(opt.verbose>1) rai::wait(opt.simTau);
-        if(!(t%10)) C.view(false, STRING("phase: " <<phase <<" t: " <<opt.simTau*t));
+        if(!(t%1)){
+          //C.get_viewer()->nonThreaded=true;
+          C.view(false, STRING("phase: " <<phase <<" t: " <<opt.simTau*t));
+          //C.get_viewer()->savePng();
+        }
       }
     }
 
@@ -299,28 +310,38 @@ arr ShapenetGrasps::evaluateGrasp(){
     C.view(opt.verbose>1, STRING("evaluation: " <<(succ?"success":"failure") <<" scores:\n" <<scores));
   }
 
+  C["floatBall"]->setJoint(rai::JT_quatBall);
+
   return scores;
 }
 
-bool ShapenetGrasps::loadObject(uint shape, bool rndPose){
+bool ShapenetGrasps::loadObject(uint shape, bool rndOrientation){
   clearScene();
   str file = opt.filesPrefix + files(shape);
-  bool succ = addSceneObject(file, 0, rndPose);
-  if(!succ) LOG(0) <<"loading object " <<shape <<" '" <<file <<"' failed";
-  addSceneGripper();
-  return succ;
+  bool succ = addSceneObject(file, 0, rndOrientation);
+  if(!succ){
+    LOG(0) <<"loading object " <<shape <<" '" <<file <<"' failed";
+    return false;
+  }
+  C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaFloatingGripper.g"));
+
+  rai::Frame *ref = C.getFrame("ref");
+  if(ref->parent) ref->unLink();
+  ref->setParent(C.getFrame("obj0_pts"), false);
+
+  return true;
 }
 
 arr ShapenetGrasps::getPointCloud(){
-  rai::Frame * objPts = C["objPts0"];
+  rai::Frame * objPts = C["obj0_pts"];
   return objPts->getMeshPoints();
 }
 
 void ShapenetGrasps::getSamples(arr& X, uintA& shapes, arr& Scores, uint N){
-  if(opt.numShapes<0) opt.numShapes = files.N - opt.startShape;
+  if(opt.endShape<0) opt.endShape = files.N;
 
   for(uint n=0;n<N;){
-    uint shape = opt.startShape + rnd(opt.numShapes);
+    uint shape = opt.startShape + rnd(opt.endShape-opt.startShape);
     if(opt.verbose>0){
       cout <<"sample " <<n <<", shape " <<shape <<" (" <<files(shape) <<")" <<endl;
     }
@@ -329,6 +350,7 @@ void ShapenetGrasps::getSamples(arr& X, uintA& shapes, arr& Scores, uint N){
     bool succ = loadObject(shape, true);
     if(!succ) continue;
     if(opt.verbose>0) C.view(opt.verbose>2, STRING(shape <<"\nrandom obj pose"));
+    //if(!n) C.view(true);
 
     //== SAMPLE A RANDOM+REJECT+REFINE GRASP POSE
     arr relGripperPose = sampleGraspPose();
@@ -390,7 +412,7 @@ void ShapenetGrasps::displaySamples(const arr& X, const uintA& shapes, const arr
     uint idx=0;
     if(shape2place.find(shape) == shape2place.end()){
       idx = shape2place.size();
-      bool succ = addSceneObject(opt.filesPrefix+files(shape), idx, false, true);
+      bool succ = addSceneObject(opt.filesPrefix+files(shape), idx, false, false);
       CHECK(succ, "");
       shape2place[shape] = idx;
     }else{
@@ -399,11 +421,13 @@ void ShapenetGrasps::displaySamples(const arr& X, const uintA& shapes, const arr
 
     //add gripper relative to objPts-idx
     C.addCopy(Cgripper.frames, {});
-    setGraspPose(pose, STRING("objPts"<<idx));
+    setGraspPose(pose, STRING("obj"<<idx<<"_pts"));
 
     if(opt.verbose>0){
+      //C.get_viewer()->nonThreaded=true;
       if(Scores.N) C.view(opt.verbose>1, STRING("sample #" <<i << " score:\n" <<Scores[i]));
       else C.view(opt.verbose>1, STRING("sample #" <<i));
+      //for(uint k=0;k<20;k++) C.get_viewer()->savePng();
     }
   }
   C.view(true, STRING("display n:" <<N));

src/DataGen/shapenetGrasps.h

@@ -7,8 +7,8 @@
 struct ShapenetGrasps_Options {
   RAI_PARAM("ShapenetGrasps/", int, verbose, 1)
   RAI_PARAM("ShapenetGrasps/", rai::String, filesPrefix, "shapenet/models/")
-  RAI_PARAM("ShapenetGrasps/", int, numShapes, -1)
-  RAI_PARAM("ShapenetGrasps/", int, startShape, 0)
+  RAI_PARAM("ShapenetGrasps/", int, startShape, 3)
+  RAI_PARAM("ShapenetGrasps/", int, endShape, 5)
   RAI_PARAM("ShapenetGrasps/", int, simVerbose, 0)
   RAI_PARAM("ShapenetGrasps/", int, optVerbose, 0)
   RAI_PARAM("ShapenetGrasps/", double, simTau, .01)
@@ -29,19 +29,20 @@ struct ShapenetGrasps{
   void displaySamples(const arr& X, const uintA& shapes, const arr& Scores={});
 
   //-- direct interfaces
-  bool loadObject(uint shape, bool rndPose=true);
+  bool loadObject(uint shape, bool rndOrientation=true);
+  void resetObjectPose(int idx=0, bool rndOrientation=true);
   arr getPointCloud();
   arr sampleGraspPose();
   void setGraspPose(const arr& pose, const char* objPts="objPts0");
   arr evaluateGrasp();
 
 
   rai::Configuration C;
-private:
   StringA files;
+
+private:
   void clearScene();
-  void addSceneGripper();
-  bool addSceneObject(const char* file, int idx, bool rndPose=true, bool visual=false);
+  bool addSceneObject(const char* file, int idx, bool rndOri=true, bool visual=false);
 };
 
 arr sampleGraspCandidate(rai::Configuration& C, const char *ptsFrame, const char* refFrame, double pregraspNormalSdv=.2, int verbose=1);

src/Kin/dof_direction.cpp

@@ -51,7 +51,7 @@ arr DirectionDof::calcDofsFromConfig() const {
 void DirectionDof::read(const Graph& ats){
 }
 
-void DirectionDof::write(Graph& ats){
+void DirectionDof::write(Graph& ats) const{
   ats.add<String>("joint", "direction");
 }
 

src/Kin/dof_direction.h

@@ -24,7 +24,7 @@ struct DirectionDof : Dof, NonCopyable {
   virtual void setDofs(const arr& q_full, uint qIndex=0);
   virtual arr calcDofsFromConfig() const;
   void read(const Graph& ats);
-  void write(Graph& ats);
+  void write(Graph& ats) const;
 
   void kinVec(arr& y, arr& J) const;
 

src/Kin/dof_path.h

@@ -28,6 +28,5 @@ struct PathDof : Dof, NonCopyable {
 
   void getJacobians(arr& Jpos, arr& Jang) const;
 };
-stdOutPipe(PathDof)
 
 }

src/Kin/frame.cpp

@@ -1097,8 +1097,8 @@ const rai::Joint* rai::Dof::joint() const { return dynamic_cast<const Joint*>(th
 
 const rai::ForceExchangeDof* rai::Dof::fex() const { return dynamic_cast<const ForceExchangeDof*>(this); }
 
-void rai::Dof::write(std::ostream& os) const {
-  os <<"DOF of frame '" <<frame->name <<"'";
+void rai::Dof::write(Graph& ats) const {
+  ats.add<bool>(STRING("DOF_"<<frame->name));
 }
 
 //===========================================================================

src/Kin/frame.h

@@ -246,9 +246,8 @@ struct Dof {
   const Joint* joint() const;
   const ForceExchangeDof* fex() const;
 
-  virtual void write(std::ostream& os) const;
+  virtual void write(Graph& ats) const;
 };
-stdOutPipe(Dof)
 
 //===========================================================================
 
@@ -315,6 +314,7 @@ struct Inertia : NonCopyable {
   void setZero() { mass=0; com=0; matrix=0; }
   void add(const Inertia& I, const rai::Transformation& rel);
   void defaultInertiaByShape();
+  void scaleTo(double _mass){ matrix*=_mass/mass; mass=_mass; }
 
   rai::Transformation getDiagTransform(arr& diag);