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