def close(self) -> NoReturn: """A method that cleanly closes environment. """ vrep.simxStopSimulation(self._client, vrep.simx_opmode_blocking) vrep.simxCloseScene(self._client, vrep.simx_opmode_blocking) vrep.simxFinish(self._client) subprocess.call('pkill -9 vrep &', shell=True)
def reset(self): vrep.simxStopSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._robot.reset() vrep.simxStartSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._goal = self._goal_generator() for _ in range(2): vrep.simxSynchronousTrigger(self._client) vrep.simxGetPingTime(self._client) cart_pose = self._robot.get_position() self._navigation.reset(cart_pose) prox_dist = self._robot.get_proximity_values() gyro_data = self._robot.get_gyroscope_values() delta_phi = self._robot.get_encoders_rotations() self._navigation.compute_position(position=cart_pose, phi=delta_phi, anuglar_velocity=gyro_data[2]) polar_coords = self._navigation.polar_coordinates self._prev_polar_coords = polar_coords state = { 'observation': np.concatenate( (prox_dist, self._navigation.pose[:2])), 'desired_goal': self._goal, 'achieved_goal': self._navigation.pose[:2], } return state
def reset(self): vrep.simxStopSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._robot.reset() vrep.simxStartSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._env_steps = 0 for _ in range(2): vrep.simxSynchronousTrigger(self._client) vrep.simxGetPingTime(self._client) cart_pose = self._robot.get_position() self._navigation.reset(cart_pose) prox_dist = self._robot.get_proximity_values() gyro_data = self._robot.get_gyroscope_values() delta_phi = self._robot.get_encoders_rotations() self._navigation.compute_position(position=cart_pose, phi=delta_phi, anuglar_velocity=gyro_data[2]) polar_coords = self._navigation.polar_coordinates self._prev_polar_coords = polar_coords state = np.concatenate((prox_dist, polar_coords)) return state
def step(self, action): self._robot.set_motor_velocities(action) vrep.simxSynchronousTrigger(self._client) vrep.simxGetPingTime(self._client) cart_pose = self._robot.get_position() gyro_data = self._robot.get_gyroscope_values() delta_phi = self._robot.get_encoders_rotations() self._navigation.compute_position(position=cart_pose, phi=delta_phi, anuglar_velocity=gyro_data[2]) polar_coords = self._navigation.polar_coordinates prox_dist = self._robot.get_proximity_values() done = False info = { 'is_success': 0.0, } state = { 'observation': np.concatenate((prox_dist, self._navigation.pose[:2])).copy(), 'desired_goal': self._goal.copy(), 'achieved_goal': self._navigation.pose[:2].copy(), } reward = -1.0 if not np.all(prox_dist > self._collision_dist): reward = -1.0 done = True if polar_coords[0] < self._goal_threshold: reward = 1.0 info['is_success'] = 1.0 done = True if done: vrep.simxStopSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._prev_polar_coords = polar_coords return state, reward, done, info
def reset(self) -> np.ndarray: """Resets environment to initial state. Returns: state: The sensors readings, polar coordinates, linear and angular velocities. """ self._goal, start_pose = self._sample_start_parameters() print('Current goal: {}'.format(self._goal)) vrep.simxStopSimulation(self._client, vrep.simx_opmode_blocking) self._robot.reset() self._set_start_pose(self._robot._object_handlers['robot'], start_pose) self._navigation.reset(np.append(start_pose[0:3], start_pose[3:])) self._start() vrep.simxSynchronousTrigger(self._client) vrep.simxGetPingTime(self._client) state = self._get_observation() self._prev_distance = state[5] return state
def step(self, action): self._robot.set_motor_velocities(action) vrep.simxSynchronousTrigger(self._client) vrep.simxGetPingTime(self._client) cart_pose = self._robot.get_position() gyro_data = self._robot.get_gyroscope_values() delta_phi = self._robot.get_encoders_rotations() self._navigation.compute_position(position=cart_pose, phi=delta_phi, anuglar_velocity=gyro_data[2]) polar_coords = self._navigation.polar_coordinates prox_dist = self._robot.get_proximity_values() done = False state = np.concatenate((prox_dist, polar_coords)) ds = self._prev_polar_coords[0] - polar_coords[0] reward = ds * self._alpha_factor if not np.all(prox_dist > self._collision_dist): reward = -1.0 done = True if polar_coords[0] < self._goal_threshold: reward = 1.0 done = True if done: vrep.simxStopSimulation(self._client, vrep.simx_opmode_oneshot_wait) self._prev_polar_coords = polar_coords return state, reward, done, {}