import numpy as np import cloudprocpy cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd") cloud = cloudprocpy.downsampleCloud(cloud, .01) cloud = cloudprocpy.boxFilter(cloud, .5,5,-3,3,.1,2.5) cloud.save("/tmp/cloud.ply") xyzorig = cloud.to2dArray()[:,:3] xyzn = cloudprocpy.mlsAddNormals(cloud).to2dArray() xyz = xyzn[:,:3] normals = xyzn[:,4:7] from trajoptpy import math_utils normals = math_utils.normr(normals) camerapos = np.array([0,0,1.3]) ndotp = ((camerapos[None,:] - xyz) * normals).sum(axis=1) normals *= np.sign(ndotp)[:,None] import trajoptpy, openravepy env = openravepy.RaveGetEnvironment(1) if env is None: env = openravepy.Environment() viewer = trajoptpy.GetViewer(env) import IPython IPython.lib.inputhook.set_inputhook(viewer.Step) xyzn[:,4:7] = normals cloudwn = cloudprocpy.CloudXYZN() cloudwn.from2dArray(xyzn) cloudwn.save("/tmp/lasercloud.ply") handles = []
atexit.register(rave.RaveDestroy) #env.SetViewer('qtcoin') robot = env.GetRobots()[0] robot.SetDOFValues(np.zeros(robot.GetDOF())) robot.SetTransform(np.eye(4)) rgj = robot.GetJoint("r_gripper_l_finger_joint") robot.SetDOFValues([rgj.GetLimits()[-1]], [rgj.GetDOFIndex()]) tlj = robot.GetJoint("torso_lift_joint") robot.SetDOFValues([tlj.GetLimits()[-1]], [tlj.GetDOFIndex()]) print "processing point cloud" if env.GetKinBody("convexsoup") is None: cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd") cloud = cloudprocpy.boxFilter(cloud, -1,5,-5,5,.1,2) aabb = robot.GetLink("base_link").ComputeAABB() (xmin,ymin,zmin) = aabb.pos() - aabb.extents() (xmax,ymax,zmax) = aabb.pos() + aabb.extents() cloud = cloudprocpy.boxFilterNegative(cloud, xmin,xmax,ymin,ymax,zmin,zmax) cloud = cloudprocpy.downsampleCloud(cloud, .015) convex_soup.create_convex_soup(cloud, env) print "done processing" T = np.eye(4) T[:3,3] = [ 1.27 , -0.2 , 0.790675] ##################