Merge pull request #4 from gtrll/feature/savesdf

save gpmp2 sdf to bin file and an example

Author: mhmukadam

examples/save_sdf_pr2.py

@@ -0,0 +1,80 @@
+import atexit
+from openravepy import *
+import orgpmp2.orgpmp2
+import time
+import types
+import prpy.kin
+import prpy.rave
+import sys
+sys.path.append('data')
+import problemsets
+
+# Initialize openrave logging
+from openravepy.misc import InitOpenRAVELogging
+InitOpenRAVELogging()
+
+# Problemset
+problemset = 'industrial2'
+
+# Start and end joint angles
+n_states, states = problemsets.states(problemset)
+start_joints = numpy.array(states[3])
+end_joints = numpy.array(states[1])
+
+# Set up openrave and load env
+RaveInitialize(True, level=DebugLevel.Info)
+atexit.register(RaveDestroy)
+e = Environment()
+atexit.register(e.Destroy)
+e.Load(problemsets.env_file(problemset))
+e.Load('data/robots/pr2_gpmp2spheres.robot.xml')
+e.SetViewer('qtcoin')
+
+# Set up robot
+r = e.GetRobots()[0]
+r.SetTransform(matrixFromPose(problemsets.default_base_pose(problemset)))
+rave_joint_names = [joint.GetName() for joint in r.GetJoints()]
+rave_inds, rave_values = [],[]
+for (name,val) in zip(problemsets.joint_names(problemset), problemsets.default_joint_values(problemset)):
+  if name in rave_joint_names:
+    i = rave_joint_names.index(name)
+    rave_inds.append(i)
+    rave_values.append(val)
+r.SetDOFValues(rave_values, rave_inds)
+active_joint_inds = [rave_joint_names.index(name) for name in problemsets.active_joints(problemset)]
+r.SetActiveDOFs(active_joint_inds, problemsets.active_affine(problemset))
+r.SetActiveDOFValues(start_joints)
+
+# Calculate arm_pose
+l = r.GetLinks()
+for i in l:  
+  if (i.GetName() == "torso_lift_link"):
+    lp1 = poseFromMatrix(i.GetTransform())
+  if (i.GetName() == "r_shoulder_pan_link"):
+    lp2 = poseFromMatrix(i.GetTransform())
+arm_pose = numpy.array([lp1[0], lp1[1], lp1[2], lp1[3], lp2[4], lp2[5], lp2[6]])
+arm_origin = numpy.array([lp2[4], lp2[5], lp2[6]])
+
+# Load gpmp2
+m_gpmp2 = RaveCreateModule(e,'orgpmp2')
+orgpmp2.orgpmp2.bind(m_gpmp2)
+
+# SDF
+# remove right arm links to calculate sdf
+l = r.GetLinks()
+right_arm_links = ['r_shoulder_pan_link', 'r_shoulder_lift_link', 
+'r_upper_arm_roll_link', 'r_upper_arm_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 
+'r_forearm_cam_frame', 'r_forearm_cam_optical_frame', 'r_forearm_link', 'r_wrist_flex_link', 
+'r_wrist_roll_link', 'r_gripper_palm_link', 'r_gripper_l_finger_link', 'r_gripper_l_finger_tip_link', 
+'r_gripper_motor_slider_link', 'r_gripper_motor_screw_link', 'r_gripper_led_frame', 
+'r_gripper_motor_accelerometer_link', 'r_gripper_r_finger_link', 'r_gripper_r_finger_tip_link', 
+'r_gripper_l_finger_tip_frame', 'r_gripper_tool_frame']
+for i in l:
+  if i.GetName() in right_arm_links:
+    i.Enable(False)
+# Compute distance field for the env and remaining robot
+m_gpmp2.computedistancefield(cache_filename='sdf_env_'+problemset+'.dat',
+  centroid=arm_origin,extents=numpy.array([1.2,1.2,1.2]),res=0.02,save_sdf=1)
+
+raw_input()
+sys.exit()

pythonsrc/orgpmp2/orgpmp2.py

@@ -26,7 +26,7 @@ def viewspheres(mod, robot=None, releasegil=False):
    return mod.SendCommand(cmd, releasegil)
 
 def computedistancefield(mod, res=None, centroid=None, extents=None,
-   cache_filename=None, releasegil=False):
+   cache_filename=None, save_sdf=None, releasegil=False):
    cmd = 'computedistancefield'
    if res is not None:
       cmd += ' res %f' % res
@@ -36,6 +36,8 @@ def computedistancefield(mod, res=None, centroid=None, extents=None,
       cmd += ' extents %s' % shquot(' '.join([str(v) for v in extents]))
    if cache_filename is not None:
       cmd += ' cache_filename %s' % shquot(cache_filename)
+   if save_sdf is not None:
+      cmd += ' save_sdf %d' % save_sdf
    print 'cmd:', cmd
    return mod.SendCommand(cmd, releasegil)
 

src/orgpmp2_mod.cpp

@@ -5,6 +5,7 @@
 
 #include <time.h>
 #include <cblas.h>
+#include <string>
 
 extern "C" {
 #include "utils/grid.h"
@@ -252,6 +253,7 @@ int mod::computedistancefield(int argc, char * argv[], std::ostream& sout)
   struct timespec ticks_tic;
   struct timespec ticks_toc;
   int sdf_data_loaded;
+  int save_sdf = 0;
 
   // lock environment
   lockenv = OpenRAVE::EnvironmentMutex::scoped_lock(this->e->GetMutex());
@@ -289,6 +291,8 @@ int mod::computedistancefield(int argc, char * argv[], std::ostream& sout)
       res = atof(argv[++i]);
     else if (strcmp(argv[i],"cache_filename")==0 && i+1<argc)
       cache_filename = argv[++i];
+    else if (strcmp(argv[i],"save_sdf")==0 && i+1<argc)
+      save_sdf = atoi(argv[++i]);
     else {
       RAVELOG_ERROR("argument %s not known!\n", argv[i]);
       throw OpenRAVE::openrave_exception("Bad arguments!");
@@ -459,7 +463,7 @@ int mod::computedistancefield(int argc, char * argv[], std::ostream& sout)
   this->n_sdfs++;
 
   // assign sdf to gtsam variable
-  RAVELOG_INFO("Saving sdf to gtsam variable.\n");
+  RAVELOG_INFO("Saving sdf to gpmp2 variable.\n");
 
   {
     using namespace std;
@@ -482,22 +486,14 @@ int mod::computedistancefield(int argc, char * argv[], std::ostream& sout)
     Point3 gorigin(origin[0],origin[1],origin[2]);
 
     this->gtsam_sdf = gpmp2::SignedDistanceField(gorigin, res, env_sdf);
-/*
-    // save to text file
-    std::ofstream myFile;
-    myFile.open((std::string(getenv("HOME"))+"/Desktop/sdf.txt").c_str(), std::ios::app);
-    for (k=0; k<gsize_z; k++) {
-      for (i=0; i<gsize_y; i++) {
-        for (j=0; j<gsize_x; j++) {
-          myFile<<env_sdf[k](i,j)<<"\t";
-        }
-        myFile<<std::endl;
-      }
-      myFile<<std::endl;
-      myFile<<std::endl;
+
+    if (save_sdf)
+    {
+      string save_file(cache_filename);
+      save_file = save_file.substr(0,save_file.find_last_of('.'))+".bin";
+      RAVELOG_INFO("Saving gpmp2 sdf to .bin file.\n");
+      gtsam_sdf.saveSDF(save_file);
     }
-    myFile.close();
-*/
   }
   return 0;
 }