def _act(self, variables_dict): """ :param variables_dict: :return: """ # action 1 is [0, 1, 2, 3, 4, 5, 6] 0 do nothing, 1 go # up 2 down, 3 right, 4 left, 5, front, 6 back # action 2 is [0, 1, 2, 3, 4, 5, 6] 0 do nothing, 1 yaw clockwise, # 2 yaw anticlockwise, 3 roll clockwise, # 4 roll anticlockwise, 5 pitch clockwise, 6 pitch anticlockwise interventions_dict = dict() # interventions_dict['joint_positions'] = self.low_joint_positions interventions_dict[self.selected_object] = dict() if self.current_action[1] != 0: interventions_dict[self.selected_object]['cartesian_position'] = \ variables_dict[self.selected_object]['cartesian_position'] if self.current_action[1] == 1: interventions_dict[ self.selected_object]['cartesian_position'][-1] += 0.002 elif self.current_action[1] == 2: interventions_dict[ self.selected_object]['cartesian_position'][-1] -= 0.002 elif self.current_action[1] == 3: interventions_dict[ self.selected_object]['cartesian_position'][0] += 0.002 elif self.current_action[1] == 4: interventions_dict[ self.selected_object]['cartesian_position'][0] -= 0.002 elif self.current_action[1] == 5: interventions_dict[ self.selected_object]['cartesian_position'][1] += 0.002 elif self.current_action[1] == 6: interventions_dict[ self.selected_object]['cartesian_position'][1] -= 0.002 else: raise Exception("The passed action mode is not supported") if self.current_action[2] != 0: euler_orientation = \ quaternion_to_euler(variables_dict [self.selected_object]['orientation']) if self.current_action[2] == 1: euler_orientation[-1] += 0.1 elif self.current_action[2] == 2: euler_orientation[-1] -= 0.1 elif self.current_action[2] == 3: euler_orientation[1] += 0.1 elif self.current_action[2] == 4: euler_orientation[1] -= 0.1 elif self.current_action[2] == 5: euler_orientation[0] += 0.1 elif self.current_action[2] == 6: euler_orientation[0] -= 0.1 else: raise Exception("The passed action mode is not supported") interventions_dict[self.selected_object]['orientation'] = \ euler_to_quaternion(euler_orientation) return interventions_dict
def apply_interventions(self, interventions_dict): """ :param interventions_dict: (dict) specifies the interventions to be performed on the goal shape itself. :return: """ #TODO: Add frictions to apply interventions if 'cylindrical_position' in interventions_dict: interventions_dict['cartesian_position'] = \ cyl2cart(interventions_dict['cylindrical_position']) if 'euler_orientation' in interventions_dict: interventions_dict['orientation'] = euler_to_quaternion( interventions_dict['euler_orientation']) if 'cartesian_position' not in interventions_dict or \ 'orientation' not in interventions_dict: position, orientation = pybullet.\ getBasePositionAndOrientation(self._block_ids[0], physicsClientId= self._pybullet_client_ids[0]) position = np.array(position) position[-1] -= WorldConstants.FLOOR_HEIGHT if 'cartesian_position' in interventions_dict: position = interventions_dict['cartesian_position'] if 'orientation' in interventions_dict: orientation = interventions_dict['orientation'] if 'size' in interventions_dict: self._size = interventions_dict['size'] self._set_volume() self.reinit_object() if 'cartesian_position' in interventions_dict or 'orientation' in \ interventions_dict: for i in range(0, len(self._pybullet_client_ids)): position[-1] += WorldConstants.FLOOR_HEIGHT pybullet.resetBasePositionAndOrientation( self._block_ids[i], position, orientation, physicsClientId=self._pybullet_client_ids[i]) if 'color' in interventions_dict: self._color = interventions_dict['color'] self._set_color(self._color) return
def _create_new_challenge(self, num_of_levels, blocks_min_size, blocks_mass, max_level_width): """ :param num_of_levels: :param blocks_min_size: :param blocks_mass: :param max_level_width: :return: """ silhouettes_creation_dicts = [] self._stage.remove_everything() self._task_stage_observation_keys = [] block_sizes, positions, chosen_y = self._generate_random_target( num_of_levels=num_of_levels, min_size=blocks_min_size, max_level_width=max_level_width) for level_num in range(len(block_sizes)): for i in range(len(block_sizes[level_num])): creation_dict = { 'name': "tool_" + "level_" + str(level_num) + "_num_" + str(i), 'shape': "cube", 'mass': blocks_mass, 'color': np.random.uniform(0, 1, size=[3]), 'size': block_sizes[level_num][i] } self._stage.add_rigid_general_object(**creation_dict) block_position = self._stage.random_position( height_limits=block_sizes[level_num][i][-1] / 2.0) block_orientation = euler_to_quaternion( [0, 0, np.random.uniform(-np.pi, np.pi)]) self._stage.set_objects_pose(names=[ "tool_" + "level_" + str(level_num) + "_num_" + str(i) ], positions=[block_position], orientations=[block_orientation]) trial_index = 0 self._robot.step_simulation() while not self._stage.check_feasiblity_of_stage() and \ trial_index < 10: block_position = self._stage.random_position( height_limits=[ block_sizes[level_num][i][-1] / 2.0, 0.15 ]) block_orientation = euler_to_quaternion( [0, 0, np.random.uniform(-np.pi, np.pi)]) self._stage.set_objects_pose(names=[ "tool_" + "level_" + str(level_num) + "_num_" + str(i) ], positions=[block_position], orientations=[ block_orientation ]) self._robot.step_simulation() trial_index += 1 silhouette_position = [ positions[level_num][i], chosen_y, (level_num + 1) * blocks_min_size + (-blocks_min_size / 2 + 0) ] self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_type') self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_size') self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_cartesian_position') self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_orientation') self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_linear_velocity') self._task_stage_observation_keys.append("tool_" + "level_" + str(level_num) + "_num_" + str(i) + '_angular_velocity') self._task_stage_observation_keys.append("goal_" + "level_" + str(level_num) + "_num_" + str(i) + '_type') self._task_stage_observation_keys.append("goal_" + "level_" + str(level_num) + "_num_" + str(i) + '_size') self._task_stage_observation_keys.append("goal_" + "level_" + str(level_num) + "_num_" + str(i) + '_cartesian_position') self._task_stage_observation_keys.append("goal_" + "level_" + str(level_num) + "_num_" + str(i) + '_orientation') creation_dict = { 'name': "goal_" + "level_" + str(level_num) + "_num_" + str(i), 'shape': "cube", 'position': np.array(silhouette_position), 'size': np.array(block_sizes[level_num][i]) } silhouettes_creation_dicts.append(copy.deepcopy(creation_dict)) self.current_stack_levels = num_of_levels self.current_blocks_mass = blocks_mass self.current_blocks_min_size = blocks_min_size self.current_max_level_width = max_level_width for i in range(len(silhouettes_creation_dicts)): self._stage.add_silhoutte_general_object( **silhouettes_creation_dicts[i]) return
def _generate_goal_configuration_with_objects(self, default_bool): """ :param default_bool: :return: """ self._stage.remove_everything() stage_low_bound = np.array(self._stage.get_arena_bb()[0]) stage_low_bound[:2] += 0.04 stage_upper_bound = np.array(self._stage.get_arena_bb()[1]) stage_upper_bound[:2] -= 0.04 stage_upper_bound[2] -= 0.08 self._task_stage_observation_keys = [] joint_positions = self._robot.get_joint_positions_raised() self._robot.reset_state(joint_positions=joint_positions, joint_velocities=np.zeros(9)) for object_num in range(self.nums_objects): if default_bool: dropping_position = self.default_drop_positions[ object_num % len(self.default_drop_positions)] dropping_orientation = [0, 0, 0, 1] else: dropping_position = np.random.uniform(stage_low_bound, stage_upper_bound) dropping_orientation = euler_to_quaternion( np.random.uniform(low=0, high=2 * math.pi, size=3)) creation_dict = { 'name': "tool_" + str(object_num), 'shape': "cube", 'initial_position': dropping_position, 'initial_orientation': dropping_orientation, 'mass': self.tool_mass, 'size': np.repeat(self.tool_block_size, 3) } self._stage.add_rigid_general_object(**creation_dict) self._task_stage_observation_keys.append("tool_" + str(object_num) + '_type') self._task_stage_observation_keys.append("tool_" + str(object_num) + '_size') self._task_stage_observation_keys.append("tool_" + str(object_num) + '_cartesian_position') self._task_stage_observation_keys.append("tool_" + str(object_num) + '_orientation') self._task_stage_observation_keys.append("tool_" + str(object_num) + '_linear_velocity') self._task_stage_observation_keys.append("tool_" + str(object_num) + '_angular_velocity') self._robot.forward_simulation(time=0.8) for rigid_object in self._stage._rigid_objects: creation_dict = { 'name': rigid_object.replace('tool', 'goal'), 'shape': "cube", 'position': self._stage.get_object_state(rigid_object, 'cartesian_position'), 'orientation': self._stage.get_object_state(rigid_object, 'orientation'), 'size': np.repeat(self.tool_block_size, 3) } self._stage.add_silhoutte_general_object(**creation_dict) self._task_stage_observation_keys.append( rigid_object.replace('tool', 'goal') + '_type') self._task_stage_observation_keys.append( rigid_object.replace('tool', 'goal') + '_size') self._task_stage_observation_keys.append( rigid_object.replace('tool', 'goal') + '_cartesian_position') self._task_stage_observation_keys.append( rigid_object.replace('tool', 'goal') + '_orientation') trial_index = 1 block_position = self._stage.random_position( height_limits=[self.tool_block_size / 2.0, 0.15]) block_orientation = euler_to_quaternion( [0, 0, np.random.uniform(-np.pi, np.pi)]) self._stage.set_objects_pose(names=[rigid_object], positions=[block_position], orientations=[block_orientation]) self._robot.step_simulation() while not self._stage.check_feasiblity_of_stage() and \ trial_index < 10: block_position = self._stage.random_position( height_limits=[self.tool_block_size / 2.0, 0.15]) block_orientation = euler_to_quaternion( [0, 0, np.random.uniform(-np.pi, np.pi)]) self._stage.set_objects_pose(names=[rigid_object], positions=[block_position], orientations=[block_orientation]) self._robot.step_simulation() trial_index += 1 self._robot.reset_state(joint_positions=joint_positions, joint_velocities=np.zeros([ 9, ])) self._robot.update_latest_full_state() return
def apply_interventions(self, interventions_dict): """ :param interventions_dict: (dict) specifies the interventions to be performed on the various variables. :return: """ #TODO: Add frictions to apply interventions if 'cylindrical_position' in interventions_dict: interventions_dict['cartesian_position'] = cyl2cart( interventions_dict['cylindrical_position']) if 'euler_orientation' in interventions_dict: interventions_dict['orientation'] = euler_to_quaternion( interventions_dict['euler_orientation']) if 'cartesian_position' not in interventions_dict or \ 'orientation' not in interventions_dict: position, orientation = pybullet.\ getBasePositionAndOrientation(self._block_ids[0], physicsClientId= self._pybullet_client_ids[0]) position = np.array(position) position[-1] -= WorldConstants.FLOOR_HEIGHT if 'cartesian_position' in interventions_dict: position = interventions_dict['cartesian_position'] if 'orientation' in interventions_dict: orientation = interventions_dict['orientation'] if 'mass' in interventions_dict: self._mass = interventions_dict['mass'] if 'friction' in interventions_dict: self._lateral_friction = interventions_dict['friction'] if 'size' in interventions_dict: self._size = interventions_dict['size'] self._set_volume() self.reinit_object() elif 'mass' in interventions_dict: for i in range(0, len(self._pybullet_client_ids)): pybullet.changeDynamics( self._block_ids[i], -1, mass=self._mass, physicsClientId=self._pybullet_client_ids[i]) elif 'friction' in interventions_dict: self._set_lateral_friction(self._lateral_friction) if 'cartesian_position' in interventions_dict or 'orientation' in \ interventions_dict: for i in range(0, len(self._pybullet_client_ids)): position[-1] += WorldConstants.FLOOR_HEIGHT pybullet.resetBasePositionAndOrientation( self._block_ids[i], position, orientation, physicsClientId=self._pybullet_client_ids[i]) if 'color' in interventions_dict: self._color = interventions_dict['color'] self._set_color(self._color) if ('linear_velocity' in interventions_dict) ^ \ ('angular_velocity' in interventions_dict): for i in range(0, len(self._pybullet_client_ids)): linear_velocity, angular_velocity = \ pybullet.getBaseVelocity( self._block_ids[i], physicsClientId= self._pybullet_client_ids[i]) if 'linear_velocity' in interventions_dict: linear_velocity = interventions_dict['linear_velocity'] if 'angular_velocity' in interventions_dict: angular_velocity = interventions_dict['angular_velocity'] if 'angular_velocity' in interventions_dict or 'linear_velocity' in \ interventions_dict: for i in range(0, len(self._pybullet_client_ids)): pybullet.resetBaseVelocity( self._block_ids[i], linear_velocity, angular_velocity, physicsClientId=self._pybullet_client_ids[i]) return