def GetState(self, request, context): try: msg = robot_server_pb2.State() return self.rosbridge.get_state() except: rospy.logerr('Failed to get state', exc_info=True) return robot_server_pb2.State(success=0)
def GetState(self, request, context): try: msg = robot_server_pb2.State() msg.state.extend(self.rosbridge.get_state()) msg.success = 1 return msg except: return robot_server_pb2.State(success = 0)
def SendActionGetState(self, request, context): try: executed_action = self.rosbridge.publish_env_arm_cmd(request.action) return self.rosbridge.get_state() except: rospy.logerr('Failed to send action and get state', exc_info=True) return robot_server_pb2.State(success = 0)
def _set_initial_robot_server_state( self, rs_state, fixed_object_position=None) -> robot_server_pb2.State: if fixed_object_position: state_msg = super()._set_initial_robot_server_state( rs_state=rs_state, fixed_object_position=fixed_object_position) return state_msg z_amplitude = np.random.default_rng().uniform(low=0.09, high=0.35) z_frequency = 0.125 z_offset = np.random.default_rng().uniform(low=0.2, high=0.6) string_params = {"object_0_function": "triangle_wave"} float_params = { "object_0_x": 0.12, "object_0_y": 0.34, "object_0_z_amplitude": z_amplitude, "object_0_z_frequency": z_frequency, "object_0_z_offset": z_offset } state = {} state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def get_state(self, ): msg = self.robot_server_stub.GetState(robot_server_pb2.State(), timeout=20) if msg.success == 1: return msg.state else: raise Exception('Error while retrieving state')
def get_state(self): self.get_state_event.clear() # # currently only working on a fixed target mode if self.target_mode == FIXED_TARGET_MODE: target = copy.deepcopy(self.target) else: raise ValueError panda_state = copy.deepcopy(self.panda_state) # # TODO is ee_to_base_transform value correctly loaded and set # (position, quaternion) = self.tf_listener.lookupTransform( # self.reference_frame) # ee_to_base_transform = position + quaternion # # TODO currently not needed # # if self.real_robot: # # panda_collision = False # # else: # # panda_collision = any(self.collision_sensors.values()) self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State() msg.state.extend(target) msg.state.extend(panda_state) # msg.state.extend(ee_to_base_transform) # msg.state.extend([panda_collision]) msg.success = True return msg
def get_state(self): self.get_state_event.clear() # Get environment state state = [] target = copy.deepcopy(self.target) mir_pose = copy.deepcopy(self.mir_pose) mir_twist = copy.deepcopy(self.mir_twist) mir_f_scan = copy.deepcopy(self.f_scan) mir_b_scan = copy.deepcopy(self.b_scan) in_collision = copy.deepcopy(self.collision) obstacles = [0.0] * 9 self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State() msg.state.extend(target) msg.state.extend(mir_pose) msg.state.extend(mir_twist) msg.state.extend(mir_f_scan) msg.state.extend(mir_b_scan) msg.state.extend([in_collision]) msg.state.extend(obstacles) msg.success = 1 return msg
def reset(self, start_pose = None, target_pose = None): """Environment reset. Args: start_pose (list[3] or np.array[3]): [x,y,yaw] initial robot position. target_pose (list[3] or np.array[3]): [x,y,yaw] target robot position. Returns: np.array: Environment state. """ self.elapsed_steps = 0 self.prev_base_reward = None # Initialize environment state self.state = np.zeros(self._get_env_state_len()) rs_state = np.zeros(self._get_robot_server_state_len()) # Set Robot starting position if start_pose: assert len(start_pose)==3 else: start_pose = self._get_start_pose() rs_state[3:6] = start_pose # Set target position if target_pose: assert len(target_pose)==3 else: target_pose = self._get_target(start_pose) rs_state[0:3] = target_pose # Generate obstacles positions self._generate_obstacles_positions() rs_state[1021:1024] = self.sim_obstacles[0] rs_state[1024:1027] = self.sim_obstacles[1] rs_state[1027:1030] = self.sim_obstacles[2] # Set initial state of the Robot Server state_msg = robot_server_pb2.State(state = rs_state.tolist()) if not self.client.set_state_msg(state_msg): raise RobotServerError("set_state") # Get Robot Server state rs_state = copy.deepcopy(np.nan_to_num(np.array(self.client.get_state_msg().state))) # Check if the length of the Robot Server state received is correct if not len(rs_state)== self._get_robot_server_state_len(): raise InvalidStateError("Robot Server state received has wrong length") # Convert the initial state from Robot Server format to environment format self.state = self._robot_server_state_to_env_state(rs_state) # Check if the environment state is contained in the observation space if not self.observation_space.contains(self.state): raise InvalidStateError() return self.state
def _set_initial_robot_server_state(self, rs_state) -> robot_server_pb2.State: string_params = {} float_params = {} state = {} state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def _set_initial_robot_server_state( self, rs_state, ee_target_pose) -> robot_server_pb2.State: string_params = {"object_0_function": "fixed_position"} float_params = { "object_0_x": ee_target_pose[0], "object_0_y": ee_target_pose[1], "object_0_z": ee_target_pose[2] } state = {} state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def _set_initial_robot_server_state( self, rs_state, fixed_object_position=None) -> robot_server_pb2.State: if fixed_object_position: state_msg = super()._set_initial_robot_server_state( rs_state=rs_state, fixed_object_position=fixed_object_position) return state_msg string_params = {"object_0_function": "fixed_trajectory"} float_params = {"object_0_trajectory_id": self.ep_n % 50} state = {} state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def get_state(self): self.get_state_event.clear() # Get environment state state = [] if self.target_mode == 'fixed': target = copy.deepcopy(self.target) elif self.target_mode == 'moving': if self.real_robot: (t_position, t_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.target_frame, rospy.Time(0)) target = t_position + [0, 0, 0] else: pose = self.get_model_state_pose(self.target_model_name) # Convert orientation target from Quaternion to RPY quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4], pose[5], pose[6]) r, p, y = quaternion.GetRPY() target = pose[0:3] + [r, p, y] else: raise ValueError ur_state = copy.deepcopy(self.ur_state) (position, quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.ee_frame, rospy.Time(0)) ee_to_base_transform = position + quaternion if self.real_robot: ur_collision = False else: ur_collision = any(self.collision_sensors.values()) self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State() msg.state.extend(target) msg.state.extend(ur_state) msg.state.extend(ee_to_base_transform) msg.state.extend([ur_collision]) msg.success = 1 return msg
def get_state(self): self.get_state_event.clear() # Get environment state state = [] mir_pose = copy.deepcopy(self.mir_pose) mir_twist = copy.deepcopy(self.mir_twist) is_collision = copy.deepcopy(self.collision) is_change_room = 0 is_change_mir_pose = 0 targets = copy.deepcopy(self.targets) if len(targets) != 0: targets = targets.flatten() map_size = copy.deepcopy(self.map_size) map_data = copy.deepcopy(self.map_data) map_data_trueth = copy.deepcopy(self.map_data_trueth) rgp = [ copy.deepcopy(self.room_generator_params[tag]) for tag in self.rgp_tags ] self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State() msg.state.extend([map_size]) msg.state.extend(map_data) msg.state.extend(map_data_trueth) msg.state.extend(mir_pose) msg.state.extend(mir_twist) msg.state.extend([is_collision]) msg.state.extend([is_change_room]) msg.state.extend([is_change_mir_pose]) msg.state.extend(rgp) msg.state.extend(targets) msg.success = 1 return msg
def _set_initial_robot_server_state( self, rs_state, fixed_object_position) -> robot_server_pb2.State: string_params = {} float_params = {} state = {} # Set initial state of the Robot Server if fixed_object_position: # Object in a fixed position string_params = {"object_0_function": "fixed_position"} float_params = { "object_0_x": fixed_object_position[0], "object_0_y": fixed_object_position[1], "object_0_z": fixed_object_position[2] } state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def _set_initial_robot_server_state( self, rs_state, fixed_object_position=None) -> robot_server_pb2.State: if fixed_object_position: state_msg = super()._set_initial_robot_server_state( rs_state=rs_state, fixed_object_position=fixed_object_position) return state_msg n_sampling_points = int(np.random.default_rng().uniform(low=8000, high=12000)) string_params = {"object_0_function": "3d_spline_ur5_workspace"} float_params = {"object_0_x_min": -1.0, "object_0_x_max": 1.0, "object_0_y_min": -1.0, "object_0_y_max": 1.0, \ "object_0_z_min": 0.1, "object_0_z_max": 1.0, "object_0_n_points": 10, \ "n_sampling_points": n_sampling_points} state = {} state_msg = robot_server_pb2.State(state=state, float_params=float_params, string_params=string_params, state_dict=rs_state) return state_msg
def reset(self, initial_joint_positions=None, ee_target_pose=None, type='random'): """Environment reset. Args: initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians. ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose. Returns: np.array: Environment state. """ self.elapsed_steps = 0 self.last_action = None self.prev_base_reward = None # Initialize environment state self.state = np.zeros(self._get_env_state_len()) rs_state = np.zeros(self._get_robot_server_state_len()) print(self.last_position_on_success) # Set initial robot joint positions if initial_joint_positions: assert len(initial_joint_positions) == 6 ur5_initial_joint_positions = initial_joint_positions elif (len(self.last_position_on_success) != 0) and (type == 'continue'): ur5_initial_joint_positions = self.last_position_on_success else: ur5_initial_joint_positions = self._get_initial_joint_positions() rs_state[6:12] = self.ur5._ur_5_joint_list_to_ros_joint_list( ur5_initial_joint_positions) # Set target End Effector pose if ee_target_pose: assert len(ee_target_pose) == 6 else: ee_target_pose = self._get_target_pose() rs_state[0:6] = ee_target_pose # Set initial state of the Robot Server state_msg = robot_server_pb2.State(state=rs_state.tolist()) if not self.client.set_state_msg(state_msg): raise RobotServerError("set_state") # Get Robot Server state rs_state = copy.deepcopy( np.nan_to_num(np.array(self.client.get_state_msg().state))) # Check if the length of the Robot Server state received is correct if not len(rs_state) == self._get_robot_server_state_len(): raise InvalidStateError( "Robot Server state received has wrong length") # Convert the initial state from Robot Server format to environment format self.state = self._robot_server_state_to_env_state(rs_state) # Check if the environment state is contained in the observation space if not self.observation_space.contains(self.state): raise InvalidStateError() # check if current position is in the range of the initial joint positions if (len(self.last_position_on_success) == 0) or (type == 'random'): joint_positions = self.ur5._ros_joint_list_to_ur5_joint_list( rs_state[6:12]) tolerance = 0.1 for joint in range(len(joint_positions)): if (joint_positions[joint] + tolerance < self.initial_joint_positions_low[joint]) or ( joint_positions[joint] - tolerance > self.initial_joint_positions_high[joint]): raise InvalidStateError( 'Reset joint positions are not within defined range') # go one empty action and check if there is a collision action = self.state[3:3 + len(self.action_space.sample())] _, _, done, info = self.step(action) self.elapsed_steps = 0 if done: raise InvalidStateError('Reset started in a collision state') return self.state
def get_state(self): self.get_state_event.clear() # Get environment state state =[] state_dict = {} if self.rs_mode == 'only_robot': # Joint Positions and Joint Velocities joint_position = copy.deepcopy(self.joint_position) joint_velocity = copy.deepcopy(self.joint_velocity) state += self._get_joint_ordered_value_list(joint_position) state += self._get_joint_ordered_value_list(joint_velocity) state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity)) # ee to ref transform ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0)) ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans) state += ee_to_ref_trans_list state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref')) # Collision sensors ur_collision = any(self.collision_sensors.values()) state += [ur_collision] state_dict['in_collision'] = float(ur_collision) elif self.rs_mode == '1object': # Object 0 Pose object_0_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.objects_frame[0], rospy.Time(0)) object_0_trans_list = self._transform_to_list(object_0_trans) state += object_0_trans_list state_dict.update(self._get_transform_dict(object_0_trans, 'object_0_to_ref')) # Joint Positions and Joint Velocities joint_position = copy.deepcopy(self.joint_position) joint_velocity = copy.deepcopy(self.joint_velocity) state += self._get_joint_ordered_value_list(joint_position) state += self._get_joint_ordered_value_list(joint_velocity) state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity)) # ee to ref transform ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0)) ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans) state += ee_to_ref_trans_list state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref')) # Collision sensors ur_collision = any(self.collision_sensors.values()) state += [ur_collision] state_dict['in_collision'] = float(ur_collision) elif self.rs_mode == '1moving2points': # Object 0 Pose object_0_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.objects_frame[0], rospy.Time(0)) object_0_trans_list = self._transform_to_list(object_0_trans) state += object_0_trans_list state_dict.update(self._get_transform_dict(object_0_trans, 'object_0_to_ref')) # Joint Positions and Joint Velocities joint_position = copy.deepcopy(self.joint_position) joint_velocity = copy.deepcopy(self.joint_velocity) state += self._get_joint_ordered_value_list(joint_position) state += self._get_joint_ordered_value_list(joint_velocity) state_dict.update(self._get_joint_states_dict(joint_position, joint_velocity)) # ee to ref transform ee_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, self.ee_frame, rospy.Time(0)) ee_to_ref_trans_list = self._transform_to_list(ee_to_ref_trans) state += ee_to_ref_trans_list state_dict.update(self._get_transform_dict(ee_to_ref_trans, 'ee_to_ref')) # Collision sensors ur_collision = any(self.collision_sensors.values()) state += [ur_collision] state_dict['in_collision'] = float(ur_collision) # forearm to ref transform forearm_to_ref_trans = self.tf2_buffer.lookup_transform(self.reference_frame, 'forearm_link', rospy.Time(0)) forearm_to_ref_trans_list = self._transform_to_list(forearm_to_ref_trans) state += forearm_to_ref_trans_list state_dict.update(self._get_transform_dict(forearm_to_ref_trans, 'forearm_to_ref')) else: raise ValueError self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State(state=state, state_dict=state_dict, success= True) return msg
def set_state(self, state): msg = self.robot_server_stub.SetState( robot_server_pb2.State(state=state), timeout=60) return msg.success
def GetState(self, request, context): try: return self.rosbridge.get_state() except: return robot_server_pb2.State(success=0)
def set_state(self, state): # Old method, to be gradually replaced in all the stack msg = self.robot_server_stub.SetState(robot_server_pb2.State(state = state), timeout = 60) return msg.success
def get_state(self): self.get_state_event.clear() # Get environment state state = [] if self.target_mode == 'fixed': trans = self.tf2_buffer.lookup_transform(self.reference_frame, 'target', rospy.Time(0)) target = [ trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z ] + [0, 0, 0] elif self.target_mode == 'moving': if self.real_robot: (t_position, t_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[0], rospy.Time(0)) target = t_position + [0, 0, 0] else: pose = self.get_model_state_pose(self.objects_model_name[0]) # Convert orientation target from Quaternion to RPY quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4], pose[5], pose[6]) r, p, y = quaternion.GetRPY() target = pose[0:3] + [r, p, y] elif self.target_mode == '2moving': if self.real_robot: (t_position, t_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[0], rospy.Time(0)) target = t_position + [0, 0, 0] (o2_position, o2_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[1], rospy.Time(0)) object2 = o2_position + [0, 0, 0] else: # Target t_pose = self.get_model_state_pose(self.objects_model_name[0]) # Convert orientation target from Quaternion to RPY t_quaternion = PyKDL.Rotation.Quaternion( t_pose[3], t_pose[4], t_pose[5], t_pose[6]) t_r, t_p, t_y = t_quaternion.GetRPY() target = t_pose[0:3] + [t_r, t_p, t_y] # Object 02 o2_pose = self.get_model_state_pose(self.objects_model_name[1]) # Convert orientation target from Quaternion to RPY o2_quaternion = PyKDL.Rotation.Quaternion( o2_pose[3], o2_pose[4], o2_pose[5], o2_pose[6]) o2_r, o2_p, o2_y = o2_quaternion.GetRPY() object2 = o2_pose[0:3] + [o2_r, o2_p, o2_y] elif self.target_mode == '2moving2points': (forearm_position, forearm_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, 'forearm_link', rospy.Time(0)) forearm_to_ref_tf = forearm_position + forearm_quaternion if self.real_robot: (t_position, t_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[0], rospy.Time(0)) target = t_position + [0, 0, 0] (o2_position, o2_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[1], rospy.Time(0)) object2 = o2_position + [0, 0, 0] else: # Target t_pose = self.get_model_state_pose(self.objects_model_name[0]) # Convert orientation target from Quaternion to RPY t_quaternion = PyKDL.Rotation.Quaternion( t_pose[3], t_pose[4], t_pose[5], t_pose[6]) t_r, t_p, t_y = t_quaternion.GetRPY() target = t_pose[0:3] + [t_r, t_p, t_y] # Object 02 o2_pose = self.get_model_state_pose(self.objects_model_name[1]) # Convert orientation target from Quaternion to RPY o2_quaternion = PyKDL.Rotation.Quaternion( o2_pose[3], o2_pose[4], o2_pose[5], o2_pose[6]) o2_r, o2_p, o2_y = o2_quaternion.GetRPY() object2 = o2_pose[0:3] + [o2_r, o2_p, o2_y] elif self.target_mode == '1moving2points': (forearm_position, forearm_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, 'forearm_link', rospy.Time(0)) forearm_to_ref_tf = forearm_position + forearm_quaternion if self.real_robot: (t_position, t_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.objects_frame[0], rospy.Time(0)) target = t_position + [0, 0, 0] else: # Target t_pose = self.get_model_state_pose(self.objects_model_name[0]) # Convert orientation target from Quaternion to RPY t_quaternion = PyKDL.Rotation.Quaternion( t_pose[3], t_pose[4], t_pose[5], t_pose[6]) t_r, t_p, t_y = t_quaternion.GetRPY() target = t_pose[0:3] + [t_r, t_p, t_y] elif self.target_mode == '1moving1point_2_2_4_voxel': (forearm_position, forearm_quaternion) = self.tf_listener.lookupTransform( self.reference_frame, 'forearm_link', rospy.Time(0)) forearm_to_ref_tf = forearm_position + forearm_quaternion if self.real_robot: # (t_position, t_quaternion) = self.tf_listener.lookupTransform(self.reference_frame,self.objects_frame[0],rospy.Time(0)) raise NotImplementedError( "voxelisation of real robot not yet implemented") else: pose = self.get_model_state_pose(self.objects_model_name[0]) # Convert orientation target from Quaternion to RPY quaternion = PyKDL.Rotation.Quaternion(pose[3], pose[4], pose[5], pose[6]) r, p, y = quaternion.GetRPY() target = pose[0:3] + [r, p, y] voxel_occupancy = self.voxel_occupancy else: raise ValueError ur_state = copy.deepcopy(self.ur_state) (position, quaternion) = self.tf_listener.lookupTransform( self.reference_frame, self.ee_frame, rospy.Time(0)) ee_to_base_transform = position + quaternion if self.real_robot: ur_collision = False else: ur_collision = any(self.collision_sensors.values()) self.get_state_event.set() # Create and fill State message msg = robot_server_pb2.State() msg.state.extend(target) msg.state.extend(ur_state) msg.state.extend(ee_to_base_transform) msg.state.extend([ur_collision]) if self.target_mode == '2moving': msg.state.extend(object2) if self.target_mode == '2moving2points': msg.state.extend(object2) msg.state.extend(forearm_to_ref_tf) if self.target_mode == '1moving2points': msg.state.extend(forearm_to_ref_tf) if self.target_mode == '1moving1point_2_2_4_voxel': msg.state.extend(forearm_to_ref_tf) # keep it in for now msg.state.extend(voxel_occupancy) msg.success = 1 return msg
def reset(self, initial_joint_positions=None, type='random'): """Environment reset. Args: initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians. ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose. Returns: np.array: Environment state. """ self.elapsed_steps = 0 self.last_action = None self.prev_base_reward = None # Initialize environment state self.state = np.zeros(self._get_env_state_len()) rs_state = np.zeros(self._get_robot_server_state_len()) # Set initial robot joint positions if initial_joint_positions: assert len(initial_joint_positions) == 6 ur5_initial_joint_positions = initial_joint_positions elif (len(self.last_position_on_success) != 0) and (type == 'continue'): ur5_initial_joint_positions = self.last_position_on_success else: ur5_initial_joint_positions = self._get_initial_joint_positions() rs_state[6:12] = self.ur5._ur_5_joint_list_to_ros_joint_list( ur5_initial_joint_positions) # Set initial state of the Robot Server # z_amplitude = 0.25 # z_frequency = 0.125 # z_offset = 0.35 z_amplitude = np.random.default_rng().uniform(low=0.09, high=0.35) z_frequency = 0.125 z_offset = np.random.default_rng().uniform(low=0.2, high=0.6) float_params = { "x": 0.13, "y": -0.30, "z_amplitude": z_amplitude, "z_frequency": z_frequency, "z_offset": z_offset } state_msg = robot_server_pb2.State(state=rs_state.tolist(), float_params=float_params) if not self.client.set_state_msg(state_msg): raise RobotServerError("set_state") # Get Robot Server state rs_state = copy.deepcopy( np.nan_to_num(np.array(self.client.get_state_msg().state))) self.prev_rs_state = copy.deepcopy(rs_state) # Check if the length of the Robot Server state received is correct if not len(rs_state) == self._get_robot_server_state_len(): raise InvalidStateError( "Robot Server state received has wrong length") # Convert the initial state from Robot Server format to environment format self.state = self._robot_server_state_to_env_state(rs_state) # save start position self.start_position = self.state[3:9] # Check if the environment state is contained in the observation space if not self.observation_space.contains(self.state): raise InvalidStateError() # check if current position is in the range of the initial joint positions if (len(self.last_position_on_success) == 0) or (type == 'random'): joint_positions = self.ur5._ros_joint_list_to_ur5_joint_list( rs_state[6:12]) tolerance = 0.1 for joint in range(len(joint_positions)): if (joint_positions[joint] + tolerance < self.initial_joint_positions_low[joint]) or ( joint_positions[joint] - tolerance > self.initial_joint_positions_high[joint]): raise InvalidStateError( 'Reset joint positions are not within defined range') # go one empty action and check if there is a collision action = self.state[4:7] _, _, done, info = self.step(action) self.elapsed_steps = 0 if done: raise InvalidStateError('Reset started in a collision state') return self.state
def reset(self, initial_joint_positions=None, ee_target_pose=None): """Environment reset. Args: initial_joint_positions (list[6] or np.array[6]): robot joint positions in radians. ee_target_pose (list[6] or np.array[6]): [x,y,z,r,p,y] target end effector pose. Returns: np.array: Environment state. """ self.elapsed_steps = 0 self.last_action = None self.prev_base_reward = None # Initialize environment state self.state = np.zeros(self._get_env_state_len()) rs_state = np.zeros(self._get_robot_server_state_len()) # Set initial robot joint positions if initial_joint_positions: assert len(initial_joint_positions) == 6 ur10_initial_joint_positions = initial_joint_positions else: ur10_initial_joint_positions = self._get_initial_joint_positions() rs_state[6:12] = self.ur10._ur_10_joint_list_to_ros_joint_list( ur10_initial_joint_positions) # Set target End Effector pose if ee_target_pose: assert len(ee_target_pose) == 6 else: ee_target_pose = self._get_target_pose() rs_state[0:6] = ee_target_pose # Set initial state of the Robot Server state_msg = robot_server_pb2.State(state=rs_state.tolist()) if not self.client.set_state_msg(state_msg): raise RobotServerError("set_state") # Get Robot Server state rs_state = copy.deepcopy( np.nan_to_num(np.array(self.client.get_state_msg().state))) # Check if the length of the Robot Server state received is correct if not len(rs_state) == self._get_robot_server_state_len(): raise InvalidStateError( "Robot Server state received has wrong length") # Convert the initial state from Robot Server format to environment format self.state = self._robot_server_state_to_env_state(rs_state) # Check if the environment state is contained in the observation space if not self.observation_space.contains(self.state): raise InvalidStateError() # go one empty action and check if there is a collision action = self.state[3:3 + len(self.action_space.sample())] _, _, done, _ = self.step(action) self.elapsed_steps = 0 if done: raise InvalidStateError('Reset started in a collision state') return self.state