def AddConstraintBoxes(env,
robot,
handedness='right',
name_base="constraint_boxes_",
visible=False):
# Modifies environment to keep the robot inside a defined space
# Does so by adding invisible boxes around the robot, which planners
# avoid collisions with
#add a box behind the robot
box_behind = openravepy.RaveCreateKinBody(env, '')
box_behind.SetName(name_base + 'behind')
box_behind.InitFromBoxes(np.array([[0., 0., 0., 0.4, 0.1, 1.0]]), False)
env.Add(box_behind)
T = np.eye(4)
T[0:3, 3] = robot.GetTransform()[0:3, 3]
T[1, 3] = 0.57
if handedness == 'right':
T[0, 3] += 0.25
else:
T[0, 3] -= 0.25
box_behind.SetTransform(T)
#add a box above so we don't swing that way too high
box_above = openravepy.RaveCreateKinBody(env, '')
box_above.SetName(name_base + 'above')
box_above.InitFromBoxes(np.array([[0., 0., 0., 0.5, 0.5, 0.1]]), visible)
env.Add(box_above)
T = np.eye(4)
T[0:3, 3] = robot.GetTransform()[0:3, 3]
T[0, 3] += 0.25
T[1, 3] -= 0.25
T[2, 3] += 0.90
box_above.SetTransform(T)
box_left = openravepy.RaveCreateKinBody(env, '')
box_left.SetName(name_base + 'left')
box_left.InitFromBoxes(np.array([[0., 0., 0., 0.1, 0.5, 1.0]]), visible)
env.Add(box_left)
T = np.eye(4)
T[0:3, 3] = robot.GetTransform()[0:3, 3]
if handedness == 'right':
T[0, 3] += 0.9
else:
T[0, 3] += 0.25
T[1, 3] = 0.25
box_left.SetTransform(T)
box_right = openravepy.RaveCreateKinBody(env, '')
box_right.SetName(name_base + 'right')
box_right.InitFromBoxes(np.array([[0., 0., 0., 0.1, 0.5, 1.0]]), visible)
env.Add(box_right)
T = np.eye(4)
T[0:3, 3] = robot.GetTransform()[0:3, 3]
if handedness == 'right':
T[0, 3] -= 0.25
else:
T[0, 3] -= 0.9
T[1, 3] = 0.25
box_right.SetTransform(T)
def addSpheresToEnvironment(endpoint1, endpoint2):
#TODO: How do we add in the doodler?
radius = 0.0005
with env:
sphere = orpy.RaveCreateKinBody(env, '')
sphere.SetName(str(np.random.rand())) #give it a random name - we'll never reference it
sphere.InitFromSpheres(np.array([[endpoint1[0, 3], endpoint1[1, 3], endpoint1[2, 3], 0.002]]), True)
sphere.Enable(False)
env.Add(sphere, True)
with env:
sphere = orpy.RaveCreateKinBody(env, '')
sphere.SetName(str(np.random.rand())) #give it a random name - we'll never reference it
sphere.InitFromSpheres(np.array([[endpoint2[0, 3], endpoint2[1, 3], endpoint2[2, 3], 0.002]]), True)
sphere.Enable(False)
env.Add(sphere, True)
def setUp(self):
self.env = openravepy.Environment()
self.env.Load('wamtest1.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.manipulator = self.robot.GetManipulator('arm')
self.robot.Enable(True)
# Set all 7 DOF of the WAM arm to active
with self.env:
self.robot.SetActiveDOFs(self.manipulator.GetArmIndices())
self.robot.SetActiveManipulator(self.manipulator)
self.active_dof_indices = self.robot.GetActiveDOFIndices()
# Get the resolution (in radians) for the 7 joints
# [0.0043, 0.0087, 0.0087, 0.0174, 0.0193, 0.0282, 0.0282]
self.dof_resolutions = \
self.robot.GetDOFResolutions()[self.active_dof_indices]
#add the box object we will grab
self.to_grab_body = openravepy.RaveCreateKinBody(self.env, '')
self.to_grab_body.SetName('box')
self.to_grab_body.InitFromBoxes(numpy.array([[0., 0., 0., 0.1, 0.1, 0.1]]), False)
self.env.Add(self.to_grab_body)
self.to_grab_body.Enable(True)
T = numpy.eye(4)
T[2,3] = 20. #move far to not be in collision
self.to_grab_body.SetTransform(T)
def compute_bounding_box_corners(body, Tbody=None, scale=1.0):
"""
Computes the bounding box corners (8 corners) for the given body.
If C{Tbody} is given (not None), the corners are transformed.
The {scale} parameters is a factor used to scale the extents of the
bounding box.
@type body: orpy.KinBody
@param body: The OpenRAVE body
@type Tbody: np.array
@param Tbody: homogeneous transformation to transform the corners. If None,
the corners are given using the current position of the body in OpenRAVE.
@type scale: factor
@param scale: the scale factor to modify the extents of the bounding box.
@rtype: list
@return: list containing the 8 box corners. Each corner is a XYZ point of type C{np.array}.
"""
# Create a dummy body an use OpenRAVE to get the corners
env = body.GetEnv()
dummy = orpy.RaveCreateKinBody(env, '')
dummy.Clone(body, 0)
if Tbody is not None:
with env:
dummy.SetTransform(Tbody)
aabb = dummy.ComputeAABB()
corners = []
for k in itertools.product([-1, 1], [-1, 1], [-1, 1]):
corners.append(aabb.pos() + np.array(k) * aabb.extents() * scale)
return corners
def make_collision_box_body(kinbody,
add_to_pos=np.array([0.0, 0.0, 0.0]),
add_to_extents=np.array([0.0, 0.0, 0.0])):
"""Creates a box at position of kin body with optionally modified values
@param kinbody body we want to create a box around
@param env openrave environment
@param add_to_pos modifications to the position of the axis aligned bounding box
@param add_to_extents modifications to the extents of the axis aligned bounding box
@return kinbody of box object
"""
env = kinbody.GetEnv()
with env:
old_transform_body = kinbody.GetTransform()
kinbody.SetTransform(np.eye(4))
kinbody_aabb = kinbody.ComputeAABB()
box_aabb_arr = np.append(kinbody_aabb.pos() + add_to_pos,
kinbody_aabb.extents() + add_to_extents)
box_aabb_arr = box_aabb_arr.reshape(1, 6)
#print 'aabb box: ' + str(box_aabb_arr)
box_body = openravepy.RaveCreateKinBody(env, '')
box_body.SetName(kinbody.GetName() + '_box')
box_body.InitFromBoxes(box_aabb_arr, False)
env.Add(box_body)
kinbody.SetTransform(old_transform_body)
box_body.SetTransform(old_transform_body)
box_body.Enable(True)
return box_body
def add_box(self, pose, extents, name=None):
"""
:param pose: 4x4 np.ndarray in frame world
:param extents: length 3 list/np.ndarray of axis lengths
:param name: name of kinbody to be added
:return kinbody
"""
name = name if name is not None else 'box'+str(self._kinbody_num)
box = rave.KinBody.Link.GeometryInfo()
box._type = rave.KinBody.Link.GeomType.Box
# box._t = pose
box._vGeomData = list(extents)
box._vDiffuseColor = np.array([1., 0., 0.])
kinbox = rave.RaveCreateKinBody(self.env, '')
kinbox.InitFromGeometries([box])
kinbox.SetTransform(pose)
kinbox.SetName(name)
self.add(kinbox)
self._kinbody_num += 1
# l = kinbox.GetLinks()[0]
# g = l.GetGeometries()[0]
# trimesh = l.GetCollisionData()
# trimesh.indices = trimesh.indices
# g.SetCollisionMesh(trimesh)
# self.env.Add(kinbox, True)
# self.kinbodies.add(kinbox)
# self._kinbody_num += 1
return kinbox
def test_cd():
global handles
env = rave.Environment()
env.Load('../envs/pr2-test.env.xml')
env.SetViewer('qtcoin')
mug = env.GetKinBody('mug')
mug_pos = tfx.pose(mug.GetTransform()).position.array
mugcd = rave.databases.convexdecomposition.ConvexDecompositionModel(mug)
if not mugcd.load():
mugcd.autogenerate()
mugcd_trimesh = mugcd.GenerateTrimeshFromHulls(mugcd.linkgeometry[0][0][1])
new_mug = rave.RaveCreateKinBody(env, '')
new_mug.SetName('new_mug')
new_mug.InitFromTrimesh(mugcd_trimesh)
new_mug.SetTransform(mug.GetTransform())
#env.Add(new_mug, True)
env.Remove(mug)
I, V = mugcd_trimesh.indices, mugcd_trimesh.vertices
for indices in I:
v0 = mug_pos + V[indices[0], :]
v1 = mug_pos + V[indices[1], :]
v2 = mug_pos + V[indices[2], :]
handles += utils.plot_segment(env, v0, v1)
handles += utils.plot_segment(env, v1, v2)
handles += utils.plot_segment(env, v2, v0)
IPython.embed()
def create_convex_soup(cloud, env, name="convexsoup"):
xyz = cloud.to2dArray()
indss = cloudprocpy.convexDecomp(cloud, .03)
geom_infos = []
for (i, inds) in enumerate(indss):
if len(inds) < 100:
continue # openrave is slow with a lot of geometries
origpts = xyz[inds]
hullpoints, hullinds = calc_hull(origpts)
if hullpoints is None:
print "failed to get convex hull!"
continue
gi = rave.KinBody.GeometryInfo()
gi._meshcollision = rave.TriMesh(hullpoints, hullinds)
gi._type = rave.GeometryType.Trimesh
gi._vAmbientColor = np.random.rand(3) / 2
geom_infos.append(gi)
body = rave.RaveCreateKinBody(env, '')
body.SetName(name)
body.InitFromGeometries(geom_infos)
env.Add(body)
def add_box(env, x, y, z, box_size=0.04):
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(np.array([[x, y, z, box_size, box_size, box_size]]),
True)
box.SetName('box')
env.Add(box, True)
return box
def PlaceCylinderAtOrigin(self, heightMinMax, radiusMinMax, name, saveBlock=True):
'''Places a single, random cylinder at the origin of the world frame.'''
# block creation parameters
colors = array(( \
(1.0, 0.0, 0.0, 0.5), (0.0, 1.0, 0.0, 0.5), (0.0, 0.0, 1.0, 0.5), (0.0, 1.0, 1.0 ,0.5),
(1.0, 0.0, 1.0, 0.5), (1.0, 1.0, 0.0, 0.5), (0.5, 1.0, 0.0, 0.5), (0.5, 0.0, 1.0, 0.5),
(0.0, 0.5, 1.0, 0.5), (1.0, 0.5, 0.0, 0.5), (1.0, 0.0, 0.5, 0.5), (0.0, 1.0, 0.5, 0.5) ))
with self.env:
# create and position block
height = uniform(heightMinMax[0], heightMinMax[1])
radius = uniform(radiusMinMax[0], radiusMinMax[1])
geomInfo = openravepy.KinBody.Link.GeometryInfo()
geomInfo._type = openravepy.KinBody.Link.GeomType.Cylinder
geomInfo._t[0,3] = radius
geomInfo._vGeomData = [radius, height]
geomInfo._bVisible = True
geomInfo._fTransparency = 0.0
geomInfo._vDiffuseColor = colors[randint(len(colors))]
body = openravepy.RaveCreateKinBody(self.env, '')
body.InitFromGeometries([geomInfo])
body.SetName(name)
self.env.Add(body, True)
# it's also possible to save blocks as ply if ctmconv is installed
if saveBlock:
self.env.Save(name + ".dae", openravepy.Environment.SelectionOptions.Body, name)
os.system("ctmconv " + name + ".dae " + name + ".ply")
print("Saved " + name + ".")
return body
def add_cube(self, x, y, z, dim_x, dim_y, dim_z, name='cube'):
body = openravepy.RaveCreateKinBody(self.env, '')
body.InitFromBoxes(np.array([[0.0, 0.0, 0.0, dim_x, dim_y, dim_z]]))
body.SetTransform([[1.0, 0.0, 0.0, x], [0.0, 1.0, 0.0, y],
[0.0, 0.0, 1.0, z], [0.0, 0.0, 0.0, 1.0]])
body.SetName(name)
self.env.Add(body, True)
def create_mesh_box(env, center, half_extents, name="box"):
box = rave.RaveCreateKinBody(env, '')
rx, ry, rz = half_extents
verts = np.array([
[-rx, -ry, -rz],
[-rx, -ry, rz],
[-rx, ry, -rz],
[-rx, ry, rz],
[rx, -ry, -rz],
[rx, -ry, rz],
[rx, ry, -rz],
[rx, ry, rz]])
faces= [
[0,1,2],
[3,1,2],
[0,1,4],
[5,1,4],
[0,2,4],
[6,2,4],
[7,6,5],
[4,6,5],
[7,6,3],
[2,6,3],
[7,5,3],
[1,5,3]]
box.SetName(name)
box.InitFromTrimesh(rave.TriMesh(verts, faces), True)
trans = np.eye(4)
trans[:3,3] = center
box.SetTransform(trans)
env.Add(box)
return box
def pixelInObstacle(self, pixel, pixelwidth, bounds):
'''
first obtain pixel bounds in terms of world coordinates
'''
pixminx = bounds[0][0] + (pixel[0] * pixelwidth)
pixminy = bounds[1][1] - ((pixel[1] + 1) * pixelwidth)
pixmaxx = bounds[0][0] + ((pixel[0] + 1) * pixelwidth)
pixmaxy = bounds[1][1] - (pixel[1] * pixelwidth)
'''
see if moving pixel-sized cube to centroid of pixel bound causes collision; return True if so, else False
'''
aveX = (pixmaxx + pixminx) / 2
aveY = (pixmaxy + pixminy) / 2
body = openravepy.RaveCreateKinBody(self.env, '')
body.SetName('tracer')
body.InitFromBoxes(
array([[aveX, aveY, 0.2, pixelwidth, pixelwidth, pixelwidth]]),
True) # make sure z-coordinate is bounded by shortest wall height
self.env.AddKinBody(body)
self.env.UpdatePublishedBodies()
if self.env.CheckCollision(body):
self.env.Remove(body)
return True
else:
self.env.Remove(body)
return False
def add_cylinder(self, pose, radius, height, name=None):
"""
:param pose: 4x4 np.ndarray
:return kinbody
"""
name = name if name is not None else 'cylinder'+str(self._kinbody_num)
cyl = rave.KinBody.Link.GeometryInfo()
cyl._type = rave.KinBody.Link.GeomType.Cylinder
# cyl._t = pose
cyl._vGeomData = [radius, height]
cyl._vDiffuseColor = np.array([0.6, 0.3, 0.])
kincyl = rave.RaveCreateKinBody(self.env, '')
kincyl.InitFromGeometries([cyl])
kincyl.SetTransform(pose)
kincyl.SetName(name)
self.add(kincyl)
self._kinbody_num += 1
# to prevent extra triangles showing up in ogre .mesh
# l = kincyl.GetLinks()[0]
# g = l.GetGeometries()[0]
# trimesh = l.GetCollisionData()
# trimesh.indices = trimesh.indices[4:]
# g.SetCollisionMesh(trimesh)
# self.env.Add(kincyl, True)
# self.added_kinbody_names.append(name)
# self._kinbody_num += 1
return kincyl
def load_body_from_bag(bagname, env):
bag = rosbag.Bag(bagname)
topic, msg, stamp = bag.read_messages('/collision_map_occ').next()
all_boxes = np.empty((len(msg.boxes), 6))
body = openravepy.RaveCreateKinBody(env, '')
body.SetName("obstacle")
for i, box in enumerate(msg.boxes):
values = [
box.center.x,
box.center.y,
box.center.z,
box.extents.x,
box.extents.y,
box.extents.z,
]
all_boxes[i, :] = values
centroid = np.mean(all_boxes[:, :3], axis=0)
all_boxes[:, :3] -= centroid
body.InitFromBoxes(all_boxes, True)
T = body.GetTransform()
T[:3, -1] = centroid
body.SetTransform(T)
return body
def __init__(self,
X,
Y,
Z,
pos=None,
rpy=None,
pose=None,
color='r',
visible=True,
transparency=None,
name=None,
dZ=0.):
self.X = X
self.Y = Y
self.Z = Z
aabb = [0., 0., dZ, X, Y, Z]
env = get_openrave_env()
with env:
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(array([array(aabb)]), True)
super(Box, self).__init__(box,
pos=pos,
rpy=rpy,
pose=pose,
color=color,
visible=visible,
transparency=transparency,
name=name)
env.Add(box, True)
def add_kinbody(self, vertices, triangles, name=None, check_collision=False):
"""
:param vertices: list of 3d np.ndarray corresponding to points in the mesh
:param triangles: list of 3d indices corresponding to vertices
:param name: name of the kinbody to be added
:param check_collision: if True, will not add kinbody if it collides with the robot
:return False if check_collision=True and there is a collision
"""
name = name if name is not None else 'kinbody'+str(time.time())
self.added_kinbody_names.append(name)
body = rave.RaveCreateKinBody(self.env, "")
body.InitFromTrimesh(trimesh=rave.TriMesh(vertices, triangles), draw=True)
body.SetName(name)
self.env.Add(body)
randcolor = np.random.rand(3)
body.GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
body.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
if check_collision:
if self.env.CheckCollision(self.robot, body):
self.env.Remove(body)
self.added_kinbody_names = self.added_kinbody_names[:-1]
return False
return True
def GenerateCylinderMesh(self, heightMinMax, radiusMinMax, name):
'''Generates a cylinder and saves it into a CAD model file.
- Input heightMinMax: Tuple specifying range (min, max) from which to select cylinder height.
- Input radiusMinmax: Tuple specifying range (min, max) from which to select cylinder radius.
- Input name: String name of object; also determines name of file to save.
- Returns body: Handle to the openrave object, added to the environment.
'''
# create object
height = uniform(heightMinMax[0], heightMinMax[1])
radius = uniform(radiusMinMax[0], radiusMinMax[1])
geomInfo = openravepy.KinBody.Link.GeometryInfo()
geomInfo._type = openravepy.KinBody.Link.GeomType.Cylinder
geomInfo._vGeomData = [radius, height]
geomInfo._vDiffuseColor = self.colors[randint(len(self.colors))]
body = openravepy.RaveCreateKinBody(self.env, "")
body.InitFromGeometries([geomInfo])
body.SetName(name)
body.height = height
body.radius = radius
self.env.Add(body, True)
# save mesh file
self.env.Save(name + ".dae",
openravepy.Environment.SelectionOptions.Body, name)
print("Saved " + name + ".")
return body
def set_env(self):
with self.env:
box = orpy.RaveCreateKinBody(self.env, '')
box.SetName('box')
box.InitFromBoxes(np.array([[0.5, 0.3, 1, 0.01, 0.04, 0.22]]),
True)
self.env.AddKinBody(box)
self.box_centroid = box.ComputeAABB().pos()
print self.box_centroid
def preview_in_rave(self, env):
body = orpy.RaveCreateKinBody(env, "")
body.SetName(self.name)
body.InitFromBoxes(
np.array([[
self.com_position[0], self.com_position[1],
self.com_position[2], self.size[0] / 2, self.size[1] / 2,
self.size[2] / 2
]]), True)
env.Add(body, True)
def __init__(self, X, Y, Z, pos=None, rpy=None, pose=None, color='r',
dZ=0.):
aabb = [0., 0., dZ, X, Y, Z]
env = get_openrave_env()
with env:
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(array([array(aabb)]), True)
super(Box, self).__init__(
box, pos=pos, rpy=rpy, pose=pose, color=color)
env.Add(box, True)
def setup_obj_rave(obj_cloud_xyz, obj_name):
rave_env = Globals.pr2.env
pc_down = voxel_downsample(obj_cloud_xyz, .02)
body = rave.RaveCreateKinBody(rave_env, '')
body.SetName(obj_name) #might want to change this name later
delaunay = scipy.spatial.Delaunay(pc_down)
body.InitFromTrimesh(rave.TriMesh(delaunay.points, delaunay.convex_hull),
True)
rave_env.Add(body)
return body
def create_box(T, color = [0, 0.6, 0]):
box = orpy.RaveCreateKinBody(env, '')
box.SetName('box')
box.InitFromBoxes(np.array([[0,0,0,0.035,0.03,0.005]]), True)
g = box.GetLinks()[0].GetGeometries()[0]
g.SetAmbientColor(color)
g.SetDiffuseColor(color)
box.SetTransform(T)
env.Add(box,True)
return box
def trimesh_cylinder(env, name, z_range, r):
trimesh_cylinder = rave.RaveCreateKinBody(env, '')
trimesh_cylinder.SetName(name)
check_range_z = z_range
check_range_r = r
circle_divide = 36
cylinder_trimesh_vertices = np.zeros((circle_divide * 2 + 2, 3))
cylinder_trimesh_indices = np.zeros((circle_divide * 4, 3))
for t in range(circle_divide * 2):
if t < circle_divide:
cz = check_range_z[0]
else:
cz = check_range_z[1]
cos_theta = math.cos(
(t % circle_divide) * (360.0 / circle_divide) * deg_to_rad)
sin_theta = math.sin(
(t % circle_divide) * (360.0 / circle_divide) * deg_to_rad)
cx = check_range_r * cos_theta
cy = check_range_r * sin_theta
cylinder_trimesh_vertices[t:t + 1, 0:3] = np.array((cx, cy, cz))
cylinder_trimesh_vertices[circle_divide * 2, 0:3] = np.array(
(0, 0, check_range_z[0]))
cylinder_trimesh_vertices[circle_divide * 2 + 1, 0:3] = np.array(
(0, 0, check_range_z[1]))
for t in range(circle_divide * 2):
if t < circle_divide:
center_index = circle_divide * 2
vertex1 = t % circle_divide
vertex2 = (t + 1) % circle_divide
vertex3 = t % circle_divide + circle_divide
cylinder_trimesh_indices[2 * t:2 * t + 1, 0:3] = np.array(
[center_index, vertex2, vertex1])
cylinder_trimesh_indices[2 * t + 1:2 * (t + 1), 0:3] = np.array(
[vertex1, vertex2, vertex3])
else:
center_index = circle_divide * 2 + 1
vertex1 = t % circle_divide + circle_divide
vertex2 = (t + 1) % circle_divide + circle_divide
vertex3 = (t + 1) % circle_divide
cylinder_trimesh_indices[2 * t:2 * t + 1, 0:3] = np.array(
[center_index, vertex1, vertex2])
cylinder_trimesh_indices[2 * t + 1:2 * (t + 1), 0:3] = np.array(
[vertex1, vertex3, vertex2])
trimesh_cylinder.InitFromTrimesh(
rave.TriMesh(cylinder_trimesh_vertices, cylinder_trimesh_indices),
False)
return trimesh_cylinder
def showGoal(env, T):
#TODO: Potential memory leak here?
with env:
dummy = _rave.RaveCreateKinBody(env, '')
dummy.SetName('dummy_1')
dummy.InitFromBoxes(_np([[0, 0, 0, 0.01, 0.03, 0.06]]), True)
env.Add(dummy, True)
dummy.SetTransform(_np.array(T))
_time.sleep(5)
env.Remove(dummy)
return 0
def make_box(center, half_extents, name):
box = openravepy.RaveCreateKinBody(env, '')
extents = [0, 0, 0, .1, .1, .1]
box.SetName(name)
box.InitFromBoxes(np.array([extents]), True)
trans = np.eye(4)
trans[:3, 3] = center
box.SetTransform(trans)
env.Add(box)
box.GetLinks()[0].SetMass(1)
return box
def planning_env(self):
env_obj = orpy.RaveCreateKinBody(self.env, '')
env_obj.SetName('over_hang_obstacles')
env_obj.InitFromBoxes(
np.array([[0.6, 0.3, 0.86, 0.1, 0.1, 0.1],
[0.6, -0.2, 0.86, 0.1, 0.1, 0.1],
[0.6, 0.3, 1.06, 0.1, 0.1, 0.1],
[0.6, -0.2, 1.06, 0.1, 0.1, 0.1],
[0.6, 0.3, 1.26, 0.1, 0.1, 0.1],
[0.6, -0.2, 1.26, 0.1, 0.1, 0.1],
[0.61, 0.05, 1.53, 0.11, 0.21, 0.17]]), True)
return env_obj
def PlaceCylinders(self, nObjects, heightMinMax, radiusMinMax, isSupportObjects, \
maxPlaceAttempts=10, workspace=((-0.18, 0.18), (-0.18, 0.18))):
'''Places a set of cylinders, with size drawn uniformly at random, on the table, in xy positions
drawn uniformly at random.
- Input nObjects: The number of objects to place.
- Input heightMinMax: Pair of floats specifying range of cylinder height.
- Input radiusMinMax: Pair of floats specifying range of cylinder radius.
- Input isSupportObjects: True if objects are ungraspable support objects.
- Input maxPlaceAttempts: Max reattempts (position samples) if the placement is in collision.
- Input workspace: xy placement extents: [(xMin, xMax), (yMin, yMax)].
'''
objectHandles = []
for i in xrange(nObjects):
# create object
height = uniform(heightMinMax[0], heightMinMax[1])
radius = uniform(radiusMinMax[0], radiusMinMax[1])
name = "support-" + str(i+len(self.supportObjects)) if isSupportObjects \
else "object-" + str(i+len(self.objects))
geomInfo = openravepy.KinBody.Link.GeometryInfo()
geomInfo._type = openravepy.KinBody.Link.GeomType.Cylinder
geomInfo._vGeomData = [radius, height]
geomInfo._vDiffuseColor = self.colors[randint(len(self.colors))]
body = openravepy.RaveCreateKinBody(self.env, "")
body.InitFromGeometries([geomInfo])
body.SetName(name)
body.height = height
body.radius = radius
self.env.Add(body, True)
objectHandles.append(body)
# position object
for j in xrange(maxPlaceAttempts):
xy = array([ \
uniform(workspace[0][0], workspace[0][1]),
uniform(workspace[1][0], workspace[1][1])])
T = eye(4)
T[0:2, 3] = xy
T[2, 3] = height / 2.0 + self.tableExtents[2] / 2.0
body.SetTransform(T)
if not self.env.CheckCollision(body): break
if isSupportObjects:
self.supportObjects += objectHandles
else:
self.objects += objectHandles
def create_spheres(env, points, radii = .01, name="spheres"):
points = np.asarray(points)
assert points.ndim == 2
if np.isscalar(radii):
radii = np.ones(len(points)) * radii
else:
assert len(radii) == len(points)
sphereinfo = np.empty((len(points), 4))
sphereinfo[:,:3] = points
sphereinfo[:,3] = radii
spheres = rave.RaveCreateKinBody(env, "")
spheres.InitFromSpheres(sphereinfo, True)
spheres.SetName(name)
env.Add(spheres)
return spheres
def create_boxes(env, points, radii = .01, name="boxes"):
points = np.asarray(points)
assert points.ndim == 2
radii2d = np.empty((len(points),3))
radii2d = radii
boxinfo = np.empty((len(points), 6))
boxinfo[:,:3] = points
boxinfo[:,3:6] = radii2d
boxes = rave.RaveCreateKinBody(env, "")
boxes.InitFromBoxes(boxinfo, True)
boxes.SetName(name)
env.Add(boxes)
return boxes
