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
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
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
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
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)
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
},
"costs" : [
{
"type" : "collision",
"params" : {"coeffs" : [10],"dist_pen" : [0.025]}
},
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
},
"costs": [{
"type": "collision",
"params": {
"coeffs": [10],
"dist_pen": [0.025]
