def _get_obs(self):
robot_qpos, robot_qvel = robot_get_obs(self.sim)
object_qvel = self.sim.data.get_joint_qvel('object:joint')
achieved_goal = self._get_achieved_goal().ravel(
) # this contains the object position + rotation
if self.include_full_object:
# dimension: 24:24:6:7
observation = np.concatenate(
[robot_qpos, robot_qvel, object_qvel, achieved_goal])
elif self.include_object_rot:
observation = np.concatenate(
[robot_qpos, robot_qvel, object_qvel, achieved_goal[-4:]])
elif self.include_object_loc:
observation = np.concatenate(
[robot_qpos, robot_qvel, object_qvel, achieved_goal[:-4]])
else:
observation = np.concatenate([robot_qpos, robot_qvel, object_qvel])
if self.include_contact:
sim = self.sim
num_of_contacts = sim.data.ncon
total_num_of_geoms = sim.model.ngeom
contacts = [sim.data.contact[i] for i in range(num_of_contacts)]
geom1s = [contact.geom1 for contact in contacts]
geom2s = [contact.geom2 for contact in contacts]
geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s]
geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s]
geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s]
positions = [contact.pos for contact in contacts]
forces = [
sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys
]
processed, valid_contact_num = process_data(
total_num_of_geoms,
num_of_contacts,
geom1s,
geom2s,
geom1_names,
geom2_names,
positions,
forces,
fixed_num_of_contact=self.fixed_num_of_contact,
object_indices=self.object_indices,
include_geoms=self.include_geoms,
include_force=self.include_force,
include_position=self.include_position)
self.contact_num = valid_contact_num
if self.permute:
np.random.shuffle(processed)
processed = processed.reshape((-1))
observation = np.concatenate((processed, observation), -1)
return {
'observation': observation.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.ravel().copy(),
}
def _get_obs(self):
robot_qpos, robot_qvel = robot_get_obs(self.sim)
object_qvel = self.sim.data.get_joint_qvel('object:joint')
achieved_goal = self._get_achieved_goal().ravel(
) # this contains the object position + rotation
observation = np.concatenate(
[robot_qpos, robot_qvel, object_qvel, achieved_goal])
# # dimension: 24:24:6:7
# sim = self.sim
# ngeom = sim.model.ngeom
# for idx in range(ngeom):
# print(idx, sim.model.geom_id2name(idx))
if self.obs_type == 'object':
observation = observation[48:]
elif self.obs_type == 'original':
pass
else:
sim = self.sim
num_of_contacts = sim.data.ncon
total_num_of_geoms = sim.model.ngeom
contacts = [sim.data.contact[i] for i in range(num_of_contacts)]
geom1s = [contact.geom1 for contact in contacts]
geom2s = [contact.geom2 for contact in contacts]
geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s]
geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s]
geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s]
positions = [contact.pos for contact in contacts]
forces = [
sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys
]
processed, valid_contact_num = process_data(
total_num_of_geoms,
num_of_contacts,
geom1s,
geom2s,
geom1_names,
geom2_names,
positions,
forces,
fixed_num_of_contact=self.fixed_num_of_contact,
object_indices=self.object_indices,
hand_geoms=self.hand_geoms)
self.contact_num = valid_contact_num
processed = processed.reshape((-1))
self.num_of_contacts = valid_contact_num
if self.obs_type == 'contact':
observation = np.concatenate(
(processed, object_qvel, achieved_goal), -1)
elif self.obs_type == 'full_contact':
observation = np.concatenate((processed, observation), -1)
elif self.obs_type == 'joint_contact':
observation = np.concatenate(
(processed, robot_qpos, robot_qvel), -1)
return {
'observation': observation.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.ravel().copy(),
}
def _get_obs(self):
# positions
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
if self.has_object:
object_pos = self.sim.data.get_site_xpos('object0')
# rotations
object_rot = rotations.mat2euler(
self.sim.data.get_site_xmat('object0'))
# velocities
object_velp = self.sim.data.get_site_xvelp('object0') * dt
object_velr = self.sim.data.get_site_xvelr('object0') * dt
# gripper state
object_rel_pos = object_pos - grip_pos
object_velp -= grip_velp
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(
0)
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[
-2:] * dt # change to a scalar if the gripper is made symmetric
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = np.squeeze(object_pos.copy())
obs = np.concatenate([
grip_pos,
object_pos.ravel(),
object_rel_pos.ravel(),
gripper_state,
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
grip_velp,
gripper_vel,
])
st()
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
robot_qpos, robot_qvel = robot_get_obs(self.sim)
achieved_goal = self._get_achieved_goal().ravel()
observation = np.concatenate([robot_qpos, robot_qvel, achieved_goal])
if self.obs_type == 'object':
observation = achieved_goal
elif self.obs_type == 'contact' or self.obs_type == 'full_contact':
sim = self.sim
num_of_contacts = sim.data.ncon
total_num_of_geoms = sim.model.ngeom
contacts = [sim.data.contact[i] for i in range(num_of_contacts)]
geom1s = [contact.geom1 for contact in contacts]
geom2s = [contact.geom2 for contact in contacts]
geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s]
geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s]
geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s]
positions = [contact.pos for contact in contacts]
forces = [
sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys
]
processed = utils.process_data(
total_num_of_geoms,
num_of_contacts,
geom1s,
geom2s,
geom1_names,
geom2_names,
positions,
forces,
fixed_num_of_contact=self.fixed_num_of_contact)
processed = processed.reshape((-1))
observation = np.concatenate((processed, achieved_goal), -1)
return {
'observation': observation.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}