def check_place_at_obj_pose_feasible(self, obj_region, obj_pose, swept_volume_to_avoid): obj = self.robot.GetGrabbed()[0] if type(obj_region) == str: obj_region = self.problem_env.regions[obj_region] T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform()) original_trans = self.robot.GetTransform() original_obj_trans = obj.GetTransform() target_robot_region1 = self.problem_env.regions['home_region'] target_robot_region2 = self.problem_env.regions['loading_region'] target_obj_region = obj_region # for fetching, you want to move it around robot_xytheta = self.compute_robot_base_pose_given_object_pose( obj, self.robot, obj_pose, T_r_wrt_o) set_robot_config(robot_xytheta, self.robot) is_feasible = self.is_collision_and_region_constraints_satisfied( target_robot_region1, target_robot_region2, target_obj_region) if not is_feasible: action = { 'operator_name': 'two_arm_place', 'q_goal': None, 'object_pose': None, 'action_parameters': obj_pose } self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) return action, 'NoSolution' else: release_obj() if swept_volume_to_avoid is not None: # release the object if not swept_volume_to_avoid.is_swept_volume_cleared(obj): self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(obj) action = { 'operator_name': 'two_arm_place', 'q_goal': None, 'object_pose': None, 'action_parameters': obj_pose } return action, 'NoSolution' self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(obj) action = { 'operator_name': 'two_arm_place', 'q_goal': robot_xytheta, 'object_pose': obj_pose, 'action_parameters': obj_pose } return action, 'HasSolution'
def compute_robot_base_pose_given_object_pose(obj, robot, obj_pose, T_r_wrt_o): original_robot_T = robot.GetTransform() release_obj() set_obj_xytheta(obj_pose, obj) 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() grab_obj(obj) robot.SetTransform(original_robot_T) return robot_xytheta
def place_object_and_robot_at_new_pose(self, obj, obj_pose, obj_region): T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform()) if len(self.robot.GetGrabbed()) > 0: release_obj() set_obj_xytheta(obj_pose, obj) new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) self.robot.SetTransform(new_T_robot) new_base_pose = get_body_xytheta(self.robot) set_robot_config(new_base_pose, self.robot) fold_arms() set_point(obj, np.hstack([obj_pose[0:2], obj_region.z + 0.001])) return new_base_pose
def get_placement(self, obj, target_obj_region, T_r_wrt_o): original_trans = self.robot.GetTransform() original_config = self.robot.GetDOFValues() self.robot.SetTransform(original_trans) self.robot.SetDOFValues(original_config) release_obj() with self.robot: # print target_obj_region obj_pose = randomly_place_in_region(self.env, obj, target_obj_region) # randomly place obj obj_pose = obj_pose.squeeze() # compute the resulting robot transform new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) self.robot.SetTransform(new_T_robot) self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = self.robot.GetActiveDOFValues() set_robot_config(robot_xytheta, self.robot) grab_obj(obj) return obj_pose, robot_xytheta
def check_feasibility(self, operator_skeleton, place_parameters, swept_volume_to_avoid=None): # Note: # this function checks if the target region contains the robot when we place object at place_parameters # and whether the robot will be in collision obj = self.robot.GetGrabbed()[0] obj_region = operator_skeleton.discrete_parameters['region'] if type(obj_region) == str: obj_region = self.problem_env.regions[obj_region] obj_pose = place_parameters T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform()) original_trans = self.robot.GetTransform() original_obj_trans = obj.GetTransform() target_robot_region1 = self.problem_env.regions['home_region'] target_robot_region2 = self.problem_env.regions['loading_region'] target_obj_region = obj_region # for fetching, you want to move it around robot_xytheta = self.compute_robot_base_pose_given_object_pose( obj, self.robot, obj_pose, T_r_wrt_o) set_robot_config(robot_xytheta, self.robot) # why do I disable objects in region? Most likely this is for minimum constraint computation #self.problem_env.disable_objects_in_region('entire_region') target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \ target_robot_region2.contains(self.robot.ComputeAABB()) is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \ (not target_region_contains) is_object_pose_infeasible = self.env.CheckCollision(obj) or \ (not target_obj_region.contains(obj.ComputeAABB())) #self.problem_env.enable_objects_in_region('entire_region') if is_base_pose_infeasible or is_object_pose_infeasible: action = { 'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters } self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) return action, 'NoSolution' else: release_obj(self.robot, obj) if swept_volume_to_avoid is not None: # release the object if not swept_volume_to_avoid.is_swept_volume_cleared(obj): self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = { 'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters } return action, 'NoSolution' """ for config in swept_volume_to_avoid: set_robot_config(config, self.robot) if self.env.CheckCollision(self.robot, obj): self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = {'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters} return action, 'NoSolution' """ self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = { 'operator_name': 'two_arm_place', 'base_pose': robot_xytheta, 'object_pose': obj_pose, 'action_parameters': place_parameters } return action, 'HasSolution'