def get_additional_states(self):
"""
:return: non-perception observation, such as goal location
"""
additional_states = self.global_to_local(self.target_pos)[:2]
if self.goal_format == 'polar':
additional_states = np.array(
cartesian_to_polar(additional_states[0], additional_states[1]))
# linear velocity along the x-axis
linear_velocity = rotate_vector_3d(
self.robots[0].get_linear_velocity(), *self.robots[0].get_rpy())[0]
# angular velocity along the z-axis
angular_velocity = rotate_vector_3d(
self.robots[0].get_angular_velocity(),
*self.robots[0].get_rpy())[2]
additional_states = np.append(additional_states,
[linear_velocity, angular_velocity])
if self.config['task'] == 'reaching':
end_effector_pos_local = self.global_to_local(
self.robots[0].get_end_effector_position())
additional_states = np.append(additional_states,
end_effector_pos_local)
assert additional_states.shape[0] == self.additional_states_dim, \
'additional states dimension mismatch {} v.s. {}'.format(additional_states.shape[0], self.additional_states_dim)
return additional_states
def get_goal(self):
"""
:return: goal location
"""
goal = []
goal = self.global_to_local(self.target_pos)[:2]
if self.goal_format == 'polar':
goal = np.array(cartesian_to_polar(goal[0], goal[1]))
goal = np.append(goal, self.target_pos[2:])
assert goal.shape[0] == self.goal_dim, \
'goal state dimension mismatch {} v.s. {}'.format(goal.shape[0], self.goal_dim)
return goal
def get_additional_states(self):
"""
:return: non-perception observation, such as goal location
"""
additional_states = []
additional_states = self.global_to_local(self.target_pos)[:2]
if self.goal_format == 'polar':
additional_states = np.array(cartesian_to_polar(additional_states[0], additional_states[1]))
if self.config['task'] == 'reaching':
additional_states = np.append(additional_states, self.target_pos[2:])
#additional_states = []
# linear velocity along the x-axis
linear_velocity = rotate_vector_3d(self.robots[0].get_linear_velocity(),
*self.robots[0].get_rpy())[0]
# angular velocity along the z-axis
angular_velocity = rotate_vector_3d(self.robots[0].get_angular_velocity(),
*self.robots[0].get_rpy())[2]
additional_states = np.append(additional_states, [linear_velocity, angular_velocity])
if self.config['task'] == 'reaching':
# End-effector
end_effector_pos_local = self.global_to_local(self.robots[0].get_end_effector_position())
additional_states = np.append(additional_states, end_effector_pos_local)
# Height
#additional_states = np.append(additional_states, self.target_pos[2:])
# L2 distance between end-effector and goal
#additional_states = np.append(additional_states, self.get_l2_potential())
# Joint positions and velocities
self.robots[0].calc_state()
additional_states = np.append(additional_states, np.sin(self.robots[0].joint_position[2:]))
additional_states = np.append(additional_states, np.cos(self.robots[0].joint_position[2:]))
additional_states = np.append(additional_states, self.robots[0].joint_velocity[2:])
#additional_states = np.append(additional_states, self.robots[0].joint_torque)
assert additional_states.shape[0] == self.additional_states_dim, \
'additional states dimension mismatch {} v.s. {}'.format(additional_states.shape[0], self.additional_states_dim)
return additional_states
def get_task_obs(self, env):
"""
Get task-specific observation, including goal position, current velocities, etc.
:param env: environment instance
:return: task-specific observation
"""
task_obs = self.global_to_local(env, self.target_pos)[:2]
if self.goal_format == 'polar':
task_obs = np.array(cartesian_to_polar(task_obs[0], task_obs[1]))
# linear velocity along the x-axis
linear_velocity = rotate_vector_3d(env.robots[0].get_linear_velocity(),
*env.robots[0].get_rpy())[0]
# angular velocity along the z-axis
angular_velocity = rotate_vector_3d(
env.robots[0].get_angular_velocity(), *env.robots[0].get_rpy())[2]
task_obs = np.append(task_obs, [linear_velocity, angular_velocity])
return task_obs