def initiate_crp_problem(self, target_obj, action, region_name): self.pick_region_name = region_name self.pick_config, self.pick_grasp = action self.prepick_config = get_body_xytheta(self.robot) self.prepick_state_saver = DynamicEnvironmentStateSaver( self.problem_env.env) self.namo_target_obj = target_obj
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 update_target_namo_place_base_pose(self): T_robot = self.get_world_robot_transform_given_obj_transform( self.packing_region_obj) with self.robot: self.robot.SetTransform(T_robot) fetch_place_base_pose = get_body_xytheta(self.robot) self.fetch_place_conf[-3:] = fetch_place_base_pose self.fetch_place_op_instance['action'][ 'base_pose'] = fetch_place_base_pose
def get_rotations_around_z_wrt_gripper(self, object, conf): # todo place the object at the designated conf self.problem_env.set_arm_base_config(conf) T_robot_obj = self.get_robot_transform_wrt_obj(object) conf_list = [] for rotation in [0, np.pi / 2, np.pi, 3 * np.pi / 2]: xytheta = get_body_xytheta(object) xytheta[0, -1] = xytheta[0, -1] + rotation set_obj_xytheta(xytheta, object) T_obj_new = object.GetTransform() T_robot_new = np.dot(T_obj_new, T_robot_obj) self.robot.SetTransform(T_robot_new) new_conf = copy.deepcopy(conf) new_conf[-3:] = get_body_xytheta(self.robot).squeeze() conf_list.append(new_conf) return conf_list
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 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 namo_domain_initialize_namo_problem(self, fetch_plan, fetch_goal_node): self.fetching_obj = self.env.GetKinBody(fetch_plan[0]['obj_name']) self.fetch_pick_op_instance = fetch_plan[0] #self.fetch_place_op_instance = fetch_plan[1] self.fetch_pick_path = fetch_plan[0]['path'] #self.fetch_place_path = fetch_plan[1]['path'] self.fetch_pick_conf = self.problem_env.make_config_from_op_instance( self.fetch_pick_op_instance) #self.fetch_place_conf = self.problem_env.make_config_from_op_instance(self.fetch_place_op_instance) self.problem_env.high_level_planner = self.high_level_controller self.fetch_goal_node = fetch_goal_node self.fetch_goal_node.state_saver = DynamicEnvironmentStateSaver( self.env) # this assumes that we're in prefetch state self.prefetching_robot_config = get_body_xytheta( self.problem_env.robot).squeeze()
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 initialize_namo_problem(self, fetch_plan, target_obj, fetch_goal_node, fetch_pick_path_exit, fetch_place_path_entrance, target_packing_region_name): self.packing_region_obj = None self.is_movable_packing_region = target_packing_region_name.find( 'packing_box') != -1 self.fetching_obj = target_obj self.fetch_pick_op_instance = fetch_plan[0] self.fetch_place_op_instance = fetch_plan[1] self.fetch_pick_path = fetch_plan[0]['path'] self.fetch_place_path = fetch_plan[1]['path'] self.fetch_pick_path_exit = fetch_pick_path_exit self.fetch_place_path_entrance = fetch_place_path_entrance self.fetch_pick_conf = self.problem_env.make_config_from_op_instance( self.fetch_pick_op_instance) self.fetch_place_conf = self.problem_env.make_config_from_op_instance( self.fetch_place_op_instance) self.problem_env.high_level_planner = self.high_level_controller if self.is_movable_packing_region: # getting relative robot base config wrt the packing region object with self.robot: self.set_arm_base_config(self.fetch_place_conf) self.packing_region_obj = self.problem_env.env.GetKinBody( target_packing_region_name) self.T_robot_obj = self.get_robot_transform_wrt_obj( self.packing_region_obj) self.target_namo_place_base_pose = self.fetch_place_conf self.fetch_goal_node = fetch_goal_node self.prefetching_robot_config = get_body_xytheta( self.problem_env.robot).squeeze()
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
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