示例#1
0
def generateBodies(cloud, id):
    # BEGIN addtoenv
    names = []
    if args.geom_type == "mesh":
        mesh = generate_mesh(cloud)
        name = "simple_mesh_%i" % id
        mesh_body = mk.create_trimesh(env,
                                      get_xyz_world_frame(mesh.getCloud()),
                                      np.array(mesh.getFaces()),
                                      name=name)
        mesh_body.SetUserData(
            "bt_use_trimesh", True
        )  # Tell collision checker to use the trimesh rather than the convex hull of it
        names.append(name)
    elif args.geom_type == "cd":
        big_mesh = generate_mesh(cloud)
        convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
        for (i, mesh) in enumerate(convex_meshes):
            name = "mesh_%i_%i" % (id, i)
            verts = get_xyz_world_frame(mesh.getCloud())
            mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
            randcolor = np.random.rand(3)

            print 'vertices'
            print mesh.getVertices()

            if not isValidMesh(mesh):
                print('Mesh invalid. Removing kinbody')
                env.RemoveKinBody(env.GetKinBody(name))
                continue

            env.GetKinBody(name).GetLinks()[0].GetGeometries(
            )[0].SetAmbientColor(randcolor)
            env.GetKinBody(name).GetLinks()[0].GetGeometries(
            )[0].SetDiffuseColor(randcolor)
            names.append(name)
    # END  addtoenv
    elif args.geom_type == "spheres":
        raise Exception("don't use spheres--there's a performance issue")
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "spheres_%i" % id
        mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    elif args.geom_type == "boxes":
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "boxes_%i" % id
        mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    return names
示例#2
0
def extract_red_alphashape(cloud, robot):
    """
    extract red, get alpha shape, downsample
    """
    raise NotImplementedError
    
    # downsample cloud
    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
    
    # transform into body frame
    xyz1_kinect = cloud_ds.to2dArray()
    xyz1_kinect[:,3] = 1
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    xyz1_robot = xyz1_kinect.dot(T_w_k.T)
    
    # compute 2D alpha shape
    xyz1_robot_flat = xyz1_robot.copy()
    xyz1_robot_flat[:,2] = 0 # set z coordinates to zero
    xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat)
    
    # unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search
    cloud_robot_flatalphashape = cloudprocpy.CloudXYZ()
    cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape)
    cloud_robot_flat = cloudprocpy.CloudXYZ()
    cloud_robot_flat.from2dArray(xyz1_robot_flat)
    alpha_inds = cloudprocpy.getNearestNeighborIndices(xyz1_robot_flatalphashape, xyz1_robot_flat)

    xyz_robot_alphashape = xyz1_robot_flatalphashape[:,:3]
    
    # put back z coordinate
    xyz_robot_alphashape[:,2] = xyz1_robot[alpha_inds,2] 

    return xyz_robot_alphashape[:,:3]
示例#3
0
文件: clouds.py 项目: zzz622848/lfd
def downsample(xyz, v):
    import cloudprocpy
    if xyz.shape[1] == 3:
        cloud = cloudprocpy.CloudXYZ()
        xyz1 = np.ones((len(xyz), 4), 'float')
        xyz1[:, :3] = xyz
        cloud.from2dArray(xyz1)
        cloud = cloudprocpy.downsampleCloud(cloud, v)
        return cloud.to2dArray()[:, :3]
    else:
        # rgb fields needs to be packed and upacked as described in here
        # http://docs.pointclouds.org/1.7.0/structpcl_1_1_point_x_y_z_r_g_b.html

        xyzrgb = xyz
        n = xyzrgb.shape[0]
        cloud = cloudprocpy.CloudXYZRGB()
        xyzrgb1 = np.ones((n, 8), 'float')
        xyzrgb1[:, :3] = xyzrgb[:, :3]
        xyzrgb1[:, 4] = cloudprocpy.packRGBs(xyzrgb[:, 3:] * 255.0)
        xyzrgb1[:, 5:] = np.zeros(
            (n, 3))  # padding that is not used. set to zero just in case
        cloud.from2dArray(xyzrgb1)
        cloud = cloudprocpy.downsampleColorCloud(cloud, v)
        xyzrgb1 = cloud.to2dArray()
        return np.c_[xyzrgb1[:, :3],
                     cloudprocpy.unpackRGBs(xyzrgb1[:, 4]) / 255.0]
示例#4
0
def extract_red_alphashape(cloud, robot):
    """
    extract red, get alpha shape, downsample
    """
    raise NotImplementedError
    
    # downsample cloud
    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
    
    # transform into body frame
    xyz1_kinect = cloud_ds.to2dArray()
    xyz1_kinect[:,3] = 1
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    xyz1_robot = xyz1_kinect.dot(T_w_k.T)
    
    # compute 2D alpha shape
    xyz1_robot_flat = xyz1_robot.copy()
    xyz1_robot_flat[:,2] = 0 # set z coordinates to zero
    xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat)
    
    # unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search
    cloud_robot_flatalphashape = cloudprocpy.CloudXYZ()
    cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape)
    cloud_robot_flat = cloudprocpy.CloudXYZ()
    cloud_robot_flat.from2dArray(xyz1_robot_flat)
    alpha_inds = cloudprocpy.getNearestNeighborIndices(xyz1_robot_flatalphashape, xyz1_robot_flat)

    xyz_robot_alphashape = xyz1_robot_flatalphashape[:,:3]
    
    # put back z coordinate
    xyz_robot_alphashape[:,2] = xyz1_robot[alpha_inds,2] 

    return xyz_robot_alphashape[:,:3]
示例#5
0
def downsample(xyz, v):
  cloud = cloudprocpy.CloudXYZ()
  xyz1 = np.ones((len(xyz),4),'float')
  xyz1[:,:3] = xyz
  cloud.from2dArray(xyz1)
  cloud = cloudprocpy.downsampleCloud(cloud, v)
  return cloud.to2dArray()[:,:3]
示例#6
0
def downsample(xyz, v):
    cloud = cloudprocpy.CloudXYZ()
    xyz1 = np.ones((len(xyz), 4), 'float')
    xyz1[:, :3] = xyz
    cloud.from2dArray(xyz1)
    cloud = cloudprocpy.downsampleCloud(cloud, v)
    return cloud.to2dArray()[:, :3]
示例#7
0
def generateBodies(cloud, id):
    # BEGIN addtoenv
    names = []
    if args.geom_type == "mesh":
        mesh = generate_mesh(cloud)
        name = "simple_mesh_%i"%id
        mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
        mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
        names.append(name)
    elif args.geom_type == "cd":
        big_mesh = generate_mesh(cloud)
        convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
        for (i,mesh) in enumerate(convex_meshes):
            name = "mesh_%i_%i" % (id,i)
            verts = get_xyz_world_frame(mesh.getCloud())
            mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
            randcolor = np.random.rand(3)

	    print 'vertices'
	    print mesh.getVertices()

	    if not isValidMesh(mesh):
	        print('Mesh invalid. Removing kinbody')
	        env.RemoveKinBody(env.GetKinBody(name))
	        continue
	

            env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
            env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
            names.append(name)
    # END  addtoenv        
    elif args.geom_type == "spheres":
        raise Exception("don't use spheres--there's a performance issue")
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "spheres_%i"%id
        mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    elif args.geom_type == "boxes":
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "boxes_%i"%id
        mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    return names
示例#8
0
def downsample_colored(xyzrgb, v):
    import cloudprocpy
    cloud = cloudprocpy.CloudXYZ()
    xyz1 = np.ones((len(xyzrgb),4),'float')
    xyz1[:,:3] = xyzrgb[:,:3]
    cloud.from2dArray(xyz1)
    
    cloud_downsampled = cloudprocpy.downsampleCloud(cloud, v)
    indices = cloudprocpy.getNearestNeighborIndices(cloud_downsampled, cloud)
    
    return np.hstack((cloud_downsampled.to2dArray()[:,:3], xyzrgb[indices,3:]))    
 def generateBodies(self, cloud, id):
     # BEGIN addtoenv
     names = []
     if self.geom_type == "mesh":
         mesh = self.generate_mesh(cloud)
         name = "simple_mesh_%i"%id
         mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
         mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
         names.append(name)
         
     elif self.geom_type == "cd":
         big_mesh = self.generate_mesh(cloud)
         #name = 'big_mesh'
         #verts = self.get_xyz_world_frame(big_mesh.getCloud())
         #mk.create_trimesh(self.env, verts, big_mesh.getTriangles(), name=name)
             
         convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
         for (i,mesh) in enumerate(convex_meshes):
             name = "mesh_%i_%i" % (id,i)
             verts = self.get_xyz_world_frame(mesh.getCloud())
             mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
             randcolor = np.random.rand(3)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
             names.append(name)
     # END  addtoenv   
          
     elif self.geom_type == "spheres":
         raise Exception("don't use spheres--there's a performance issue")
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "spheres_%i"%id
         mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
         
     elif self.geom_type == "boxes":
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "boxes_%i"%id
         mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
     return names
示例#10
0
    def voxelize(self, cloud, voxelSize=0.003):
        '''TODO'''

        cloud = numpy.ascontiguousarray(
            cloud)  # cloudprocpy assumes this ordering
        n = cloud.shape[0]
        augment = zeros((n, 1))
        bigCloud = concatenate((cloud, augment), axis=1)
        cloudTrajopt = cloudprocpy.CloudXYZ()
        cloudTrajopt.from2dArray(bigCloud)
        dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, voxelSize)
        cloud = dsCloud.to2dArray()[:, :3]
        return cloud
示例#11
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # XXXX pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red",red_mask.astype('uint8')*255)
        cv2.imshow("above_table", height_mask.astype('uint8')*255)
        cv2.imshow("red and above table", good_mask.astype('uint8')*255)
        print "press enter to continue"
        cv2.waitKey()
        
        
    

    good_xyz = xyz_w[good_mask]
    
    cloud = cloudprocpy.CloudXYZ()    
    good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
    good_xyz1[:,:3] = good_xyz
    cloud.from2dArray(good_xyz1)
    
    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
    
    return cloud_ds.to2dArray()[:,:3]
示例#12
0
 def generateBodies(self, cloud, id):
     # BEGIN addtoenv
     names = []
     if self.geom_type == "mesh":
         mesh = self.generate_mesh(cloud)
         name = "simple_mesh_%i"%id
         mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
         mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
         names.append(name)
         
     elif self.geom_type == "cd":
         big_mesh = self.generate_mesh(cloud)
         convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
         for (i,mesh) in enumerate(convex_meshes):
             name = "mesh_%i_%i" % (id,i)
             verts = self.get_xyz_world_frame(mesh.getCloud())
             mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
             randcolor = np.random.rand(3)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
             names.append(name)
     # END  addtoenv   
          
     elif self.geom_type == "spheres":
         raise Exception("don't use spheres--there's a performance issue")
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "spheres_%i"%id
         mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
         
     elif self.geom_type == "boxes":
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "boxes_%i"%id
         mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
     return names
示例#13
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """

    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:, :, 0]
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]

    red_mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100)

    valid_mask = depth > 0

    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]

    z = xyz_w[:, :, 2]
    z0 = xyz_k[:, :, 2]
    if DEBUG_PLOTS:
        cv2.imshow("z0", z0 / z0.max())
        cv2.imshow("z", z / z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()

    height_mask = xyz_w[:, :, 2] > .7  # XXXX pass in parameter

    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red", red_mask.astype('uint8') * 255)
        cv2.imshow("above_table", height_mask.astype('uint8') * 255)
        cv2.imshow("red and above table", good_mask.astype('uint8') * 255)
        print "press enter to continue"
        cv2.waitKey()

    good_xyz = xyz_w[good_mask]

    cloud = cloudprocpy.CloudXYZ()
    good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
    good_xyz1[:, :3] = good_xyz
    cloud.from2dArray(good_xyz1)

    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)

    return cloud_ds.to2dArray()[:, :3]
def add_convexified_pointcloud_to_env(sim, pc2, transform=np.eye(4), num_cd_components=20, point_cloud_filter=lambda point: True):
    """
    Convexifies point cloud and adds to openrave environment
    
    :param sim: pr2_sim.simulator.Simulator
    :param pc2: the point cloud to read from
    :type  pc2: sensor_msgs.PointCloud2
    :param transform: 4x4 np.ndarray of transform for cloud in frame base_link
    :param point_cloud_filter: True if keep point
    """
    transform_world = sim.transform_from_to(transform, 'base_link', 'world')
    
    full_cloud = pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter)
    cloud = cloudprocpy.downsampleCloud(full_cloud, .005) # .005

    big_mesh = generate_mesh(cloud)
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, num_cd_components)
    for i, mesh in enumerate(convex_meshes):
        sim.add_kinbody(mesh.getVertices(), mesh.getTriangles(), name='mesh_{0}'.format(i), check_collision=True)
示例#15
0
def add_convexified_pointcloud_to_env(sim, pc2, transform=np.eye(4), num_cd_components=20, point_cloud_filter=lambda point: True):
    """
    Convexifies point cloud and adds to openrave environment
    
    :param sim: pr2_sim.simulator.Simulator
    :param pc2: the point cloud to read from
    :type  pc2: sensor_msgs.PointCloud2
    :param transform: 4x4 np.ndarray of transform for cloud in frame base_link
    :param point_cloud_filter: True if keep point
    """
    transform_world = sim.transform_from_to(transform, 'base_link', 'world')
    
    full_cloud = pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter)
    cloud = cloudprocpy.downsampleCloud(full_cloud, .005) # .005

    big_mesh = generate_mesh(cloud)
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, num_cd_components)
    for i, mesh in enumerate(convex_meshes):
        sim.add_kinbody(mesh.getVertices(), mesh.getTriangles(), name='mesh_{0}'.format(i), check_collision=True)
示例#16
0
    def addCloudToEnvironment(self, cloud, cubeSize=0.02):
        '''
    Add a point cloud to the OpenRAVE environment as a collision obstacle.
    
    @param cloud: list of 3-tuples, [(x1,y1,z1),...,(xn,yn,zn)]
    '''

        # remove existing cloud, if there is one
        self.removeCloudFromEnvironment()

        # convert point cloud to expected format
        cloud = ascontiguousarray(cloud)  # cloudprocpy assumes this
        cloud = concatenate((cloud, zeros((cloud.shape[0], 1))), axis=1)
        cloudTrajopt = cloudprocpy.CloudXYZ()
        cloudTrajopt.from2dArray(cloud)

        # downsample and add to environment
        dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, cubeSize)
        self.obstacleCloud = make_kinbodies.create_boxes(
            self.env,
            dsCloud.to2dArray()[:, :3], cubeSize / 2.0)
示例#17
0
文件: clouds.py 项目: amoliu/lfd
def downsample(xyz, v):
    import cloudprocpy
    if xyz.shape[1] == 3:
        cloud = cloudprocpy.CloudXYZ()
        xyz1 = np.ones((len(xyz),4),'float')
        xyz1[:,:3] = xyz
        cloud.from2dArray(xyz1)
        cloud = cloudprocpy.downsampleCloud(cloud, v)
        return cloud.to2dArray()[:,:3]
    else:
        # rgb fields needs to be packed and upacked as described in here
        # http://docs.pointclouds.org/1.7.0/structpcl_1_1_point_x_y_z_r_g_b.html

        xyzrgb = xyz
        n = xyzrgb.shape[0]
        cloud = cloudprocpy.CloudXYZRGB()
        xyzrgb1 = np.ones((n,8),'float')
        xyzrgb1[:,:3] = xyzrgb[:,:3]
        xyzrgb1[:,4] = cloudprocpy.packRGBs(xyzrgb[:,3:] * 255.0)
        xyzrgb1[:,5:] = np.zeros((n,3)) # padding that is not used. set to zero just in case
        cloud.from2dArray(xyzrgb1)
        cloud = cloudprocpy.downsampleColorCloud(cloud, v)
        xyzrgb1 = cloud.to2dArray()
        return np.c_[xyzrgb1[:,:3], cloudprocpy.unpackRGBs(xyzrgb1[:,4]) / 255.0]
    mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
elif args.geom_type == "cd":
    big_mesh = generate_mesh(cloud_orig)
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
    for (i,mesh) in enumerate(convex_meshes):
        name = "mesh%i"%i
        verts = get_xyz_world_frame(mesh.getCloud())
        mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
        randcolor = np.random.rand(3)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
# END  addtoenv        
elif args.geom_type == "spheres":
    raise Exception("don't use spheres--there's a performance issue")
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)
elif args.geom_type == "boxes":
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02)
    

def full_body_drive_and_reach(robot, link_name, xyz_targ, quat_targ):
        
    request = {        
        "basic_info" : {
            "n_steps" : 10,
            "manip" : "active",
            "start_fixed" : True
        },
示例#19
0
    big_mesh = generate_mesh(cloud_orig)
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
    for (i, mesh) in enumerate(convex_meshes):
        name = "mesh%i" % i
        verts = get_xyz_world_frame(mesh.getCloud())
        mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
        randcolor = np.random.rand(3)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(
            randcolor)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
            randcolor)
# END  addtoenv
elif args.geom_type == "spheres":
    raise Exception("don't use spheres--there's a performance issue")
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)
elif args.geom_type == "boxes":
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02)


def full_body_drive_and_reach(link_name, xyz_targ, quat_targ):

    request = {
        "basic_info": {
            "n_steps": 10,
            "manip": "active",
            "start_fixed": True
        },
示例#20
0
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 = []