def reset(self) -> Observation:
# TODO realtime reset
# Get the observation
observation = self.task.get_observation()
return Observation(observation)
def reset(self) -> Observation:
# Initialize gazebo if not yet done
gazebo = self.gazebo
assert gazebo, "Gazebo object not valid"
# Remove the robot and insert a new one
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
# Execute a dummy step to process model removal
self.gazebo.run()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
def reset(self) -> Observation:
# Initialize gazebo if not yet done
gazebo = self.gazebo
assert gazebo, "Gazebo object not valid"
# Remove the model and insert it again. This is the reset strategy for
# floating-base robots. Resetting the joint state, instead, is sufficient to
# reset fixed-based robots. Though, while avoiding the model deletion might
# provide better performance, we should be sure that all the internal buffers
# (e.g. those related to the low-level PIDs) are correctly re-initialized.
if self._hard_reset and self.task.has_robot():
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
def reset(self) -> Observation:
# Initialize pybullet if not yet done
p = self.pybullet
assert p, "PyBullet object not valid"
if self._hard_reset and self.task.has_robot():
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
def get_observation(self) -> Observation:
# Get the robot object
robot = self.robot
# Get the new pendulum position and velocity
theta = robot.joint_position("pivot")
theta_dot = robot.joint_velocity("pivot")
# Create the observation object
observation = Observation(
np.array([np.cos(theta), np.sin(theta), theta_dot]))
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get the model
model = self.world.get_model(self.model_name)
# Get the new joint positions and velocities
q, x = model.joint_positions(["pivot", "linear"])
dq, dx = model.joint_velocities(["pivot", "linear"])
# Create the observation
observation = Observation(np.array([x, dx, q, dq]))
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get the model
model = self.world.get_model(self.model_name)
# Get the new joint position and velocity
q = model.get_joint("pivot").position()
dq = model.get_joint("pivot").velocity()
# Create the observation
observation = Observation(np.array([np.cos(q), np.sin(q), dq]))
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get current end-effector and target positions
ee_position = self.get_ee_position()
target_position = self.get_target_position()
# Create the observation
observation = Observation(
np.concatenate([ee_position, target_position]))
if self._verbose:
print(f"\nobservation: {observation}")
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get the latest image
image = self.camera_sub.get_observation()
# Reshape and create the observation
color_image = np.array(image.data, dtype=np.uint8).reshape(self._camera_height,
self._camera_width, 3)
observation = Observation(color_image)
if self._verbose:
print(f"\nobservation: {observation}")
# Return the observation
return observation
def step(self, action: np.ndarray) -> State:
tau_m = action[0]
theta_ddot = (
self.m * self.g * self.L / 2.0 * np.sin(self.theta) + tau_m
) / self.I
# Integrate with euler.
# Note that in this way the ground truth used as comparison implements a very
# basic integration method, and the errors might be reduced implementing a
# better integrator.
self.theta_dot = self.theta_dot + theta_ddot * self.dt
self.theta = self.theta + self.theta_dot * self.dt
observation = np.array([np.cos(self.theta), np.sin(self.theta), self.theta_dot])
return State((Observation(observation), Reward(0.0), False, {}))
def get_observation(self) -> Observation:
# Get the robot object
robot = self.robot
# Get the new joint positions
x = robot.joint_position("linear")
theta = np.rad2deg(robot.joint_position("pivot"))
# Get the new joint velocities
x_dot = robot.joint_velocity("linear")
theta_dot = np.rad2deg(robot.joint_velocity("pivot"))
# Create the observation object
observation = Observation(np.array([x, x_dot, theta, theta_dot]))
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get the latest image
image = self.camera_sub.get_observation()
# Construct from buffer and reshape
depth_image = np.frombuffer(image.data, dtype=np.float32).reshape(
self._camera_height, self._camera_width, 1)
# Replace all instances of infinity with 0
depth_image[depth_image == np.inf] = 0.0
# Create the observation
observation = Observation(depth_image)
if self._verbose:
print(f"\nobservation: {observation}")
# Return the observation
return observation
def get_observation(self) -> Observation:
# Get the latest point cloud
point_cloud = self.camera_sub.get_observation()
# Contrust octree from this point cloud
octree = self.octree_creator(point_cloud).numpy()
# Pad octree with zeros to have a consistent length
# TODO: Customize replay buffer to support variable sized observations
octree_size = octree.shape[0]
if octree_size > self._octree_max_size:
print(f"ERROR: Octree is larger than the maximum "
f"allowed size (exceeded with {octree_size})")
octree = np.pad(octree, (0, self._octree_max_size - octree_size),
'constant',
constant_values=0)
# Write the original length into the padded octree for reference
octree[-4:] = np.ndarray(buffer=np.array([octree_size],
dtype='uint32').tobytes(),
shape=(4, ),
dtype='uint8')
# To get it back:
# octree_size = np.frombuffer(buffer=octree[-4:],
# dtype='uint32',
# count=1)
self.__stacked_octrees.append(octree)
# For the first buffer after reset, fill with identical observations until deque is full
while not self._octree_n_stacked == len(self.__stacked_octrees):
self.__stacked_octrees.append(octree)
# Create the observation
observation = Observation(np.array(self.__stacked_octrees))
if self._verbose:
print(f"\nobservation: {observation}")
# Return the observation
return observation
def reset(self) -> Observation:
# Get the observation
observation = self.task.get_observation()
return Observation(observation)
def get_observation(self) -> Observation:
# Get the latest point cloud
point_cloud = self.camera_sub.get_observation()
# Contrust octree from this point cloud
octree = self.octree_creator(point_cloud).numpy()
# Pad octree with zeros to have a consistent length
# TODO: Customize replay buffer to support variable sized observations
octree_size = octree.shape[0]
if octree_size > self._octree_max_size:
print(f"ERROR: Octree is larger than the maximum "
f"allowed size (exceeded with {octree_size})")
octree = np.pad(octree, (0, self._octree_max_size - octree_size),
'constant',
constant_values=0)
# Write the original length into the padded octree for reference
octree[-4:] = np.ndarray(buffer=np.array([octree_size],
dtype='uint32').tobytes(),
shape=(4, ),
dtype='uint8')
# To get it back:
# octree_size = np.frombuffer(buffer=octree[-4:],
# dtype='uint32',
# count=1)
if self._proprieceptive_observations:
# Add number of auxiliary observations to octree structure
octree[-8:-4] = np.ndarray(buffer=np.array(
[10], dtype='uint32').tobytes(),
shape=(4, ),
dtype='uint8')
# Gather proprioceptive observations
ee_position = self.get_ee_position()
ee_orientation = orientation_quat_to_6d(
quat_xyzw=self.get_ee_orientation())
aux_obs = (self._gripper_state,) + ee_position + \
ee_orientation[0] + ee_orientation[1]
# Add auxiliary observations into the octree structure
octree[-48:-8] = np.ndarray(buffer=np.array(
aux_obs, dtype='float32').tobytes(),
shape=(40, ),
dtype='uint8')
self.__stacked_octrees.append(octree)
# For the first buffer after reset, fill with identical observations until deque is full
while not self._octree_n_stacked == len(self.__stacked_octrees):
self.__stacked_octrees.append(octree)
# Create the observation
observation = Observation(np.array(self.__stacked_octrees))
if self._verbose:
print(f"\nobservation: {observation}")
# Return the observation
return observation
