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()
