def draw_configs(configs, env, name='point', colors=None, transparency=0.1): # assert configs[0].shape==(6,), 'Config shape must be (6,)' if colors is None: for i in range(len(configs)): config = configs[i] new_body = box_body(env, 0.1, 0.05, 0.05, \ name=name + '%d' % i, \ color=(1, 0, 0), \ transparency=transparency) env.Add(new_body); set_point(new_body, np.append(config[0:2], 0.075)) new_body.Enable(False) th = config[2] set_quat(new_body, quat_from_z_rot(th)) else: for i in range(len(configs)): config = configs[i] if isinstance(colors, tuple): color = colors else: color = colors[i] new_body = box_body(env, 0.1, 0.05, 0.05, \ name=name + '%d' % i, \ color=color, \ transparency=transparency) """ new_body = load_body(env,'mug.xml') set_name(new_body, name+'%d'%i) set_transparency(new_body, transparency) """ env.Add(new_body); set_point(new_body, np.append(config[0:2], 0.075)) new_body.Enable(False) th = config[2] set_quat(new_body, quat_from_z_rot(th))
def randomly_place_in_region_need_to_be_fixed(env, obj, region, th=None): # todo fix this function min_x = region.box[0][0] max_x = region.box[0][1] min_y = region.box[1][0] max_y = region.box[1][1] aabb = aabb_from_body(obj) # try 1000 placements for _ in range(300): x = np.random.rand(1) * (max_x - min_x) + min_x y = np.random.rand(1) * (max_y - min_y) + min_y z = [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET] xyz = np.array([x[0],y[0],z[0]]) - aabb.pos() + get_point(obj) # todo: recheck this function; I think it failed if obj was robot if th == None: th = np.random.rand(1) * 2 * np.pi set_point(obj, xyz) set_quat(obj, quat_from_angle_vector(th, np.array([0, 0, 1]))) obj_quat = get_quat(obj) # refer to conversion between quaternions and euler angles on Wiki for the following eqn. assert (np.isclose(th, np.arccos(obj_quat[0]) * 2) or np.isclose(th, np.arccos(-obj_quat[0]) * 2)) # print not env.CheckCollision(obj) and region.contains(obj.ComputeAABB()) if not env.CheckCollision(obj) \ and region.contains(obj.ComputeAABB()): return [x[0], y[0], th[0]] return None
def randomly_place_region(body, region, n_limit=None): env = openravepy.RaveGetEnvironments()[0] if env.GetKinBody(get_name(body)) is None: env.Add(body) orig = get_body_xytheta(body) # for _ in n_limit: i = 0 while True: set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point( body, np.array([uniform(*range) for range in cspace] + [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return if n_limit is not None: i += 1 if i >= n_limit: set_obj_xytheta(orig, body) return
def randomly_place_region(env, body, region): if env.GetKinBody(get_name(body)) is None: env.Add(body) while True: set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*range) for range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') set_default_robot_config(env.GetRobots()[0]) set_point(env.GetRobots()[0], (-1.5, 0, 0)) m, n = 3, 3 #m, n = 5, 5 n_obj = 8 side_dim = .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) pose_indices = list(product(range(m), range(n))) colors = {} for r, c in pose_indices: color = np.zeros(4) color[2-r] = 1. colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0]) poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r, c in pose_indices: x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) initial_indices = randomize(pose_indices[:]) initial_poses = {} goal_poses = {} for i, indices in enumerate(pose_indices[:n_obj]): name = 'block%d-%d'%indices color = colors[indices] initial_poses[name] = poses[initial_indices[i]] goal_poses[name] = poses[indices] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, initial_poses[name].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=poses.values())
def randomly_place_in_region(env, body, region): if env.GetKinBody(get_name(body)) is None: env.Add(body) for i in range(1000): set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not env.CheckCollision(body): return get_body_xytheta(body) return None
def load_objects(env, obj_shapes, obj_poses, color): objects = [] i = 0 nobj = len(obj_shapes.keys()) for obj_name in obj_shapes.keys(): xytheta = obj_poses[obj_name].squeeze() width, length, height = obj_shapes[obj_name] quat = quat_from_z_rot(xytheta[-1]) new_body = box_body(env, width, length, height, \ name=obj_name, \ color=np.array(color) / float(nobj - i)) i += 1 env.Add(new_body) set_point(new_body, [xytheta[0], xytheta[1], 0.075]) set_quat(new_body, quat) objects.append(new_body) return objects
def load_objects(env, obj_shapes, obj_poses, color): # sets up the object at their locations in the original env OBJECTS = [] i = 0 nobj = len(obj_shapes.keys()) for obj_name in obj_shapes.keys(): obj_xyz = obj_poses[obj_name]['obj_xyz'] obj_rot = obj_poses[obj_name]['obj_rot'] width, length, height = obj_shapes[obj_name] new_body = box_body(env, width, length, height, name=obj_name, color=np.array(color) / float(nobj - i)) i += 1 env.Add(new_body) set_point(new_body, obj_xyz) set_quat(new_body, obj_rot) OBJECTS.append(new_body) return OBJECTS
def grid_arrangement(env, m, n): # (Dealing with Difficult Instances of Object Rearrangment) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') box_dims = (.12, .04, .08) #separation = (.08, .08) separation = (.12, .12) #separation = (.16, .16) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) #set_point(table, (1.75, 0, 0)) set_point(table, (0, 0, 0)) env.Add(table) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) objects = [] goal_poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for i in range(m): x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0]) row_color = np.zeros(4) row_color[2-i] = 1. for j in range(n): y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1]) name = 'block%d-%d'%(i, j) color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0]) goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) objects.append(box_body(env, *box_dims, name=name, color=color)) object_names = [get_name(body) for body in objects] for obj in randomize(objects): randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(None, object_names=object_names, table_names=[get_name(table)], goal_poses=goal_poses)
def gaussian_randomly_place_in_region(env, body, region, center, var): if env.GetKinBody(get_name(body)) is None: env.Add(body) for i in range(1000): xytheta = np.random.normal(center, var) set_obj_xytheta(xytheta, body) if not body_collision(env, body): return xytheta import pdb;pdb.set_trace() for i in range(1000): set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return get_body_xytheta(body) return None
def shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment) env.Load(ENVIRONMENTS_DIR + 'empty.xml') #m, n = 2, 10 m, n = 2, 4 box_dims = (.07, .07, .2) #separation = (.08, .08) separation = (.15, .15) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (1.75, 0, 0)) env.Add(table) # TODO - place walls and/or a roof to make more similar to pebble graph people objects = [] goal_poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for i in range(m): x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0]) row_color = np.zeros(4) row_color[2-i] = 1. for j in range(n): y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1]) name = 'block%d-%d'%(i, j) color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0]) goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) objects.append(box_body(env, *box_dims, name=name, color=color)) object_names = [get_name(body) for body in objects] for obj in randomize(objects): randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(None, object_names=object_names, table_names=[get_name(table)], goal_poses=goal_poses)
def srivastava_table(env): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae') camera_trans = camera_look_at((1., -3., 4), look_point=(2, 1, 0)) body_names = [ str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() ] dx = .5 for body_name in body_names: body = env.GetKinBody(body_name) point = get_point(body) point[0] += dx set_point(body, point) table_names = [ body_name for body_name in body_names if 'table' in body_name ] object_names = [ body_name for body_name in body_names if body_name not in table_names ] for object_name in object_names: body = env.GetKinBody(object_name) point = get_point(body) point[2] += BODY_PLACEMENT_Z_OFFSET set_point(body, point) goal_holding = 'object1' goal_config = 'initial' # None return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding)
def srivastava_table(env, n=INF): env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae') set_default_robot_config(env.GetRobots()[0]) body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()] table_names = [body_name for body_name in body_names if 'table' in body_name] dx = .5 for body_name in body_names: body = env.GetKinBody(body_name) set_point(body, get_point(body) + np.array([dx, 0, 0])) objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names] for obj in objects: env.Remove(obj) object_names = [] for obj in take(objects, n): randomly_place_body(env, obj, table_names) object_names.append(get_name(obj)) goal_holding = 'object1' goal_config = 'initial' # None return ManipulationProblem(None, object_names=object_names, table_names=table_names, goal_config=goal_config, goal_holding=goal_holding)
def move_several(env, n): assert not REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') box_dims = (.07, .07, .2) #separation = (.08, .08) separation = (.10, .10) length = math.sqrt(n+1)*(box_dims[0] + separation[0]) width = math.sqrt(n+1)*(box_dims[1] + separation[1]) height = .7 table1 = box_body(env, length, width, height, name='table1', color=get_color('tan1')) set_point(table1, (0, 0, 0)) env.Add(table1) table2 = box_body(env, length, width, height, name='table2', color=get_color('tan1')) set_point(table2, (1.5, 0, 0)) env.Add(table2) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) # TODO - place walls and/or a roof to make more similar to pebble graph people objects = [] goal_regions = {} obj = box_body(env, .07, .07, .2, name='blue', color=BLUE) set_point(obj, (0, 0, height + BODY_PLACEMENT_Z_OFFSET)) objects.append(obj) goal_regions[get_name(obj)] = get_name(table2) env.Add(obj) for i in range(n): objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED)) for obj in randomize(objects[1:]): randomly_place_body(env, obj, [get_name(table1)]) return ManipulationProblem(None, object_names=[get_name(body) for body in objects], table_names=[get_name(table) for table in [table1, table2]], goal_regions=goal_regions)
def sample_placement_using_gen(env, obj, robot, p_samples, obj_region, robot_region, GRAB_SLEEP_TIME=0.05): # This script, with a given grasp of an object, # - finding colfree obj placement within 100 tries. no robot at this point # - checking colfree ik solution for robot; if not, trial is over # - if above two passes, colfree path finding; if not, trial is over status = "Failed" path = None original_trans = robot.GetTransform() obj_orig_trans = obj.GetTransform() tried = [] n_trials = 1 # try 5 different samples of placements with a given grasp T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform()) for _ in range(n_trials): #print 'releasing obj' sleep(GRAB_SLEEP_TIME) robot.Release(obj) robot.SetTransform(original_trans) obj.SetTransform(obj_orig_trans) #print 'released' # get robot pose wrt obj # sample obj pose #print 'randmly placing obj' #obj_xytheta = randomly_place_in_region(env,obj,obj_region) # randomly place obj inCollision = True np.random.shuffle(p_samples) for idx, obj_xytheta in enumerate(p_samples): if idx > 100: break x = obj_xytheta[0] y = obj_xytheta[1] z = get_point(obj)[-1] set_point(obj, [x, y, z]) th = obj_xytheta[2] set_quat(obj, quat_from_z_rot(th)) new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) robot.SetTransform(new_T_robot) inCollision = (check_collision_except(obj,robot,env))\ or (check_collision_except(robot,obj,env)) inRegion = (robot_region.contains(robot.ComputeAABB())) and\ (obj_region.contains(obj.ComputeAABB())) if (not inCollision) and inRegion: break if inCollision or not (inRegion): break # if you tried all p samples and ran out, get new pick # compute the resulting robot transform sleep(GRAB_SLEEP_TIME) robot.Grab(obj) robot.SetTransform(new_T_robot) robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = robot.GetActiveDOFValues() robot.SetTransform(original_trans) stime = time.time() for node_lim in [1000, 5000, np.inf]: path,tpath,status = get_motion_plan(robot,\ robot_xytheta,env,maxiter=10,n_node_lim=node_lim) if path == 'collision': # import pdb;pdb.set_trace() pass if status == "HasSolution": robot.SetTransform(new_T_robot) return obj_xytheta, robot_xytheta, path else: print('motion planning failed', tpath) sleep(GRAB_SLEEP_TIME) robot.Grab(obj) robot.SetTransform(original_trans) print("Returnining no solution") return None, None, None
def set_obj_xyztheta(xyztheta, obj): set_point(obj, xyztheta[0:-1]) set_obj_xytheta(np.hstack([xyztheta[0:2], xyztheta[-1]]), obj)
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') m, n = 3, 3 #m, n = 5, 5 side_dim = .07 # .05 | .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) #separation = (side_dim/2, side_dim/2) coordinates = list(product(range(m), range(n))) assert n_obj <= len(coordinates) obj_coordinates = sample(coordinates, n_obj) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) #set_base_values(robot, (0, width/2 + .5, math.pi)) #set_base_values(robot, (.35, width/2 + .35, math.pi)) #set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4)) poses = [] z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r in range(m): row = [] x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) for c in range(n): y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))) poses.append(row) initial_poses = {} goal_poses = {} # TODO - randomly assign goal poses here for i, (r, c) in enumerate(obj_coordinates): row_color = np.zeros(4) row_color[2-r] = 1. if i == 0: name = 'goal%d-%d'%(r, c) color = BLUE goal_poses[name] = poses[m/2][n/2] else: name = 'block%d-%d'%(r, c) color = RED initial_poses[name] = poses[r][c] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, poses[r][c].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) known_poses = list(flatten(poses)) #known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses)