def set_target_object(self, random_object=False, random_position=False):
if random_object:
rand_object = random.choice(self.object_list)
self.object_name = rand_object["name"]
self.object_type_str = rand_object["type"]
self.object_type = self.object_list.index(rand_object)
self.object_height = rand_object["height"]
init_pos = rand_object["init_pos"]
self.object_initial_position = Pose(position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]),
orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5]))
else:
self.object_name = self.object_list[0]["name"]
self.object_type_str = self.object_list[0]["type"]
self.object_type = 0
self.object_height = self.object_list[0]["height"]
init_pos = self.object_list[0]["init_pos"]
self.object_initial_position = Pose(position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]),
orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5]))
if random_position:
if self.object_type_str == "door_handle":
box_pos = U.get_random_door_handle_pos()
else:
box_pos = Pose(position=Point(x=np.random.uniform(low=-0.3, high=0.3, size=None),
y=np.random.uniform(low=0.9, high=1.1, size=None),
z=1.05),
orientation=quaternion_from_euler(0, 0, 0))
else:
box_pos = self.object_initial_position
U.change_object_position(self.object_name, box_pos)
print("Current target: ", self.object_name)
def set_target_object(self, position):
self.object_name = self.object_list[0]["name"]
self.object_type_str = self.object_list[0]["type"]
self.object_type = 0
box_pos = Pose(position=Point(x=position[0],
y=position[1],
z=position[2]),
orientation=quaternion_from_euler(
position[3], position[4], position[5]))
U.change_object_position(self.object_name, box_pos)
print("Current target: ", self.object_name)
def set_target_object(self, random_object=False, random_position=False):
"""
Set target object
:param random_object: spawn object randomly from the object pool. If false, object will be the first entry of the object list
:param random_position: spawn object with random position
"""
if random_object:
rand_object = random.choice(self.object_list)
self.object_name = rand_object["name"]
self.object_type_str = rand_object["type"]
self.object_type = self.object_list.index(rand_object)
init_pos = rand_object["init_pos"]
self.object_initial_position = Pose(
position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]),
orientation=quaternion_from_euler(init_pos[3], init_pos[4],
init_pos[5]))
else:
self.object_name = self.object_list[0]["name"]
self.object_type_str = self.object_list[0]["type"]
self.object_type = 0
init_pos = self.object_list[0]["init_pos"]
self.object_initial_position = Pose(
position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]),
orientation=quaternion_from_euler(init_pos[3], init_pos[4],
init_pos[5]))
if random_position:
if self.object_type_str == "door_handle":
box_pos = U.get_random_door_handle_pos()
else:
box_pos = Pose(position=Point(x=np.random.uniform(low=-0.3,
high=0.3,
size=None),
y=np.random.uniform(low=0.9,
high=1.1,
size=None),
z=1.05),
orientation=quaternion_from_euler(0, 0, 0))
else:
box_pos = self.object_initial_position
U.change_object_position(self.object_name, box_pos)
print("Current target: ", self.object_name)
def reset(self):
"""
Reset The Robot to its initial Position and restart the Controllers
1) Change Gravity to 0 -> That arm doesnt fall
2) Turn Controllers off
3) Pause Simulation
4) Delete previous target object if randomly chosen object is set to True
4) Reset Simulation
5) Set Model Pose to desired one
6) Unpause Simulation
7) Turn on Controllers
8) Restore Gravity
9) Get Observations and return current State
10) Check all Systems work
11) Spawn new target
12) Pause Simulation
13) Write initial Position into Yaml File
14) Create YAML Files for contact forces in order to get the average over 2 contacts
15) Create YAML Files for collision to make sure to see a collision due to high noise in topic
16) Unpause Simulation cause in next Step System must be running otherwise no data is seen by Subscribers
17) Publish Episode Reward and set accumulated reward back to 0 and iterate the Episode Number
18) Return State
"""
# print("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Reset %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
self.gazebo.change_gravity(0, 0, 0)
self.controllers_object.turn_off_controllers()
# turn off the gripper
# U.turn_off_gripper()
self.gazebo.resetSim()
self.gazebo.pauseSim()
self.gazebo.resetSim()
time.sleep(0.1)
# turn on the gripper
# U.turn_on_gripper()
if self._load_init_pos:
# load sample from previous training result
sample_ep = random.choice(self.init_samples)
print("Joints from samples: {}".format(sample_ep[0:6]))
# self.pickbot_joint_publisher_object.set_joints(sample_ep[0:6])
self.set_target_object(sample_ep[-6:])
else:
self.pickbot_joint_publisher_object.set_joints()
vg_geo = U.get_link_state("vacuum_gripper_link")
to_geo = U.get_link_state("target")
orientation_error = quaternion_multiply(
vg_geo[3:], quaternion_conjugate(to_geo[3:]))
# print("Orientation error {}".format(orientation_error))
box_pos = U.get_random_door_handle_pos(
) if self._random_position else self.object_initial_position
U.change_object_position(self.object_name, box_pos)
# Code above is hard-coded for door handle, modify later.
# TO-DO: Modify reset wrt the object type as in the reach env
self.gazebo.unpauseSim()
self.controllers_object.turn_on_controllers()
self.gazebo.change_gravity(0, 0, -9.81)
self._check_all_systems_ready()
# last_position = [1.5, -1.2, 1.4, -1.87, -1.57, 0]
# last_position = [0, 0, 0, 0, 0, 0]
# with open('last_position.yml', 'w') as yaml_file:
# yaml.dump(last_position, yaml_file, default_flow_style=False)
# with open('contact_1_force.yml', 'w') as yaml_file:
# yaml.dump(0.0, yaml_file, default_flow_style=False)
# with open('contact_2_force.yml', 'w') as yaml_file:
# yaml.dump(0.0, yaml_file, default_flow_style=False)
with open('collision.yml', 'w') as yaml_file:
yaml.dump(False, yaml_file, default_flow_style=False)
observation = self.get_obs()
# print("current joints {}".format(observation[:6]))
# get maximum distance to the object to calculate reward
# self.max_distance, _ = U.get_distance_gripper_to_object()
# self.min_distance = self.max_distance
self.gazebo.pauseSim()
state = U.get_state(observation)
self._update_episode()
self.gazebo.unpauseSim()
return state