def reset_agent(self, env): """ Reset robot initial pose. Sample initial pose, check validity, and land it. :param env: environment instance """ reset_success = False max_trials = 100 # cache pybullet state # TODO: p.saveState takes a few seconds, need to speed up state_id = p.saveState() for _ in range(max_trials): initial_pos, initial_orn = self.sample_initial_pose(env) reset_success = env.test_valid_position(env.robots[0], initial_pos, initial_orn) p.restoreState(state_id) if reset_success: break if not reset_success: logging.warning("WARNING: Failed to reset robot without collision") env.land(env.robots[0], initial_pos, initial_orn) p.removeState(state_id) for reward_function in self.reward_functions: reward_function.reset(self, env)
def reset_agent(self, env): """ Reset robot initial pose. Sample initial pose and target position, check validity, and land it. :param env: environment instance """ reset_success = False max_trials = 100 # cache pybullet state # TODO: p.saveState takes a few seconds, need to speed up state_id = p.saveState() for i in range(max_trials): initial_pos, initial_orn, target_pos = \ self.sample_initial_pose_and_target_pos(env) reset_success = env.test_valid_position( env.robots[0], initial_pos, initial_orn) and \ env.test_valid_position( env.robots[0], target_pos) p.restoreState(state_id) if reset_success: break if not reset_success: logging.warning("WARNING: Failed to reset robot without collision") p.removeState(state_id) self.target_pos = target_pos self.initial_pos = initial_pos super(PointNavRandomTask, self).reset_agent(env)
def __exit__(self, type, value, traceback): p.restoreState(stateId=self.state_id) p.removeState(stateUniqueId=self.state_id)
for i in range(numSteps2): p.stepSimulation() file = open("saveFile.txt", "w") dumpStateToFile(file) file.close() ################################# setupWorld() #both restore from file or from in-memory state should work p.restoreState(fileName="state.bullet") stateId = p.saveState() print("stateId=", stateId) p.removeState(stateId) stateId = p.saveState() print("stateId=", stateId) if verbose: p.setInternalSimFlags(1) p.stepSimulation() if verbose: p.setInternalSimFlags(0) print("contact points restored=") for q in p.getContactPoints(): print(q) for i in range(numSteps2): p.stepSimulation() file = open("restoreFile.txt", "w")
def open_one_obj(self, body_id, mode='random'): """ Attempt to open one object without collision :param body_id: body id of the object :param mode: opening mode (zero, max, or random) """ body_joint_pairs = [] for joint_id in range(p.getNumJoints(body_id)): # cache current physics state state_id = p.saveState() j_low, j_high = p.getJointInfo(body_id, joint_id)[8:10] j_type = p.getJointInfo(body_id, joint_id)[2] parent_idx = p.getJointInfo(body_id, joint_id)[-1] if j_type not in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]: p.removeState(state_id) continue # this is the continuous joint if j_low >= j_high: p.removeState(state_id) continue # this is the 2nd degree joint, ignore for now if parent_idx != 0: p.removeState(state_id) continue if mode == 'max': # try to set the joint to the maxr value until no collision # step_size is 5cm for prismatic joint and 5 degrees for revolute joint step_size = np.pi / 36.0 if j_type == p.JOINT_REVOLUTE else 0.05 for j_pos in np.arange(0.0, j_high + step_size, step=step_size): p.resetJointState( body_id, joint_id, j_high - j_pos) p.stepSimulation() has_collision = self.check_collision( body_a=body_id, link_a=joint_id) p.restoreState(state_id) if not has_collision: p.resetJointState(body_id, joint_id, j_high - j_pos) break elif mode == 'random': # try to set the joint to a random value until no collision reset_success = False # make 10 attemps for _ in range(10): j_pos = np.random.uniform(j_low, j_high) p.resetJointState( body_id, joint_id, j_pos) p.stepSimulation() has_collision = self.check_collision( body_a=body_id, link_a=joint_id) p.restoreState(state_id) if not has_collision: p.resetJointState(body_id, joint_id, j_pos) reset_success = True break # if none of the random values work, set it to 0.0 by default if not reset_success: p.resetJointState(body_id, joint_id, 0.0) elif mode == 'zero': p.resetJointState(body_id, joint_id, 0.0) else: assert False body_joint_pairs.append((body_id, joint_id)) # Remove cached state to avoid memory leak. p.removeState(state_id) return body_joint_pairs
for i in range(numSteps2): p.stepSimulation() file = open("saveFile.txt", "w") dumpStateToFile(file) file.close() ################################# setupWorld() #both restore from file or from in-memory state should work p.restoreState(fileName="state.bullet") stateId = p.saveState() print("stateId=", stateId) p.removeState(stateId) stateId = p.saveState() print("stateId=", stateId) if verbose: p.setInternalSimFlags(1) p.stepSimulation() if verbose: p.setInternalSimFlags(0) print("contact points restored=") for q in p.getContactPoints(): print(q) for i in range(numSteps2): p.stepSimulation() file = open("restoreFile.txt", "w")
def plan_arm_motion(self, arm_joint_positions): """ Attempt to reach arm arm_joint_positions and return arm trajectory If failed, reset the arm to its original pose and return None :param arm_joint_positions: final arm joint position to reach :return: arm trajectory or None if no plan can be found """ disabled_collisions = {} if self.robot_type == 'Fetch': disabled_collisions = { (link_from_name(self.robot_id, 'torso_lift_link'), link_from_name(self.robot_id, 'torso_fixed_link')), (link_from_name(self.robot_id, 'torso_lift_link'), link_from_name(self.robot_id, 'shoulder_lift_link')), (link_from_name(self.robot_id, 'torso_lift_link'), link_from_name(self.robot_id, 'upperarm_roll_link')), (link_from_name(self.robot_id, 'torso_lift_link'), link_from_name(self.robot_id, 'forearm_roll_link')), (link_from_name(self.robot_id, 'torso_lift_link'), link_from_name(self.robot_id, 'elbow_flex_link')) } elif self.robot_type == 'Movo': disabled_collisions = { (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_shoulder_link')), (link_from_name(self.robot_id, 'right_base_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_arm_half_1_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_arm_half_2_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_forearm_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_wrist_spherical_1_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_wrist_spherical_2_link')), (link_from_name(self.robot_id, 'linear_actuator_link'), link_from_name(self.robot_id, 'right_wrist_3_link')), (link_from_name(self.robot_id, 'right_wrist_spherical_2_link'), link_from_name(self.robot_id, 'right_robotiq_coupler_link')), (link_from_name(self.robot_id, 'right_shoulder_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'left_base_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'left_shoulder_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'left_arm_half_2_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'right_arm_half_2_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'right_arm_half_1_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), (link_from_name(self.robot_id, 'left_arm_half_1_link'), link_from_name(self.robot_id, 'linear_actuator_fixed_link')), } if self.fine_motion_plan: self_collisions = True mp_obstacles = self.mp_obstacles else: self_collisions = False mp_obstacles = [] plan_arm_start = time() p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False) state_id = p.saveState() allow_collision_links = [] if self.robot_type == 'Fetch': allow_collision_links = [19] elif self.robot_type == 'Movo': allow_collision_links = [23, 24] arm_path = plan_joint_motion( self.robot_id, self.arm_joint_ids, arm_joint_positions, disabled_collisions=disabled_collisions, self_collisions=self_collisions, obstacles=mp_obstacles, algorithm=self.arm_mp_algo, allow_collision_links=allow_collision_links, ) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True) p.restoreState(state_id) p.removeState(state_id) return arm_path
def get_arm_joint_positions(self, arm_ik_goal): """ Attempt to find arm_joint_positions that satisfies arm_subgoal If failed, return None :param arm_ik_goal: [x, y, z] in the world frame :return: arm joint positions """ ik_start = time() max_limits, min_limits, rest_position, joint_range, joint_damping = \ self.get_ik_parameters() n_attempt = 0 max_attempt = 75 sample_fn = get_sample_fn(self.robot_id, self.arm_joint_ids) base_pose = get_base_values(self.robot_id) state_id = p.saveState() #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False) # find collision-free IK solution for arm_subgoal while n_attempt < max_attempt: if self.robot_type == 'Movo': self.robot.tuck() set_joint_positions(self.robot_id, self.arm_joint_ids, sample_fn()) arm_joint_positions = p.calculateInverseKinematics( self.robot_id, self.robot.end_effector_part_index(), targetPosition=arm_ik_goal, # targetOrientation=self.robots[0].get_orientation(), lowerLimits=min_limits, upperLimits=max_limits, jointRanges=joint_range, restPoses=rest_position, jointDamping=joint_damping, solver=p.IK_DLS, maxNumIterations=100) if self.robot_type == 'Fetch': arm_joint_positions = arm_joint_positions[2:10] elif self.robot_type == 'Movo': arm_joint_positions = arm_joint_positions[:8] set_joint_positions(self.robot_id, self.arm_joint_ids, arm_joint_positions) dist = l2_distance(self.robot.get_end_effector_position(), arm_ik_goal) # print('dist', dist) if dist > self.arm_ik_threshold: n_attempt += 1 continue # need to simulator_step to get the latest collision self.simulator_step() # simulator_step will slightly move the robot base and the objects set_base_values_with_z(self.robot_id, base_pose, z=self.initial_height) # self.reset_object_states() # TODO: have a princpled way for stashing and resetting object states # arm should not have any collision if self.robot_type == 'Movo': collision_free = is_collision_free( body_a=self.robot_id, link_a_list=self.arm_joint_ids_all) # ignore linear link else: collision_free = is_collision_free( body_a=self.robot_id, link_a_list=self.arm_joint_ids) if not collision_free: n_attempt += 1 # print('arm has collision') continue # gripper should not have any self-collision collision_free = is_collision_free( body_a=self.robot_id, link_a_list=[self.robot.end_effector_part_index()], body_b=self.robot_id) if not collision_free: n_attempt += 1 print('gripper has collision') continue #self.episode_metrics['arm_ik_time'] += time() - ik_start #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True) p.restoreState(state_id) p.removeState(state_id) return arm_joint_positions #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True) p.restoreState(state_id) p.removeState(state_id) #self.episode_metrics['arm_ik_time'] += time() - ik_start return None
def save(self): if self.last_backup_id is not None: p.removeState(self.last_backup_id) self.last_backup_id = p.saveState()