def _is_phase_ready(self, phase, num_action_steps):
"""Check if the current phase is ready.
Args:
phase: A string variable.
num_action_steps: Number of steps in the `start` phase.
Returns:
The boolean value indicating if the current phase is ready.
"""
if self.is_simulation:
if phase == 'start':
if num_action_steps >= self.config.SIM.MAX_ACTION_STEPS:
logger.debug('The grasping motion is stuck.')
return True
if phase == 'start' or phase == 'end':
if self.simulator.check_contact(self.robot.arm, self.table):
logger.debug('The gripper contacts the table')
return True
if self.robot.is_limb_ready() and self.robot.is_gripper_ready():
return True
else:
return False
def __init__(self,
simulator=None,
config=None,
debug=True):
"""Initialize.
Args:
simulator: Instance of the simulator.
config: Environment configuration.
debug: True if it is debugging mode, False otherwise.
"""
self._simulator = simulator
self._config = config or self.default_config
self._debug = debug
# Camera.
self.camera = self._create_camera(
height=self.config.KINECT2.DEPTH.HEIGHT,
width=self.config.KINECT2.DEPTH.WIDTH,
intrinsics=self.config.KINECT2.DEPTH.INTRINSICS,
translation=self.config.KINECT2.DEPTH.TRANSLATION,
rotation=self.config.KINECT2.DEPTH.ROTATION)
# Graspable object.
if self.is_simulation:
self.graspable = None
self.graspable_path = None
self.graspable_pose = None
self.all_graspable_paths = []
self.graspable_index = 0
for pattern in self.config.SIM.GRASPABLE.PATHS:
if not os.path.isabs(pattern):
pattern = os.path.join(self.simulator.assets_dir, pattern)
if pattern[-4:] == '.txt':
with open(pattern, 'r') as f:
paths = [line.rstrip('\n') for line in f]
else:
paths = glob.glob(pattern)
print(pattern)
self.all_graspable_paths += paths
self.all_graspable_paths.sort()
num_graspable_paths = len(self.all_graspable_paths)
assert num_graspable_paths > 0, (
'Found no graspable objects at %s'
% (self.config.SIM.GRASPABLE.PATHS))
logger.debug('Found %d graspable objects.', num_graspable_paths)
super(Grasp4DofEnv, self).__init__(
simulator=self.simulator,
config=self.config,
debug=self.debug)
def execute_grasping_action(self, target_grasp_pose):
"""Execute the grasping action given a target grasp pose
Args:
target_grasp_pose (Pose)
"""
phase = 'initial'
if self.is_simulation:
num_action_steps = 0
pregrasp_pose = target_grasp_pose.copy()
pregrasp_pose.z += 0.1
while (phase != 'done'):
if self.is_simulation:
self.simulator.step()
if phase == 'pregrasp':
num_action_steps += 1
if self._is_grasping_phase_ready(phase, num_action_steps):
phase = self._get_grasping_next_phase(phase)
logger.debug(f'phase: {phase}')
self.robot.grip(0)
if phase == 'pregrasp':
self.robot.move_to_gripper_pose(pregrasp_pose)
elif phase == 'grasp':
self.robot.move_to_gripper_pose(target_grasp_pose, straight_line=True)
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=0.001,
spinning_friction=0.001)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=0.001,
spinning_friction=0.001)
self.table.set_dynamics(
lateral_friction=100)
elif phase == 'end':
self.robot.grip(1)
elif phase == 'postend':
self.robot.move_to_gripper_pose(pregrasp_pose, straight_line=True)
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.table.set_dynamics(
lateral_friction=1)
def _reset_scene(self):
"""Reset the scene in simulation or the real world.
"""
super(Grasp4DofEnv, self)._reset_scene()
# Reload graspable object.
if self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES:
if (self.num_episodes %
self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES == 0):
self.graspable_path = None
if self.graspable_path is None:
if self.config.SIM.GRASPABLE.USE_RANDOM_SAMPLE:
self.graspable_path = random.choice(self.all_graspable_paths)
else:
self.graspable_index = ((self.graspable_index + 1) %
len(self.all_graspable_paths))
self.graspable_path = (
self.all_graspable_paths[self.graspable_index])
pose = Pose.uniform(x=self.config.SIM.GRASPABLE.POSE.X,
y=self.config.SIM.GRASPABLE.POSE.Y,
z=self.config.SIM.GRASPABLE.POSE.Z,
roll=self.config.SIM.GRASPABLE.POSE.ROLL,
pitch=self.config.SIM.GRASPABLE.POSE.PITCH,
yaw=self.config.SIM.GRASPABLE.POSE.YAW)
pose = get_transform(source=self.table_pose).transform(pose)
scale = np.random.uniform(*self.config.SIM.GRASPABLE.SCALE)
logger.info('Loaded the graspable object from %s with scale %.2f...',
self.graspable_path, scale)
self.graspable = self.simulator.add_body(self.graspable_path,
pose,
scale=scale,
name=GRASPABLE_NAME)
logger.debug('Waiting for graspable objects to be stable...')
self.simulator.wait_until_stable(self.graspable)
# Reset camera.
self._reset_camera(
self.camera,
intrinsics=self.config.KINECT2.DEPTH.INTRINSICS,
translation=self.config.KINECT2.DEPTH.TRANSLATION,
rotation=self.config.KINECT2.DEPTH.ROTATION,
intrinsics_noise=self.config.KINECT2.DEPTH.INTRINSICS_NOISE,
translation_noise=self.config.KINECT2.DEPTH.TRANSLATION_NOISE,
rotation_noise=self.config.KINECT2.DEPTH.ROTATION_NOISE)
def sample(self, depth, camera, num_samples):
"""Samples a set of 2D grasps from a given RGB-D image.
Args:
depth: Depth image.
camera: The camera model.
num_samples: Number of grasps to sample.
Returns:
List of 2D grasp candidates
"""
# Sample an initial set of grasps (without depth).
logger.debug('Sampling grasp candidates...')
grasps = self._sample(depth, camera, num_samples)
logger.debug('Sampled %d grasp candidates from the image.' %
(len(grasps)))
return grasps
def get_reward(self):
"""Returns the reward value of the current step.
Returns:
success: The success signal.
terminate_after_grasp: The termination signal.
"""
if self.env.simulator:
self.env.simulator.wait_until_stable(self.graspable)
success = self.env.simulator.check_contact(self.end_effector,
self.graspable)
else:
raise NotImplementedError
self._update_history(success)
success_rate = np.mean(self.history or [-1])
logger.debug('Grasp Success: %r, Success Rate %.3f', success,
success_rate)
return success, self.terminate_after_grasp
def _check_joint_target_done(self):
"""Check if the joint target has been reached.
Returns:
True if the target has reached, False otherwise.
"""
# Check timeout.
if self._joint_target.stop_time is None:
return True
elif self.physics.time() >= self._joint_target.stop_time:
logger.debug('Time out for the position control.')
return True
# Check if target joints have been reached.
if self.check_joints_reached():
return True
# Check if joints are safe.
if self._max_reaction_forces is not None:
if not self.check_joints_safe():
return True
return False
def execute_moving_action(self, target_pose, straight_line=True):
"""Execute the gripper to move to a target psoe.
Args:
target_pose (Pose)
straight_line (bool, optional): interpolate linearly between start pose and target pose
"""
phase = 'initial'
if self.is_simulation:
num_action_steps = 0
while (phase != 'done'):
if self.is_simulation:
self.simulator.step()
if phase == 'start':
num_action_steps += 1
if self._is_moving_phase_ready(phase, num_action_steps):
phase = self._get_moving_next_phase(phase)
logger.debug(f'phase: {phase}')
if phase == 'start':
self.robot.move_to_gripper_pose(target_pose, straight_line=True)
def _execute_action(self, action):
"""Execute the grasp action.
Args:
action: A 4-DoF grasp defined in the image space or the 3D space.
"""
if self.config.ACTION.TYPE == 'CUBOID':
x, y, z, angle = action
elif self.config.ACTION.TYPE == 'IMAGE':
grasp = Grasp2D.from_vector(action, camera=self.camera)
x, y, z, angle = grasp.as_4dof()
else:
raise ValueError('Unrecognized action type: %r' %
(self.config.ACTION.TYPE))
start = Pose([[x, y, z + self.config.ARM.FINGER_TIP_OFFSET],
[0, np.pi, angle]])
phase = 'initial'
# Handle the simulation robustness.
if self.is_simulation:
num_action_steps = 0
while (phase != 'done'):
if self.is_simulation:
self.simulator.step()
if phase == 'start':
num_action_steps += 1
if self._is_phase_ready(phase, num_action_steps):
phase = self._get_next_phase(phase)
logger.debug('phase: %s', phase)
if phase == 'overhead':
self.robot.move_to_joint_positions(
self.config.ARM.OVERHEAD_POSITIONS)
# self.robot.grip(0)
elif phase == 'prestart':
prestart = start.copy()
prestart.z = self.config.ARM.GRIPPER_SAFE_HEIGHT
self.robot.move_to_gripper_pose(prestart)
elif phase == 'start':
self.robot.move_to_gripper_pose(start, straight_line=True)
# Prevent problems caused by unrealistic frictions.
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=0.001, spinning_friction=0.001)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=0.001, spinning_friction=0.001)
self.table.set_dynamics(lateral_friction=100)
elif phase == 'end':
self.robot.grip(1)
elif phase == 'postend':
postend = self.robot.end_effector.pose
postend.z = self.config.ARM.GRIPPER_SAFE_HEIGHT
self.robot.move_to_gripper_pose(postend,
straight_line=True)
# Prevent problems caused by unrealistic frictions.
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.table.set_dynamics(lateral_friction=1)
def execute_gentle_grasping_action(self, target_grasp_pose):
"""Execute a more gentle grasping action given a target grasp pose
Args:
target_grasp_pose (Pose)
"""
phase = 'initial'
if self.is_simulation:
num_action_steps = 0
pregrasp_pose = target_grasp_pose.copy()
pregrasp_pose.z += 0.1
while (phase != 'done'):
if self.is_simulation:
self.simulator.step()
if phase == 'pregrasp':
num_action_steps += 1
if self._is_grasping_phase_ready(phase, num_action_steps):
phase = self._get_grasping_next_phase(phase)
logger.debug(f'phase: {phase}')
self.robot.grip(0)
if phase == 'pregrasp':
self.robot.move_to_gripper_pose(pregrasp_pose)
elif phase == 'grasp':
self.robot.move_to_gripper_pose(target_grasp_pose, straight_line=True)
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=0.001,
spinning_friction=0.001)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=0.001,
spinning_friction=0.001)
self.table.set_dynamics(
lateral_friction=100)
elif phase == 'end':
value = 0.2
stop_l_finger = False
stop_r_finger = False
# This is an example of grasping gently
while value < 1.0 and not (stop_l_finger or stop_r_finger):
self.robot.grip(value)
while not self.robot.is_gripper_ready():
if self.simulator.check_contact(self.robot.l_finger_tip, self.graspable):
stop_l_finger = True
self.robot.stop_l_finger()
if self.simulator.check_contact(self.robot.r_finger_tip, self.graspable):
stop_r_finger = True
self.robot.stop_r_finger()
self.simulator.step()
if stop_l_finger and stop_r_finger:
while self.robot.is_gripper_ready():
self.simulator.step()
break
value += 0.1
elif phase == 'postend':
self.robot.move_to_gripper_pose(pregrasp_pose, straight_line=True)
if self.is_simulation:
self.robot.l_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.robot.r_finger_tip.set_dynamics(
lateral_friction=100,
rolling_friction=10,
spinning_friction=10)
self.table.set_dynamics(
lateral_friction=1)
def _reset_scene(self):
"""Reset the scene in simulation or the real world.
"""
# Configure debug visualizer
if self._debug and self.is_simulation:
visualizer_info = self.simulator.physics.get_debug_visualizer_info(['yaw', 'pitch', 'dist', 'target'])
# Adjust the camera view to your own need.
visualizer_info['dist'] = 1.5
visualizer_info['yaw'] = 50
visualizer_info['pitch'] = -20
self.simulator.physics.reset_debug_visualizer(camera_distance=visualizer_info['dist'],
camera_yaw=visualizer_info['yaw'],
camera_pitch=visualizer_info['pitch'],
camera_target_position=visualizer_info['target'])
super(FrankaPandaGraspEnv, self)._reset_scene()
# Reload graspable object.
if self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES:
if (self.num_episodes %
self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES == 0):
self.graspable_path = None
if self.graspable_path is None:
if self.config.SIM.GRASPABLE.USE_RANDOM_SAMPLE:
self.graspable_path = random.choice(
self.all_graspable_paths)
else:
self.graspable_index = ((self.graspable_index + 1) %
len(self.all_graspable_paths))
self.graspable_path = (
self.all_graspable_paths[self.graspable_index])
pose = Pose.uniform(x=self.config.SIM.GRASPABLE.POSE.X,
y=self.config.SIM.GRASPABLE.POSE.Y,
z=self.config.SIM.GRASPABLE.POSE.Z,
roll=self.config.SIM.GRASPABLE.POSE.ROLL,
pitch=self.config.SIM.GRASPABLE.POSE.PITCH,
yaw=self.config.SIM.GRASPABLE.POSE.YAW)
pose = get_transform(source=self.table_pose).transform(pose)
scale = np.random.uniform(*self.config.SIM.GRASPABLE.SCALE)
logger.info('Loaded the graspable object from %s with scale %.2f...',
self.graspable_path, scale)
self.graspable = self.simulator.add_body(self.graspable_path, pose, scale=scale, name=GRASPABLE_NAME, baseMass=0.1)
# self.graspable = self.simulator.add_body(self.graspable_path, pose, scale=scale * 0.01, name=GRASPABLE_NAME, collisionFrameOrientation=[-0.5, -0.5, 0.5, 0.5], visualFrameOrientation=[-0.5, -0.5, 0.5, 0.5], baseMass=0.1)
logger.debug('Waiting for graspable objects to be stable...')
# Change some properties of the object compensating for
# artifacts due to simulation
self.table.set_dynamics(contact_damping=100., contact_stiffness=100.)
self.graspable.set_dynamics(
lateral_friction=1.,
rolling_friction=0.001,
spinning_friction=0.001,
contact_damping=100.,
contact_stiffness=100.)
self.simulator.wait_until_stable(self.graspable)
# Reset camera.
self._reset_camera(
self.camera,
intrinsics=self.config.KINECT2.DEPTH.INTRINSICS,
translation=self.config.KINECT2.DEPTH.TRANSLATION,
rotation=self.config.KINECT2.DEPTH.ROTATION,
intrinsics_noise=self.config.KINECT2.DEPTH.INTRINSICS_NOISE,
translation_noise=self.config.KINECT2.DEPTH.TRANSLATION_NOISE,
rotation_noise=self.config.KINECT2.DEPTH.ROTATION_NOISE)