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)
class ReachTargetWithObstacles(Task):
def init_task(self) -> None:
self.target = Shape('target')
self.distractor0 = Shape('distractor0')
self.distractor1 = Shape('distractor1')
self.target.set_renderable(True)
self.distractor0.set_renderable(False)
self.distractor1.set_renderable(False)
self.front_camera_exists = Object.exists('cam_front')
if self.front_camera_exists:
self.front_camera = VisionSensor('cam_front')
#curr_pos = self.front_camera.get_position()
# new_pos = curr_pos + np.array([-1.25, 1.6 , -0.35])
# self.front_camera.set_position(new_pos)
# curr_ori = self.front_camera.get_orientation()
# new_ori = np.array([1.6, 0, 0])
# self.front_camera.set_orientation(new_ori)
# front pos
#new_pos = curr_pos + np.array([0.0, 0.0 , -0.2])
#self.front_camera.set_position(new_pos)
#curr_ori = self.front_camera.get_orientation()
#new_ori = curr_ori + np.array([0, -0.25, 0])
#self.front_camera.set_orientation(new_ori)
self.step_in_episode = 0
self.obstacle = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.3, 0.3, 0.3],
renderable=True,
respondable=True,
static=True,
position=[0.1, 0.1, 1.4],
color=[0, 0, 0])
self.boundaries = Shape('boundary')
success_sensor = ProximitySensor('success')
self.register_success_conditions(
[DetectedCondition(self.robot.arm.get_tip(), success_sensor)])
def init_episode(self, index: int) -> List[str]:
color_name, color_rgb = colors[index]
self.target.set_color(color_rgb)
color_choices = np.random.choice(list(range(index)) +
list(range(index + 1, len(colors))),
size=2,
replace=False)
for ob, i in zip([self.distractor0, self.distractor1], color_choices):
name, rgb = colors[i]
ob.set_color(rgb)
b = SpawnBoundary([self.boundaries])
for ob in [self.target, self.distractor0, self.distractor1]:
b.sample(ob,
min_distance=0.2,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
# change the position of the robot
q = self.robot.arm.get_joint_positions()
new_q = np.array([1.2, 0, 0, 0, 0, 0, 0]) + q
self.robot.arm.set_joint_positions(new_q)
self.step_in_episode = 0
return [
'reach the %s target' % color_name,
'touch the %s ball with the panda gripper' % color_name,
'reach the %s sphere' % color_name
]
# def step(self) -> None:
#
# if self.step_in_episode == 0:
# rand_pos = np.array([np.random.uniform(0, +0.5), np.random.uniform(-0.5, 0.5), np.random.uniform(0.77, 1.5)])
# self.obstacle.set_position(rand_pos)
# self.step_in_episode += 1
def variation_count(self) -> int:
return len(colors)
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]
def get_low_dim_state(self) -> np.ndarray:
# One of the few tasks that have a custom low_dim_state function.
return np.array(self.target.get_position())
def is_static_workspace(self) -> bool:
return True
def reward(self):
""" Calculates the reward for the ReachTargetWithObstacles Task based on the euclidean distance. """
max_precision = 0.01 # 1cm
max_reward = 1 / max_precision
scale = 0.1
gripper_position = self.robot.arm.get_tip().get_position()
target_position = self.target.get_position()
dist = np.sqrt(
np.sum(np.square(np.subtract(target_position, gripper_position)),
axis=0)) # euclidean norm
reward = min((1 / (dist + 0.00001)), max_reward)
reward = scale * reward
return reward
def look_at(self, look_at):
# 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)
