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, {}
def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, Dict[str, bool]]:
"""Performs simulation step by applying given action.
Args:
action: A desired motors velocities.
Returns:
state: The sensors readings, polar coordinates, linear and angular velocities.
reward: The reward received in current simulation step for state-action pair
done: Flag if environment is finished.
info: Dictionary containing diagnostic information.
Raises:
ValueError: Dimension of input motors velocities is incorrect!
"""
self._robot.set_motor_velocities(action)
vrep.simxSynchronousTrigger(self._client)
vrep.simxGetPingTime(self._client)
state = self._get_observation()
reward, done, info = self._compute_reward(state)
return state, reward, done, info