def compute_grasp_action(self, obj, region, n_iter=1000): rightarm_torso_manip = self.robot.GetManipulator('rightarm_torso') for iter in range(n_iter): pick_base_pose = None print 'Sampling IR...' while pick_base_pose is None: with self.robot: pick_base_pose = sample_ir(obj, self.robot, self.env, region, n_iter=1) print 'Done!' theta, height_portion, depth_portion = sample_one_arm_grasp_parameters() grasp_params = np.array([theta[0], height_portion[0], depth_portion[0]]) with self.robot: set_robot_config(pick_base_pose, self.robot) grasps = compute_one_arm_grasp(depth_portion=grasp_params[2], height_portion=grasp_params[1], theta=grasp_params[0], obj=obj, robot=self.robot) for g in grasps: g_config = rightarm_torso_manip.FindIKSolution(g, 0) if g_config is not None: set_config(self.robot, g_config, self.robot.GetManipulator('rightarm_torso').GetArmIndices()) # Todo # I might be able to turn to disabling obstacles quickly if the collision, not the base pose, # is the problem if not self.env.CheckCollision(self.robot): #check_collision_except(obj, self.env): pick_params = {'operator_name': 'one_arm_pick', 'base_pose': pick_base_pose, 'grasp_params': grasp_params, 'g_config':g_config} return pick_params #one_arm_place_obj(obj, self.robot) print "Sampling one arm pick failed" pick_params = {'operator_name': 'one_arm_pick', 'base_pose': None, 'grasp_params': None, 'g_config': None} return pick_params
def check_feasibility(self, node, action): robot_xytheta = get_body_xytheta(self.robot).squeeze() new_q = robot_xytheta + action self.problem_env.disable_objects( ) # note that this class is only for mcr purpose set_robot_config(new_q, self.problem_env.robot) if self.collision_fn(new_q) or \ (not self.problem_env.regions['entire_region'].contains(self.robot.ComputeAABB())): action = { 'operator_name': 'next_base_pose', 'base_pose': None, 'action_parameters': action } status = "NoSolution" else: action = { 'operator_name': 'next_base_pose', 'base_pose': robot_xytheta, 'action_parameters': action } status = 'HasSolution' self.problem_env.enable_objects() set_robot_config(robot_xytheta, self.problem_env.robot) return action, status
def check_action_feasible(self,action): robot = self.robot env = self.env #curr_obj = self.objects[len(self.placements)] # fixed object order place_obj_pose = action[0,0:3] place_robot_pose = action[0,3:] with robot: status = '' set_robot_config( place_robot_pose,robot) inCollision = (check_collision_except(self.curr_obj,robot,env))\ or (check_collision_except(robot,self.curr_obj,env)) inRegion = (self.all_region.contains(robot.ComputeAABB())) and\ (self.loading_region.contains(self.curr_obj.ComputeAABB())) #if inCollision: #if not inRegion: print 'Out of collision' if (not inCollision) and inRegion: for node_lim in [1000,5000,np.inf]: path,tpath,status = get_motion_plan(robot,\ place_robot_pose,env,maxiter=10,n_node_lim=node_lim) if status=='HasSolution': break if status == "HasSolution": return True else: return False
def sample_pick_using_gen(obj, obj_shape, robot, generator, env, region): # diable all bodies; imagine being able to pick anywhere for body in env.GetBodies(): body.Enable(False) # enable the target and the robot obj.Enable(True) robot.Enable(True) original_trans = robot.GetTransform() n_trials = 100 # try 20 different samples for idx in range(n_trials): theta, height_portion, depth_portion, base_pose \ = generate_pick_grasp_and_base_pose(generator, obj_shape, get_point(obj)) set_robot_config(base_pose, robot) grasps = compute_two_arm_grasp(depth_portion, height_portion, theta, obj, robot) # tool trans g_config = solveTwoArmIKs(env, robot, obj, grasps) # turns obj collision off if g_config is None: continue for body in env.GetBodies(): if body.GetName().find('_pt_') != -1: continue body.Enable(True) return base_pose, [theta, height_portion, depth_portion], g_config return None, None, None
def fcn(obj_name, q_init): while True: # todo: disable all of objects print "Calling gengrasp" obj = pick_unif.problem_env.env.GetKinBody(obj_name) pick_unif.problem_env.reset_to_init_state_stripstream() pick_unif.problem_env.disable_objects_in_region('entire_region') obj.Enable(True) action = pick_unif.predict(obj, pick_unif.problem_env.regions['entire_region']) pick_base_pose = action['base_pose'] grasp = action['grasp_params'] g_config = action['g_config'] if g_config is None: pick_unif.problem_env.enable_objects_in_region('entire_region') yield None else: set_robot_config(q_init, pick_unif.problem_env.robot) plan, status = pick_unif.problem_env.get_base_motion_plan(pick_base_pose) pick_unif.problem_env.enable_objects_in_region('entire_region') pick_unif.problem_env.reset_to_init_state_stripstream() if status == 'NoPath': yield None else: print grasp, pick_base_pose, g_config yield [grasp, pick_base_pose, g_config, plan]
def sample_pick(obj, robot, env, region): # takes the target object to be picked and returns: # - base pose to pick the object # - grasp parameters to pick the object # - grasp to pick the object # to which there is a valid path n_trials = 5000 # try 20 different samples for _ in range(n_trials): base_pose = sample_ir(obj, robot, env, region) if base_pose is None: continue set_robot_config(base_pose, robot) theta, height_portion, depth_portion = sample_grasp_parameters() grasps = compute_two_arm_grasp(depth_portion, height_portion, theta, obj, robot) g_config = solveTwoArmIKs(env, robot, obj, grasps) if g_config is None: continue for body in env.GetBodies(): body.Enable(True) return base_pose, [theta, height_portion, depth_portion], g_config return None, None, None
def sample_placement_using_body_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): robot.SetTransform(original_trans) # sample obj pose inCollision = True np.random.shuffle(p_samples) for idx, xytheta in enumerate(p_samples): if idx > 100: break set_robot_config(xytheta, 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): print 'obj in collision' break # if you tried all p samples and ran out, get new pick print 'robot in a feasible place' # compute the resulting robot transform robot_xytheta = xytheta robot.SetTransform(original_trans) stime = time.time() print 'motion planning...' for node_lim in [1000, 5000, np.inf]: print node_lim 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 print 'done', status, time.time() - stime if status == "HasSolution": print 'returning with solution', tpath set_robot_config(xytheta, robot) return None, robot_xytheta, path else: print 'motion planning failed', tpath robot.SetTransform(original_trans) print "Returnining no solution" return None, None, None
def apply_place_action(self,action): robot = self.robot env = self.env leftarm_manip = robot.GetManipulator('leftarm') rightarm_manip = robot.GetManipulator('rightarm') if self.check_action_feasible(action): place_robot_pose = action[0,3:] set_robot_config(place_robot_pose,robot) place_obj( self.curr_obj,robot,FOLDED_LEFT_ARM,leftarm_manip,rightarm_manip) return True return False
def is_collision_at_base_pose(self, base_pose, obj=None): robot = self.robot env = self.env if obj is None: obj_holding = self.curr_obj else: obj_holding = obj with robot: set_robot_config(base_pose, robot) in_collision = check_collision_except(obj_holding, env) if in_collision: return True return False
def is_in_region_at_base_pose(self, base_pose, obj, robot_region, obj_region): robot = self.robot if obj is None: obj_holding = self.curr_obj else: obj_holding = obj with robot: set_robot_config(base_pose, robot) in_region = (robot_region.contains(robot.ComputeAABB())) and \ (obj_region.contains(obj_holding.ComputeAABB())) return in_region
def sample_placement(env, obj, robot, obj_region, robot_region): status = "Failed" path = None original_trans = robot.GetTransform() tried = [] n_trials = 1 # try 5 different samples of placements T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform()) for _ in range(n_trials): # at most n_trials number of path plans # print 'releasing obj' while True: sleep(GRAB_SLEEP_TIME) robot.Release(obj) robot.SetTransform(original_trans) # first sample collision-free object placement obj_xytheta = randomly_place_in_region(env, obj, obj_region) # randomly place obj # compute the resulting robot transform new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) robot.SetTransform(new_T_robot) robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = robot.GetActiveDOFValues() set_robot_config(robot_xytheta, robot) new_T = robot.GetTransform() assert (np.all(np.isclose(new_T, new_T_robot))) if not (check_collision_except(robot, obj, env)) \ and (robot_region.contains(robot.ComputeAABB())): break sleep(GRAB_SLEEP_TIME) grab_obj(robot, obj) robot.SetTransform(original_trans) # print 'motion planning...' stime = time.time() for node_lim in [1000, 5000, np.inf]: print node_lim path, tpath, status = get_motion_plan(robot, robot_xytheta, env, maxiter=10, n_node_lim=node_lim) print 'done', status, time.time() - stime if status == 'HasSolution': break print 'done', status, time.time() - stime if status == "HasSolution": # print 'returning with solution' return obj_xytheta, robot_xytheta, path # print 'grabbing obj' sleep(GRAB_SLEEP_TIME) robot.Grab(obj) # print 'grabbed' return None, None, None
def apply_two_arm_pick_action(self, action, obj): pick_base_pose, grasp_params = action set_robot_config(pick_base_pose, self.robot) grasps = compute_two_arm_grasp(depth_portion=grasp_params[2], height_portion=grasp_params[1], theta=grasp_params[0], obj=obj, robot=self.robot) g_config = solveTwoArmIKs(self.env, self.robot, obj, grasps) action = {} action['g_config'] = g_config action['base_pose'] = pick_base_pose two_arm_pick_object(obj, self.robot, action)
def fcn(obstacle_name, obstacle_pose, q_init, q_goal, traj): problem.reset_to_init_state_stripstream() obstacle = problem.env.GetKinBody(obstacle_name) set_obj_xytheta(obstacle_pose, obstacle) # check collision for p in traj: set_robot_config(p, problem.robot) if problem.env.CheckCollision(problem.robot): problem.reset_to_init_state_stripstream() return True problem.reset_to_init_state_stripstream() return False
def sample_ir_multiple_regions(obj, robot, env, multiple_regions): arm_len = 0.9844 # determined by spreading out the arm and measuring the dist from shoulder to ee # grasp_pos = Tgrasp[0:-1,3] obj_xy = get_point(obj)[:-1] robot_xy = get_point(robot)[:-1] dist_to_grasp = np.linalg.norm(robot_xy - obj_xy) n_samples = 1 for _ in range(300): robot_xy = sample_base_locations(arm_len, obj, env)[:-1] angle = sample_angle_facing_given_transform(obj_xy, robot_xy) # angle around z set_robot_config(np.r_[robot_xy, angle], robot) if (not env.CheckCollision(robot)) and np.any([r.contains(robot.ComputeAABB()) for r in multiple_regions]): return np.array([robot_xy[0], robot_xy[1], angle]) return None
def fcn(q_init, q_goal): while True: if np.all(q_init == q_goal): yield ([q_init],) curr_robot_config = get_body_xytheta(namo.robot) set_robot_config(q_init, namo.robot) namo.disable_objects_in_region('entire_region') plan, status = namo.get_base_motion_plan(q_goal) namo.enable_objects_in_region('entire_region') set_robot_config(curr_robot_config, namo.robot) if status == 'HasSolution': yield (plan,) else: import pdb;pdb.set_trace() yield None
def sample_ir(obj, robot, env, region, n_iter=300): arm_len = PR2_ARM_LENGTH # determined by spreading out the arm and measuring the dist from shoulder to ee # grasp_pos = Tgrasp[0:-1,3] obj_xy = get_point(obj)[:-1] robot_xy = get_point(robot)[:-1] dist_to_grasp = np.linalg.norm(robot_xy - obj_xy) n_samples = 1 for _ in range(n_iter): robot_xy = sample_xy_locations(obj, arm_len)[:-1] angle = sample_angle_facing_given_transform(obj_xy, robot_xy) # angle around z set_robot_config(np.r_[robot_xy, angle], robot) if (not env.CheckCollision(robot)) and (region.contains(robot.ComputeAABB())): return np.array([robot_xy[0], robot_xy[1], angle]) return None
def get_new_fetching_pick_path(self, namo_obj, motion_planning_region_name): # todo plan to the previous namo_obj pick configuration motion_to_namo_pick, status1 = self.problem_env.get_base_motion_plan( self.current_pick_conf, motion_planning_region_name) if status1 == 'NoPath': return None, 'NoPath' set_robot_config(self.current_pick_conf, self.robot) motion_from_namo_pick_to_fetch_pick, status2 = self.get_motion_plan_with_disabling( self.fetch_pick_conf, motion_planning_region_name, False, namo_obj) if status2 == 'NoPath': return None, 'NoPath' motion = motion_to_namo_pick + motion_from_namo_pick_to_fetch_pick status = "HasSolution" return motion, status
def process_plan(plan, namo): namo.env.SetViewer('qtcoin') namo.reset_to_init_state_stripstream() for step_idx, step in enumerate(plan): # todo finish this visualization script import pdb;pdb.set_trace() print "Executing operator ", step[0] if step[0] == 'pickup': obj_name = step[1][0] grasp = step[1][1] pick_base_pose = step[1][2] g_config = step[1][3] namo.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp, g_config), namo.env.GetKinBody(obj_name)) elif step[0] == 'movebase': q_init = step[1][0] q_goal = step[1][1] path = step[1][2] visualize_path(namo.robot, path) set_robot_config(q_goal, namo.robot) elif step[0] == 'movebase_with_object': q_init = step[1][4] q_goal = step[1][5] path = step[1][6] set_robot_config(q_init, namo.robot) visualize_path(namo.robot, path) set_robot_config(q_goal, namo.robot) else: place_obj_name = step[1][0] place_base_pose = step[1][4] path = step[1][-1] visualize_path(namo.robot, path) action = {'base_pose': place_base_pose} obj = namo.env.GetKinBody(place_obj_name) two_arm_place_object(obj, namo.robot, action)
def apply_pick_action(self): robot = self.robot env = self.env leftarm_manip = robot.GetManipulator('leftarm') rightarm_torso_manip = robot.GetManipulator('rightarm_torso') while True: pick_base_pose,grasp_params,g_config = sample_pick(self.curr_obj,robot,env,self.all_region) if pick_base_pose is None: continue set_robot_config( pick_base_pose,robot) pick_obj(self.curr_obj,robot,g_config,leftarm_manip,rightarm_torso_manip ) set_robot_config(self.init_base_conf,robot) pick = {} pick['pick_base_pose'] = pick_base_pose pick['grasp_params'] = grasp_params pick['g_config'] = g_config pick['obj'] = self.curr_obj.GetName() return pick
def fcn(holding_obj_name, grasp, pick_base_conf, g_config, placed_obj_name, placed_obj_pose, q_goal, holding_obj_path): holding_obj = problem.env.GetKinBody(holding_obj_name) placed_obj = problem.env.GetKinBody(placed_obj_name) problem.apply_two_arm_pick_action_stripstream((pick_base_conf, grasp, g_config), holding_obj) # how do I ensure that we are in the same state in both openrave and stripstream? if np.all(pick_base_conf == q_goal): return False if holding_obj_name != placed_obj_name: # set the obstacle in place set_obj_xytheta(placed_obj_pose, placed_obj) else: return False # this is already checked return False # check collision for p in holding_obj_path: set_robot_config(p, problem.robot) if problem.env.CheckCollision(problem.robot): problem.reset_to_init_state_stripstream() return True problem.reset_to_init_state_stripstream() return False
def fcn(holding_obj_name, grasp, pick_base_conf, placed_obj_name, placed_obj_pose, holding_obj_place_base_pose, holding_obj_place_traj): holding_obj = problem.env.GetKinBody(holding_obj_name) placed_obj = problem.env.GetKinBody(placed_obj_name) problem.apply_two_arm_pick_action_stripstream((pick_base_conf, grasp), holding_obj) if holding_obj_name != placed_obj_name: # set the obstacle in place set_obj_xytheta(placed_obj_pose, placed_obj) else: return False # this is already checked if len(problem.robot.GetGrabbed()) == 0: import pdb;pdb.set_trace() # check collision for p in holding_obj_place_traj: set_robot_config(p, problem.robot) if problem.env.CheckCollision(problem.robot): problem.reset_to_init_state_stripstream() return True problem.reset_to_init_state_stripstream() return False
def fcn(obj_name, grasp, pick_base_pose, g_config, region_name): # simulate pick while True: problem.reset_to_init_state_stripstream() obj = problem.env.GetKinBody(obj_name) problem.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp, g_config), obj) # how do I ensure that we are in the same state in both openrave and stripstream? print region_name problem.disable_objects_in_region('entire_region') obj.Enable(True) place_action = place_unif.predict(obj, problem.regions[region_name]) place_base_pose = place_action['base_pose'] object_pose = place_action['object_pose'].squeeze() path, status = problem.get_base_motion_plan(place_base_pose.squeeze()) problem.enable_objects_in_region('entire_region') print "Input", obj_name, grasp, pick_base_pose set_robot_config(place_base_pose, problem.robot) problem.reset_to_init_state_stripstream() if status == 'HasSolution': yield (place_base_pose, object_pose, path) else: yield None
def check_two_arm_place_feasibility(self, namo_obj, action, obj_placement_region): if action['base_pose'] is None: return None, "NoPath", self.curr_namo_object_names motion_planning_region_name = 'entire_region' goal_robot_xytheta = action['base_pose'] pick_base_pose = get_body_xytheta(self.problem_env.robot) pick_conf = self.problem_env.robot.GetDOFValues() namo_status = 'NoPath' namo_place_motion = None if self.problem_env.check_base_pose_feasible( goal_robot_xytheta, namo_obj, self.problem_env.regions[motion_planning_region_name]): namo_place_motion, namo_status = self.problem_env.get_base_motion_plan( goal_robot_xytheta, motion_planning_region_name) if namo_status == 'NoPath': return namo_place_motion, namo_status, self.curr_namo_object_names two_arm_place_object(namo_obj, self.problem_env.robot, action) fetch_pick_path = self.fetch_pick_path fetch_place_path = self.fetch_place_path new_collisions = self.get_obstacles_in_collision_from_fetching_path( fetch_pick_path, fetch_place_path) # go back to pre-place self.problem_env.robot.SetDOFValues(pick_conf) set_robot_config(action['base_pose'], self.robot) grab_obj(self.problem_env.robot, namo_obj) set_robot_config(pick_base_pose, self.robot) """ if namo_obj in new_collisions: print "Object moved still in collision" return None, "NoPath", self.curr_namo_object_names """ # if new collisions is more than or equal to the current collisions, don't bother executing it """ if len(self.curr_namo_object_names) <= len(new_collisions): print "There are more or equal number of collisions on the new path" print len(self.curr_namo_object_names), len(new_collisions) print namo_obj, new_collisions return None, "NoPath", self.curr_namo_object_names """ # otherwise, update the new namo objects self.prev_namo_object_names = self.curr_namo_object_names self.curr_namo_object_names = [obj.GetName() for obj in new_collisions] #if len(self.prev_namo_object_names) - len(self.curr_namo_object_names ) > 1: # import pdb;pdb.set_trace() # pick motion is the path to the fetching object, place motion is the path to place the namo object motion = { 'fetch_pick_path': fetch_pick_path, 'fetch_place_path': fetch_place_path, 'place_motion': namo_place_motion } # update the self.fetch_place_path if the packing region has moved self.fetch_pick_path = fetch_pick_path self.fetch_place_path = fetch_place_path # update the high level controller task plan namo_region = self.problem_env.get_region_containing(self.fetching_obj) self.high_level_controller.set_task_plan([{ 'region': namo_region, 'objects': new_collisions }]) return motion, "HasSolution", self.curr_namo_object_names
def main(): env=Environment() env.Load('env.xml') robot = env.GetRobots()[0] #set_default_robot_config(robot) robot_initial_pose = get_pose(robot) leftarm_manip = robot.GetManipulator('leftarm') robot.SetActiveManipulator('leftarm') ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \ iktype=IkParameterization.Type.Transform6D, \ forceikfast=True, freeindices=None, \ freejoints=None, manip=None) if not ikmodel1.load(): ikmodel1.autogenerate() rightarm_manip = robot.GetManipulator('rightarm') rightarm_torso_manip = robot.GetManipulator('rightarm_torso') robot.SetActiveManipulator('rightarm_torso') manip = robot.GetActiveManipulator() ee = manip.GetEndEffector() ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \ iktype=IkParameterization.Type.Transform6D, \ forceikfast=True, freeindices=None, \ freejoints=None, manip=None) if not ikmodel2.load(): ikmodel2.autogenerate() set_config(robot,FOLDED_LEFT_ARM,robot.GetManipulator('leftarm').GetArmIndices()) set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\ robot.GetManipulator('rightarm').GetArmIndices()) robot_initial_config = np.array([-1,1,0]) set_robot_config(robot_initial_config,robot) env.SetViewer('qtcoin') target = env.GetKinBody('target') width = 0.1 #np.random.rand(1)*(max_width-min_width)+min_width length = 0.8 #np.random.rand(1)*(max_length-min_length) + min_length height = 1.0 #np.random.rand(1)*(max_height-min_height)+min_height new_body = box_body(env,width,length,height,\ name='obst',\ color=(0, 5, 1)) trans = np.eye(4); trans[2,-1] = 0.075 #env.Add(new_body); #new_body.SetTransform(trans) all_region = region = AARegion('all_region',((-2.51,2.51),(-2.51,2.51)),\ z=0.0001,color=np.array((1,1,0,0.25))) activate_arms_torso_base(robot) # Try picking different objects pick_obj = lambda obj_name: pick(obj_name,env,robot,all_region) import pdb;pdb.set_trace() pbase,grasp,g_config = sample_pick(env.GetRobot('valkyrie'),robot,env,all_region) pick_obj(env.GetRobot('valkyrie'),robot,g_config,leftarm_manip,rightarm_manip) import pdb;pdb.set_trace() path,tpath,status = get_motion_plan(robot,robot_xytheta,env,maxiter=10,n_node_lim=5000) import pdb;pdb.set_trace() sample_pick(target,robot,env,all_region) import pdb;pdb.set_trace()
def compute_constraint_removal_motion(self, place_config, place_region_name): self.place_motion, status = self.problem_env.get_motion_plan( place_config, place_region_name) assert self.robot.GetGrabbed( ) != 0, 'CRP must be solved once the target obj is held' self.place_region = place_region_name self.place_config = place_config self.pick_full_conf = self.robot.GetDOFValues() if status == 'NoPath': self.problem_env.disable_objects_in_region(self.pick_region_name) held_obj = self.robot.GetGrabbed()[0] held_obj.Enable(True) self.place_motion, _ = self.problem_env.get_motion_plan( place_config, 'entire_region') self.problem_env.enable_objects_in_region(self.pick_region_name) # this might plan a path that does not collide with any else: return self.place_motion, 'HasSimpleSolution' if self.place_motion is None: set_robot_config(self.prepick_config, self.robot) self.reset_to_prepick() return None, "NoPath" objs_in_collision = self.make_constraint_removal_plan() pick_region = self.problem_env.regions[self.pick_region_name] task_plan = [{'region': pick_region, 'objects': objs_in_collision}] if len(objs_in_collision) == 0: return self.place_motion, 'HasSimpleSolution' uct_parameter = 0 widening_parameter = 0.5 # todo set the environment to the configuration before the pick node # set the robot to its init_base_pose self.problem_env.is_solving_namo = True self.problem_env.init_namo_object_names = [ o.GetName() for o in objs_in_collision ] self.problem_env.curr_namo_object_names = [ o.GetName() for o in objs_in_collision ] self.problem_env.namo_pick_path = self.pick_motion self.problem_env.namo_place_path = self.place_motion self.problem_env.namo_region = self.pick_region_name # resetting the robot to its pre-pick configuration try: self.problem_env.place_object(self.pick_config) except: import pdb pdb.set_trace() set_robot_config(self.prepick_config, self.robot) self.reset_to_prepick() two_arm_pick_pi = PickWithBaseUnif(self.problem_env.env, self.robot) two_arm_place_pi = PlaceUnif(self.problem_env.env, self.robot) sampling_strategy = Uniform(self.problem_env, two_arm_pick_pi, two_arm_place_pi) mcts = MCTS(widening_parameter, uct_parameter, sampling_strategy, self.problem_env, 'mover', task_plan) #if self.namo_target_obj.GetName() == 'rectangular_packing_box2': # import pdb;pdb.set_trace() search_time_to_reward, plan, optimal_score_achieved = mcts.search( n_iter=20) #if self.namo_target_obj.GetName() == 'rectangular_packing_box2': # import pdb;pdb.set_trace() self.problem_env.is_solving_namo = False self.problem_env.namo_object_names = [] self.problem_env.namo_pick_path = None self.problem_env.namo_place_path = None self.problem_env.namo_region = None # in either case, set the environment to the configuration just after the pick if plan is None: return None, 'NoPath' else: try: # todo how come it didn't add path_to_pick_target_obj and path_to_last_place? plan = self.add_picking_target_obj_to_plan(plan) plan = self.add_placing_target_obj_to_plan(plan) except: import pdb pdb.set_trace() return plan, 'HasSolution'
def pick_namo_target_obj(self): set_robot_config(self.pick_config, self.robot) self.robot.SetDOFValues(self.pick_full_conf) grab_obj(self.robot, self.namo_target_obj)
def get_new_fetching_place_path(self, namo_obj, motion_planning_region_name): # pick the object is_fetch_pick_one_arm = self.fetch_pick_op_instance[ 'operator'] == 'one_arm_pick' if is_fetch_pick_one_arm: one_arm_pick_object(self.fetching_obj, self.problem_env.robot, self.fetch_pick_op_instance['action']) else: two_arm_pick_object(self.fetching_obj, self.problem_env.robot, self.fetch_pick_op_instance['action']) # get pre_exit_motion -- motion prev to the exit pre_exit_motion = [] for pidx, p in enumerate(self.fetch_place_path): pre_exit_motion.append(p) if np.all(p == self.fetch_pick_path_exit): break with self.robot: # place it first one_arm_place_object(self.fetching_obj, self.robot, self.fetch_place_op_instance['action']) place_conf_list = self.get_rotations_around_z_wrt_gripper( self.fetching_obj, self.fetch_place_conf) one_arm_pick_object(self.fetching_obj, self.robot, self.fetch_place_op_instance['action']) # put it back to where it was one_arm_place_object(self.fetching_obj, self.robot, self.fetch_pick_op_instance['action']) one_arm_pick_object(self.fetching_obj, self.robot, self.fetch_pick_op_instance['action']) place_conf_list = [ conf for conf in place_conf_list if self.problem_env.check_base_pose_feasible( conf[-3:], self.fetching_obj, self.problem_env.regions[motion_planning_region_name]) ] if is_fetch_pick_one_arm: self.problem_env.set_arm_base_config(self.fetch_pick_path_exit) else: set_robot_config(self.fetch_pick_path_exit) if len(place_conf_list) == 0: print "Moved packin region, no feasible base pose" return None, "NoPath" for conf in place_conf_list: # prevent moving the namo_obj = self.packing_region_obj in this case, because you just moved it post_exit_motion, status = self.get_motion_plan_with_disabling( conf, motion_planning_region_name, is_fetch_pick_one_arm, exception_obj=namo_obj) if status == 'HasSolution': # update the fetch place config self.fetch_place_conf = conf self.fetch_place_op_instance['action']['base_pose'] = conf[ -3:] break if is_fetch_pick_one_arm: one_arm_place_object(self.fetching_obj, self.problem_env.robot, self.fetch_pick_op_instance['action']) else: two_arm_place_object(self.fetching_obj, self.problem_env.robot, self.fetch_pick_op_instance['action']) if status == "NoPath": return None, "NoPath" else: place_motion = pre_exit_motion + post_exit_motion return place_motion, status
def simulate_base_path(robot, path): for p in path: # set_config(robot, p, get_active_arm_indices(robot)) set_robot_config(p, robot) sleep(0.001)
def check_two_arm_place_feasibility(self, namo_obj, action, obj_placement_region): curr_region = self.problem_env.get_region_containing( self.problem_env.robot) if obj_placement_region.name.find('shelf') != -1: motion_planning_region_name = 'home_region' else: motion_planning_region_name = obj_placement_region.name print motion_planning_region_name goal_robot_xytheta = action['base_pose'] pick_base_pose = get_body_xytheta(self.problem_env.robot) pick_conf = self.problem_env.robot.GetDOFValues() current_collisions = self.curr_namo_object_names self.prev_namo_object_names = current_collisions namo_status = 'NoPath' namo_place_motion = None if self.problem_env.check_base_pose_feasible( goal_robot_xytheta, namo_obj, self.problem_env.regions[motion_planning_region_name]): namo_place_motion, namo_status = self.problem_env.get_base_motion_plan( goal_robot_xytheta, motion_planning_region_name) if namo_status == 'NoPath': return namo_place_motion, namo_status, self.curr_namo_object_names two_arm_place_object(namo_obj, self.problem_env.robot, action) if self.is_c_init_pre_contact: fetch_pick_path, fetching_pick_status = self.get_new_fetching_pick_path( namo_obj, motion_planning_region_name) if fetching_pick_status == "NoPath": return None, 'NoPath', self.curr_namo_object_names else: fetch_pick_path = self.fetch_pick_path packing_region_moved = self.packing_region_obj == namo_obj # note this can only happen in the pre_contact stage if packing_region_moved: self.update_target_namo_place_base_pose() fetch_place_path, fetching_place_status = self.get_new_fetching_place_path( namo_obj, motion_planning_region_name) if fetching_place_status == "NoPath": return None, 'NoPath', self.curr_namo_object_names else: fetch_place_path = self.fetch_place_path new_collisions = self.get_obstacles_in_collision_from_fetching_path( fetch_pick_path, fetch_place_path) new_collisions = [ tmp for tmp in new_collisions if self.problem_env.get_region_containing(tmp) == self.curr_namo_region ] import pdb pdb.set_trace() # go back to pre-place self.problem_env.robot.SetDOFValues(pick_conf) set_robot_config(action['base_pose'], self.robot) grab_obj(self.problem_env.robot, namo_obj) set_robot_config(pick_base_pose, self.robot) # if new collisions is more than or equal to the current collisions, don't bother executing it if len(current_collisions) <= len(new_collisions): return None, "NoPath", self.curr_namo_object_names # otherwise, update the new namo objects self.curr_namo_object_names = [obj.GetName() for obj in new_collisions] # pick motion is the path to the fetching object, place motion is the path to place the namo object motion = { 'fetch_pick_path': fetch_pick_path, 'fetch_place_path': fetch_place_path, 'place_motion': namo_place_motion } # update the self.fetch_place_path if the packing region has moved self.fetch_pick_path = fetch_pick_path self.fetch_place_path = fetch_place_path # todo: change the place entrance configuration too # update the high level controller task plan namo_region = self.problem_env.get_region_containing(self.fetching_obj) self.high_level_controller.set_task_plan([{ 'region': namo_region, 'objects': new_collisions }]) return motion, "HasSolution", self.curr_namo_object_names