sgp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #Wrapper for the SGP library
00003 #Copyright 2010 Alexis Maldonado <maldonad@cs.tum.edu>
00004 #Released under the LGPL v3 License
00005 
00006 
00007 from ctypes import *
00008 from ctypes.util import find_library
00009 
00010 #Loading the library
00011 #libname = find_library("sgp")
00012 
00013 import commands
00014 libdir = commands.getoutput("rospack find simple_grasp_planner")
00015 libname = libdir + "/lib/libsgp.so"
00016 
00017 try:
00018     if (libname):
00019         libsgp=CDLL(libname)
00020     else:
00021         libsgp=CDLL('../lib/libsgp.so')
00022 except:
00023     libsgp=CDLL('libsgp.so')
00024 
00025 
00026 
00027 #Defining the Structures
00028 class Point3D(Structure):
00029     _fields_=[ ("x", c_double), ("y", c_double), ("z",c_double) ]
00030 
00031 max_tested_poses =  1000
00032 class HandConfig(Structure):
00033     _fields_=[ ("alpha", c_double), ("beta", c_double), ("delta_max", c_double) ]
00034 
00035 class HandConfList(Structure):
00036     _fields_=[ ("length", c_int), ("min_score_index", c_int), ("scores", c_double * max_tested_poses), ("configs", HandConfig * max_tested_poses)]
00037 
00038 
00039 class CovariancePoint(Structure):
00040     _fields_=[ ("sx", c_double ), ("sxy", c_double ), ("sxz", c_double ),
00041         ("syx", c_double ), ("sy", c_double ), ("syz", c_double ),
00042         ("szx", c_double ), ("szy", c_double ), ("sz", c_double ) ]
00043 
00044 #Factory functions
00045 def new_Point3D_list(length):
00046     pl=Point3D*length
00047     return(pl())
00048 
00049 def new_CovariancePoint_list(length):
00050     cl=CovariancePoint*length
00051     return(cl())
00052 
00053 #Function prototypes
00054 libsgp.InitSGP.argtypes = [POINTER(Point3D), c_int, c_double]
00055 libsgp.setTableHeight.argtypes = [c_double]
00056 libsgp.setTableHeight.restype = None
00057 libsgp.getTableHeight.argtypes = []
00058 libsgp.getTableHeight.restype = c_double
00059 libsgp.GetGraspLM.argtypes = [POINTER(Point3D), POINTER(CovariancePoint), c_int, c_double, c_double]
00060 libsgp.GetGraspLM.restype = HandConfig
00061 
00062 libsgp.GetGraspList.argtypes = [POINTER(Point3D), POINTER(CovariancePoint), c_int, c_double, c_double]
00063 libsgp.GetGraspList.restype = HandConfList
00064 
00065 
00066 libsgp.TransformPoint.argtypes = [Point3D, Point3D, c_double, c_double, c_double]
00067 libsgp.TransformPoint.restype = Point3D
00068 
00069 if (__name__ == "__main__"):
00070     print "Doing tests:"
00071     num_hand_points = 3
00072     hpl=new_Point3D_list(num_hand_points)
00073     hpl[1].y = -0.02
00074     hpl[1].z = 0.05
00075     hpl[2].y = 0.02
00076     hpl[2].z = 0.05
00077     num_objects = 1
00078     pl=new_Point3D_list(num_objects)
00079     cl=new_CovariancePoint_list(num_objects)
00080     cl[0].sx=0.1
00081     cl[0].sxy=-0.001
00082     cl[0].sxz=-0.001
00083     cl[0].sy=0.01
00084     cl[0].syx=-0.001
00085     cl[0].syz=-0.001
00086     cl[0].sz=0.01
00087     cl[0].szy=-0.001
00088     cl[0].szx=-0.001
00089     libsgp.InitSGP(hpl,num_hand_points, -0.1)
00090     offset_rot_z_side=1.0
00091     offset_rot_z_top=1.0
00092     conf = libsgp.GetGraspLM(pl,cl,num_objects, offset_rot_z_side, offset_rot_z_top)
00093     print conf.alpha
00094     print conf.beta
00095     print conf.delta_max
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


simple_grasp_planner
Author(s): U. Klank
autogenerated on Thu May 23 2013 09:20:22