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