00001
00002
00003
00004
00005
00006
00007 from ctypes import *
00008 from ctypes.util import find_library
00009
00010
00011
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
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
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
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
