def as_4dof(self):
"""Computes the 4-DOF pose of the grasp in the world frame.
Returns:
The 4-DOF gripper pose in the world.
"""
angle = (self.angle + np.pi / 2)
grasp_pose_in_camera = Pose([self.pose.position, [0, 0, angle]])
grasp_pose_in_world = get_transform(source=self.camera.pose).transform(
grasp_pose_in_camera)
x, y, z = grasp_pose_in_world.position
angle = grasp_pose_in_world.euler[2]
return [x, y, z, angle]
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 move_along_gripper_path(self,
poses,
speed=None):
"""Move the arm to follow a path of the gripper.
Please refer to:
http://sdk.rethinkrobotics.com/intera/MoveIt_Tutorial
http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/scripts/doc/move_group_python_interface_tutorial.html#cartesian-paths
See the parent class.
"""
if speed is None:
speed = self.config.LIMB_MAX_VELOCITY_RATIO
if self._motion_planning == 'moveit':
# Plan with moveit.
waypoints = []
for pose in poses:
# Compute the 'right_gripper' pose from the 'right_hand' pose
# for moveit. This is a temporary fix.
pose_frame = get_transform(source=pose)
pose = pose_frame.transform([[0, 0, 0.0495], [0, 0, 0]])
waypoint = convert_pose_for_ros(pose)
waypoints.append(waypoint)
# Move to the starting pose.
rospy.loginfo('Moving to the starting pose...')
self._group.set_pose_target(waypoints[0])
plan = self._group.plan()
self._group.execute(plan)
# Move long the path.
rospy.loginfo('Planning the trajectory with moveit...')
plan, _ = self._group.compute_cartesian_path(
waypoints, self.config.END_EFFECTOR_STEP, 0.0)
rospy.loginfo('Moving along the planned path...')
self._group.set_max_velocity_scaling_factor(speed)
self._group.execute(plan)
else:
rospy.loginfo('Moving along the path without planning...')
for pose in poses:
self.move_to_gripper_pose(pose, speed=speed)
def load_calibration(self, path, robot_pose=[[0, 0, 0], [0, 0, 0]]):
"""Set the camera by using the camera calibration results.
Args:
path: The data directory of the calibration results.
"""
intrinsics_path = os.path.join(path, 'IR_intrinsics.npy')
intrinsics = np.load(intrinsics_path, encoding='latin1')
translation_path = os.path.join(path, 'robot_IR_translation.npy')
translation = np.load(translation_path, encoding='latin1')
rotation_path = os.path.join(path, 'robot_IR_rotation.npy')
rotation = np.load(rotation_path, encoding='latin1')
# Convert the extrinsics from the robot frame to the world frame.
from_robot_to_world = get_transform(source=robot_pose)
robot_pose_in_camera = Pose([translation, rotation])
camera_pose_in_robot = robot_pose_in_camera.inverse()
camera_pose_in_world = from_robot_to_world.transform(
camera_pose_in_robot)
world_origin_in_camera = camera_pose_in_world.inverse()
translation = world_origin_in_camera.position
rotation = world_origin_in_camera.matrix3
return intrinsics, translation, rotation
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)