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)