def setParams(self, params):
SCENE_FILE = params["scene_dir"]
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.cameras = {}
cam = VisionSensor("Camera_Shoulder")
self.cameras["Camera_Shoulder"] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"depth":
3,
"focal":
cam.get_resolution()[0] /
np.tan(np.radians(cam.get_perspective_angle())),
"position":
cam.get_position(),
"rotation":
cam.get_quaternion(),
"image_rgb":
np.array(0),
"image_rgbd":
np.array(0),
"depth":
np.ndarray(0)
}
self.grasping_objects = {}
can = Shape("can")
self.grasping_objects["002_master_chef_can"] = {
"handle": can,
"sim_pose": None,
"pred_pose_rgb": None,
"pred_pose_rgbd": None
}
with (open("objects_pcl.pickle", "rb")) as file:
self.object_pcl = pickle.load(file)
self.intrinsics = np.array(
[[
self.cameras["Camera_Shoulder"]["focal"], 0.00000000e+00,
self.cameras["Camera_Shoulder"]["width"] / 2.0
],
[
0.00000000e+00, self.cameras["Camera_Shoulder"]["focal"],
self.cameras["Camera_Shoulder"]["height"] / 2.0
], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.arm_ops = {
"MoveToHome": 1,
"MoveToObj": 2,
"CloseGripper": 3,
"OpenGripper": 4
}
self.grasping_iter = 10
self.arm_base = Shape("gen3")
self.arm_target = Dummy("target")
self.gripper = Joint("RG2_openCloseJoint")
class CustomPickAndLiftNoRotation(Task):
def init_task(self) -> None:
self.target_block = Shape('pick_and_lift_target')
self.target = Shape("success_visual")
self.target.set_renderable(False)
self.register_graspable_objects([self.target_block])
self.boundary = SpawnBoundary([Shape('pick_and_lift_boundary')])
self.success_detector = ProximitySensor('pick_and_lift_success')
self.front_camera_exists = Object.exists('cam_front')
if self.front_camera_exists:
self.front_camera = VisionSensor('cam_front')
self.init_front_camera_position = self.front_camera.get_position()
self.init_front_camera_orientation = self.front_camera.get_orientation(
)
self.panda_base = Shape("Panda_link0_visual")
cond_set = ConditionSet([
GraspedCondition(self.robot.gripper, self.target_block),
DetectedCondition(self.target_block, self.success_detector)
])
self.register_success_conditions([cond_set])
def init_episode(self, index: int) -> List[str]:
block_color_name, block_rgb = colors[index]
self.target_block.set_color(block_rgb)
self.boundary.clear()
self.boundary.sample(self.success_detector,
min_rotation=(0.0, 0.0, 0.0),
max_rotation=(0.0, 0.0, 0.0))
for block in [self.target_block]:
self.boundary.sample(block,
min_distance=0.1,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
if self.front_camera_exists:
# apply new position to front camera for better generalization
self.front_camera_new_position()
return [
'pick up the %s block and lift it up to the target' %
block_color_name,
'grasp the %s block to the target' % block_color_name,
'lift the %s block up to the target' % block_color_name
]
def base_rotation_bounds(self):
# do not allow base rotation
return (0.0, 0.0, 0), (0.0, 0.0, 0)
def variation_count(self) -> int:
return len(colors)
def step(self) -> None:
if self.front_camera_exists:
# apply new position to front camera for better generalization
self.front_camera_new_position()
def get_low_dim_state(self) -> np.ndarray:
# get x, y, z from target block position
block_position = self.target_block.get_position()
# get x, y, z from target position
target_position = self.target.get_position()
return np.concatenate((block_position, target_position))
def reward(self):
""" the custom reward consists of two parts:
1. distance between the gripper and the target_block
2. distance between the target_block and the target (lift_target)
the total reward is the sum of both parts"""
# success conditions
object_grasped = self._success_conditions[0].condition_met()[0]
max_precision = 0.01 # 1cm
max_reward = 1 / max_precision
scale = 0.1
# first part
gripper_position = self.robot.arm.get_tip().get_position()
target_block_position = self.target_block.get_position()
dist = np.sqrt(
np.sum(np.square(
np.subtract(target_block_position, gripper_position)),
axis=0)) # euclidean norm
reward1 = min((1 / (dist + 0.00001)), max_reward)
reward1 = scale * reward1
# second part
if object_grasped:
print("Object Grasped!")
target_position = self.target.get_position()
dist = np.sqrt(
np.sum(np.square(
np.subtract(target_position, target_block_position)),
axis=0)) # euclidean norm
reward2 = min((1 / (dist + 0.00001)), max_reward)
reward2 = scale * reward2 + 5.0
else:
reward2 = 0
return reward1 + reward2
def front_camera_new_position(self):
""" changes the position of the front camera to a new one, and rotates the camera in a way that it sees the
target-block and the Panda-base. """
# reset camera position
self.front_camera.set_position(self.init_front_camera_position)
# --- move front camera to new position ---
# calculate the new (random) position
new_rel_front_camera_pos = np.random.uniform(low=-0.5,
high=0.5,
size=(3, ))
# move camera
self.front_camera.set_position(new_rel_front_camera_pos,
relative_to=self.front_camera)
# --- turn the camera ---
# -> camera should be turned to look at the new position -> mean of Panda_base and target_block
# get the coordinates
tar_block_obj_pos = self.target_block.get_position(
relative_to=self.front_camera)
panda_base_pos = self.panda_base.get_position(
relative_to=self.front_camera)
# calculate the mean to get the position to look at
look_at = np.mean(np.array([tar_block_obj_pos, panda_base_pos]),
axis=0)
# rotation arround y
rot_y = math.atan2(look_at[0], look_at[2])
# rotation arround x
rot_x = math.atan2(
look_at[1],
math.sqrt(math.pow(look_at[0], 2) + math.pow(look_at[2], 2)))
# calculate random rotation around z
rot_z = random.uniform(-math.pi, math.pi)
# apply rotation
old_orientation = self.front_camera.get_orientation(
relative_to=self.front_camera)
self.front_camera.set_orientation(
[old_orientation[0] - rot_x, old_orientation[1] + rot_y, rot_z],
relative_to=self.front_camera)
