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