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)
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = self.sim.data.get_joint_qpos('object0:joint')
obs = grip_pos
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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)
object0_pos = self.sim.data.get_site_xpos('object0')
object1_pos = self.sim.data.get_site_xpos('object1')
# gripper state
object0_rel_pos = object0_pos - grip_pos
object1_rel_pos = object1_pos - grip_pos
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[
-2:] * dt # change to a scalar if the gripper is made symmetric
obs = np.concatenate([
self.goal - object0_pos, self.goal - object1_pos, object0_rel_pos,
object1_rel_pos, self.object0_color, self.object1_color,
gripper_state, gripper_vel
])
return {
'obs': obs.copy(),
'obs_task_params': self.goal_color_center.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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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)
# gripper state
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
obs_stack = grip_pos, gripper_vel, gripper_state, grip_velp,
for obj_key in self.obs_keys or self.obj_keys:
obj_key, *attrs = obj_key.split('@')
obs_dict = dict(
pos=self.sim.data.get_site_xpos(obj_key),
rot=rotations.mat2euler(self.sim.data.get_site_xmat(obj_key)),
rel_vel=self.sim.data.get_site_xvelp(obj_key) * dt - grip_velp,
vel_rot=self.sim.data.get_site_xvelr(obj_key) * dt,
)
obs_dict['rel_pos'] = obs_dict['pos'] - grip_pos
obs_stack += tuple([obs_dict[k] for k in attrs or obs_dict.keys()])
achieved_goal = self.sim.data.get_site_xpos(self.goal_key)
return {
'observation': np.concatenate(obs_stack).copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# TODO: set position that has to reach target (object or on body), set to achieved goal
# here: body_pos is the position that has to reach target
# positions
body_pos = self.sim.data.get_body_xpos(self.finder_name)
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
body_velp = self.sim.data.get_body_xvelp(self.finder_name) * dt
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
# since no object
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
gripper_state = gripper_vel = np.zeros(0)
# since no object
achieved_goal = body_pos.copy()
obs = np.concatenate([
body_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),
object_velp.ravel(), object_velr.ravel(), body_velp, gripper_vel,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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,
])
obs_noobj = np.concatenate([
grip_pos, gripper_state, grip_velp, gripper_vel, self.goal.copy()
])
success = np.array(self._is_success(achieved_goal, self.goal))
state = np.concatenate([obs.copy(), self.goal.copy()])
real_world = 1.0 if self.real_world else 0.0
if self.use_vision:
img_size = 64
img = self.sim.render(width=img_size, height=img_size, camera_name="external_camera_0")[::-1]
state = {
"state": obs_noobj,
"pixels": img,
'observation': state,
'grip_pos': grip_pos.copy(),
'obj_pos': object_pos.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
'success': success,
'real_world': np.array(real_world),
}
else:
state = {
"state" : state,
'observation': state,
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
'success': success,
'real_world': np.array(real_world),
}
return state
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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _move_gripper_to(self, position):
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
'''print(grip_pos)
print(robot_qpos)
print(robot_qvel)
print('____________________________________________')'''
first_over = np.array([grip_pos[0], grip_pos[1], position[2] + 1.])
to_x_y = np.array([position[0], position[1], position[2] + 1.])
gripper_target = np.array(
[position[0], position[1],
position[2]]) #+ self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
#move
self.sim.data.set_mocap_pos('robot0:mocap', first_over)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
self.sim.forward()
self.sim.data.set_mocap_pos('robot0:mocap', to_x_y)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
self.sim.forward()
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
self.sim.forward()
'''grip_pos = self.sim.data.get_site_xpos('robot0:grip')
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)
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[
-2:] * dt # change to a scalar if the gripper is made symmetric
achieved_goal = grip_pos.copy()
state = np.concatenate([grip_pos, grip_velp])
# pixel = self.render(mode="rgb_array")
return {
"observation": state.copy(),
"achieved_goal": np.zeros((0, )),
"desired_goal": np.zeros((0, )),
# "_pixel": pixel.copy(),
"_state": state.copy(),
"_achieved_goal": achieved_goal.copy(),
"_desired_goal": self.goal.copy(),
}
def _get_obs(self):
robot_qpos, robot_qvel = robot_get_obs(self.sim)
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
forearm_pose = self._get_body_pose('robot0:forearm', euler=True)
forearm_velp = self.sim.data.get_body_xvelp('robot0:forearm') * dt
palm_pos = self._get_palm_pose(no_rot=True)[:3]
object_pose = np.zeros(0)
object_vel = np.zeros(0)
object_rel_pos = np.zeros(0)
if self.has_object:
object_vel = self.sim.data.get_joint_qvel('object:joint')
object_pose = self._get_body_pose('object', euler=True)
object_rel_pos = object_pose[:3] - palm_pos
observation = np.concatenate([
forearm_pose, forearm_velp, palm_pos, object_rel_pos, robot_qpos,
robot_qvel, object_pose, object_vel
])
return {
'observation': observation,
'achieved_goal': self._get_achieved_goal().ravel(),
'desired_goal': self.goal.ravel().copy(),
}
def _get_obs(self):
robot_qpos, robot_qvel = robot_get_obs(self.sim)
object_qpos = self.sim.data.get_joint_qpos('object:joint')
object_qvel = self.sim.data.get_joint_qvel('object:joint')
#target_qpos, target_qvel # TODO: Fix this
observation = np.concatenate(
[robot_qpos, robot_qvel, object_qpos, object_qvel])
return {'observation': observation.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,
# ])
if self.obs_with_time:
obs = np.concatenate([
object_pos[:2], grip_pos[:2], object_pos[2:3], grip_pos[2:3],
object_rel_pos.ravel(), gripper_state,
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(), grip_velp, gripper_vel,
np.array([self.timestep / self.episode_len])
])
else:
obs = np.concatenate([
object_pos[:2],
grip_pos[:2],
object_pos[2:3],
grip_pos[2:3],
object_rel_pos.ravel(),
gripper_state,
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
grip_velp,
gripper_vel,
])
return obs.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])
return {
'observation': observation.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])
return {
'observation': observation.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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])
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])
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)
object_pos, object_rot, object_velp, object_velr, object_rel_pos = [], [], [], [], []
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
num_blocks = self.num_objs - 2
obs = np.concatenate([[num_blocks], grip_pos, gripper_state, grip_velp, gripper_vel])
block_features = None
block_indices = list(range(num_blocks))
# np.random.shuffle(block_indices)
for i in block_indices:
obj_name = 'object{}'.format(i)
temp_pos = self.sim.data.get_site_xpos(obj_name)
# rotations
temp_rot = rotations.mat2euler(self.sim.data.get_site_xmat(obj_name))
# velocities
temp_velp = self.sim.data.get_site_xvelp(obj_name) * dt
temp_velr = self.sim.data.get_site_xvelr(obj_name) * dt
# gripper state
temp_rel_pos = temp_pos - grip_pos
temp_velp -= grip_velp
block_obs = np.concatenate([temp_pos.ravel(),
temp_rel_pos.ravel(), temp_rot.ravel(),
temp_velp.ravel(), temp_velr.ravel(),
one_hot_color(self.obj_colors[i+2])
])
block_features = block_obs.shape[0]
obs = np.concatenate([obs, block_obs])
padding = np.zeros(block_features * (self.max_num_blocks - num_blocks))
obs = np.concatenate([obs, padding])
assert(self._check_goal())
achieved_goal = self.achieved_goal.copy().ravel()
max_goal_len = (self.max_num_blocks + 2) ** 2
achieved_goal = np.append(achieved_goal,
np.zeros(max_goal_len - achieved_goal.shape[0]))
desired_goal = np.append(self.goal.copy(),
np.zeros(max_goal_len - self.goal.shape[0]))
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': desired_goal.copy(),
}
def _get_obs(self):
# positions
# grip_pos - Position of the gripper given in 3 positional elements and 4 rotational elements
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
# grip_velp - The velocity of gripper moving
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 - Position of the object with respect to the world frame
object_pos = self.sim.data.get_site_xpos('object0')
# rotations object_rot - Yes. That is the orientation of the object with respect to world frame.
object_rot = rotations.mat2euler(
self.sim.data.get_site_xmat('object0'))
# velocities object_velp - Positional velocity of the object with respect to the world frame
object_velp = self.sim.data.get_site_xvelp('object0') * dt
# object_velr - Rotational velocity of the object with respect to the world frame
object_velr = self.sim.data.get_site_xvelr('object0') * dt
# gripper state
# object_rel_pos - Position of the object relative to the gripper
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 - No. It's not 0/1 signal, it is a float value and varies from 0 to 0.2
# for fetch robot. This varied gripper state helps in grasping different sized
# object with different strengths.
# gripper_state - The quantity to measure the opening of gripper
gripper_state = robot_qpos[-2:]
# gripper_vel - The velocity of gripper opening/closing
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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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())
if self.observation_type == "image":
#can parameterize kwargs for the rendering options later
# obs = np.transpose(self.render("rgb_array", 64, 64), (2,0,1))
# temporary send 1d, which will be reshaped by model.
# obs = self.render("rgb_array", 64, 64).flatten()
obs = np.transpose(self.render("rgb_array", 64, 64),
(2, 0, 1)).flatten()
else:
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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
'''
Gets the observation of the previous action
____________
Returns:
dictionary containing:
observation
achieved_goal
desired_goal
'''
grip_pos = self.sim.data.get_site_xpos('robot0:gripper')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('robot0:gripper') * 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(
3)
gripper_state = robot_qpos[-2:]
# Change to a scalar if the gripper is symmetric
gripper_vel = robot_qvel[-2:] * dt
if not self.has_object:
achieved_goal = np.squeeze(
self.sim.data.get_site_xpos('robot0:gripper').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
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy()
}
def _get_obs_gripper_at(env, position):
gripper_target = np.array([position[0], position[1], position[2]])
# positions
grip_pos = env.env.env.sim.data.get_site_xpos('robot0:grip')
dt = env.env.env.sim.nsubsteps * env.env.env.sim.model.opt.timestep
grip_velp = env.env.env.sim.data.get_site_xvelp('robot0:grip') * dt
robot_qpos, robot_qvel = utils.robot_get_obs(env.env.env.sim)
if env.env.env.has_object:
object_pos = env.env.env.sim.data.get_site_xpos('object0')
# rotations
object_rot = rotations.mat2euler(
env.env.env.sim.data.get_site_xmat('object0'))
# velocities
object_velp = env.env.env.sim.data.get_site_xvelp('object0') * dt
object_velr = env.env.env.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 env.env.env.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,
])
# env.env.env._get_image('obs.png')
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': env.env.env.goal.copy(),
}
def _get_obs(self):
# TODO: set position that has to reach target (object or on body), set to achieved goal
# here: body_pos is the position that has to reach target
# positions
grip_pos = self.sim.data.get_body_xpos(self.finder_name)
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_body_xvelp(self.finder_name) * dt
# for the following to work, all joints associated with the robot have to start with "robot"
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
# since 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
gripper_state = robot_qpos[-1:]
gripper_vel = robot_qvel[
-1:] * dt # changed to a scalar since the gripper is made symmetric [-3:] else
achieved_goal = np.squeeze(object_pos.copy())
#if not object_pos[2] == 0.: # TODO: had changes in her/rollout.py (see todo)
# achieved_goal = np.zeros_like(object_pos.copy())
# achieved_goal.fill(np.nan)
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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
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)
#gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
obs = np.concatenate([grip_pos, grip_velp, gripper_vel])
for g in self.goals:
goal = [g.position[0], g.position[1], g.position[2], g.reached]
obs = np.concatenate([obs, goal])
# print('Calculated observation at the end %s' % obs)
#print('Shape %s' % obs.shape)
return obs.copy()
def _get_obs(self):
# proprioception
robot_qpos, robot_qvel = robot_get_obs(self.sim)
# touch sensor information
touch = self.sim.data.sensordata[self._touch_sensor_id]
achieved_goal = self._get_achieved_goal().ravel()
observation = np.concatenate(
[robot_qpos, robot_qvel, touch, achieved_goal,
self.goal.copy()])
return {
'observation': observation.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# images
img = self.sim.render(width=400, height=400, camera_name="external_camera_1")
# positions
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
holder_pos = grip_pos.copy()
grip_pos += self.obs_noise_vector
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 = grip_pos
object_rot = np.zeros(3)
object_velp = grip_velp
object_velr = np.zeros(3)
object_rel_pos = np.zeros(3)
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()# - self.sim.data.get_site_xpos("robot0:cam")
else:
achieved_goal = np.squeeze(object_pos.copy())# - self.sim.data.get_site_xpos("robot0:cam")
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,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
'image':(img/255).copy(),
'gripper_pose': holder_pos.copy()
}
def _get_obs(self):
palm = [self.sim.data.sensordata[-19], self.sim.data.sensordata[-18]]
fingers = np.take(self.sim.data.sensordata, [-1, -4, -7, -10, -13])
robot_qpos, robot_qvel = robot_get_obs(self.sim)
robot_qpos = np.delete(robot_qpos,
[0, 1]) # ignore fixed joints: Wrist UDEV+PRO
robot_qvel = np.delete(robot_qvel, [0, 1])
#object_qvel = self.sim.data.get_joint_qvel(self.target_body)
#object_pos = self._get_achieved_qpos().ravel()[:3] # this contains the object position + rotation
#mocap_pos = self.sim.data.mocap_pos.ravel()
#delta = object_pos - mocap_pos
observation = np.concatenate([
palm, fingers, robot_qpos, robot_qvel
]) #, np.zeros(delta.size), np.zeros(object_qvel.size)])
observation += self.np_random.normal(size=observation.size,
scale=0.005)
return {'observation': observation.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)
object_pos, object_rot, object_velp, object_velr, object_rel_pos = [], [], [], [], []
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
obs = []
for i in range(self.num_objs-2):
obj_name = 'object{}'.format(i)
temp_pos = self.sim.data.get_site_xpos(obj_name)
# rotations
temp_rot = rotations.mat2euler(self.sim.data.get_site_xmat(obj_name))
# velocities
temp_velp = self.sim.data.get_site_xvelp(obj_name) * dt
temp_velr = self.sim.data.get_site_xvelr(obj_name) * dt
# gripper state
temp_rel_pos = temp_pos - grip_pos
temp_velp -= grip_velp
block_obs = np.concatenate([temp_pos.ravel(),
temp_rel_pos.ravel(), temp_rot.ravel(),
temp_velp.ravel(), temp_velr.ravel()])
obs = np.concatenate([obs, block_obs])
assert(self._check_goal())
achieved_goal = self.achieved_goal.copy().ravel()
obs = np.concatenate([grip_pos, gripper_state, grip_velp, gripper_vel, obs])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.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
# 得到机器人的位置和速度非常简单，只需要调用位于utils中的robot_get_obs方法即可
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
if self.has_object:
object_list = [1]*self.n_objects
for i in range(self.n_objects):
object_pos = self.sim.data.get_site_xpos('object0')
# rotations
# Convert Rotation Matrix to Euler Angles
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
object_list[i] = np.concatenate([object_pos, object_rot, object_velp, object_velr, object_rel_pos, object_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
obs = np.concatenate([
grip_pos, gripper_state, grip_velp, gripper_vel, object_list[0],object_list[1],object_list[2]
])
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = self._get_goal(obs,self.desired_shape)
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# positions
#print("get_obs")
grip_pos = self.sim.data.get_site_xpos('robot0:grip') #end_effector position
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) #Returns all joint positions and velocities associated with a robot.
if self.has_object: ## has_object=false
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: ##has_object=false
achieved_goal = grip_pos.copy() #end_effector postion
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,
])
# print('grip_pos:'+str(grip_pos))
# print('gripper_state'+str(gripper_state))
# print('grip_velp'+str(grip_velp))
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# images
# grip_pos = self.sim.data.get_site_xpos('robot0:grip')
#
# img = self.sim.render(width=512, height=512, camera_name="external_camera_1")
#
# # camera position and quaternion
# cam_pos = self.sim.model.cam_pos[4].copy()
# cam_quat = self.sim.model.cam_quat[4].copy()
#
# 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
#
# achieved_goal = np.squeeze(object_pos.copy())# - self.sim.data.get_site_xpos("robot0:cam")
# obs = np.concatenate([
# cam_pos, cam_quat
# ])
# obs += self.obs_noise_vector
#
# return {
# 'observation': obs.copy(),
# 'achieved_goal': achieved_goal.copy(),
# 'desired_goal': self.goal.copy(),
# 'image':(img/255).copy(),
# 'gripper_pose': grip_pos.copy()
# }
# images
if self.depth:
img = self.sim.render(width=224,
height=224,
camera_name="external_camera_1",
depth=True)[1]
else:
if self.two_cam:
img = self.sim.render(width=224,
height=224,
camera_name="external_camera_2") / 255
# normalize by imagenet parameters
img = (img - np.array([0.485, 0.456, 0.406])) / np.array(
[0.229, 0.224, 0.225])
# second image
img2 = self.sim.render(width=224,
height=224,
camera_name="external_camera_3") / 255
# normalize by imagenet parameters
img2 = (img2 - np.array([0.485, 0.456, 0.406])) / np.array(
[0.229, 0.224, 0.225])
img = np.concatenate([img, img2], axis=-1)
else:
img = self.sim.render(width=224,
height=224,
camera_name="external_camera_1") / 255
# normalize by imagenet parameters
img = (img - np.array([0.485, 0.456, 0.406])) / np.array(
[0.229, 0.224, 0.225])
# positions
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
holder_pos = grip_pos.copy()
# 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)
object_pos = self.sim.data.get_site_xpos('object0')
# obj rotations
rot_mat = np.reshape(self.sim.data.site_xmat[3], (3, 3))
v = np.zeros(3)
v[np.argmax(self.sim.model.site_size[-1])] = 1
v = np.matmul(rot_mat, v)
v[2] = 0
obj_radius = (math.atan2(v[0], v[1]) +
math.pi) % (math.pi) - math.pi / 2
if obj_radius > math.pi / 2:
obj_radius = (obj_radius - math.pi)
# gripper rotations
grip_mat = rotations.quat2mat(self.sim.data.mocap_quat)
grip_v = np.squeeze(np.matmul(grip_mat, np.array([0, 1, 0])))
grip_radius = (math.atan2(grip_v[0], grip_v[1]) +
2 * math.pi) % (2 * math.pi)
if grip_radius > math.pi:
grip_radius = (grip_radius - 2 * math.pi)
# 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
#
# gripper_state = robot_qpos[-2:]
# gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
counter = np.array([self.counter])
achieved_goal = np.concatenate(
[np.squeeze(object_pos.copy()), [obj_radius]])
holder_pos = np.concatenate([np.squeeze(holder_pos), [grip_radius]])
# 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, counter
# ])
# obs = np.concatenate([
# grip_pos, gripper_state, grip_velp, gripper_vel, counter
# ])
if self.use_task_index:
obs = np.concatenate([counter, [0, 1, 0]])
else:
# obs = np.concatenate([
# counter, self.cam_offset.copy()
# ])
obs = counter
# obs = np.empty(0)
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
'image': img.copy(),
'gripper_pose': holder_pos.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') # + 0.02 * (np.random.random(3) - 0.5)
# 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
# if np.linalg.norm(object_rel_pos) < 0.03: self.sim.data.qvel[self.sim.model.joint_name2id('object0:joint')+1] += np.random.random()*1.0-0.5
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
goal_rel_pos = self.goal.copy()[:3] - object_pos
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = np.squeeze(object_pos.copy())
if self.fragile_on:
# if self.sim.data.sensordata[2] > 100 and self.broken_table == False:
# self.sim.model.geom_matid[self.sim.model.body_geomadr[self.sim.model.body_name2id('table0')]] = 2
# self.broken_table = True
l_finger_force = (
self.sim.data.ctrl[0] -
self.sim.data.sensordata[self.sim.model.sensor_name2id(
'l_finger_jnt')]) * self.sim.model.actuator_gainprm[
self.sim.model.
actuator_name2id('robot0:l_gripper_finger_joint'), 0]
r_finger_force = (
self.sim.data.ctrl[1] -
self.sim.data.sensordata[self.sim.model.sensor_name2id(
'r_finger_jnt')]) * self.sim.model.actuator_gainprm[
self.sim.model.
actuator_name2id('robot0:r_gripper_finger_joint'), 0]
l_finger_force = self.prev_lforce + (l_finger_force -
self.prev_lforce) * dt / 0.05
r_finger_force = self.prev_rforce + (r_finger_force -
self.prev_rforce) * dt / 0.05
object_force = self.prev_oforce + (self.sim.data.sensordata[
self.sim.model.sensor_name2id('object_frc')] -
self.prev_oforce) * dt / 0.1
if (object_force > self.object_fragility):
self.sim.model.geom_matid[self.sim.model.body_geomadr[
self.sim.model.body_name2id('object0')]] = 4
self.broken_object = 1.0
elif object_force > 0.0:
self.sim.model.geom_matid[self.sim.model.body_geomadr[
self.sim.model.body_name2id('object0')]] = 3
self.broken_object = 0.0
# else:
# self.sim.model.geom_matid[self.sim.model.body_geomadr[self.sim.model.body_name2id('object0')]] = 3
# self.broken_object = 2.0
conc_stiffness_data = np.concatenate([[
l_finger_force, r_finger_force, self.prev_stiffness
]]) / 1000.0
# print("Fragility:{}, minimum force:{}, grasping force:{}".format(self.object_fragility, self.min_grip*2, l_finger_force+r_finger_force))
# 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, goal_rel_pos.ravel(), conc_stiffness_data
# ])
obs = np.concatenate([
grip_pos,
object_rel_pos.ravel(), gripper_state,
goal_rel_pos.ravel(), conc_stiffness_data
])
achieved_goal = np.concatenate(
[achieved_goal, conc_stiffness_data[:2]])
self.prev_lforce = l_finger_force
self.prev_rforce = r_finger_force
self.prev_oforce = object_force
else:
# 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, goal_rel_pos.ravel()
# ])
obs = np.concatenate([
grip_pos,
object_rel_pos.ravel(), gripper_state,
goal_rel_pos.ravel()
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# add noise to distractor objects positions
object0_qpos = self.sim.data.get_joint_qpos('object0:joint')
object1_qpos = self.sim.data.get_joint_qpos('object1:joint')
object2_qpos = self.sim.data.get_joint_qpos('object2:joint')
tmp = self.object3_xpos[:2].copy() + np.random.randn(2) * 0.005
i=1
while la.norm(tmp - object1_qpos[:2]) < 0.05 or la.norm(tmp - object0_qpos[:2]) < 0.05 or la.norm(tmp - object2_qpos[:2]) < 0.05 or la.norm(tmp - self.object4_xpos[:2]) < \
0.05:
tmp = self.object3_xpos[:2].copy() + np.random.randn(2) * 0.005
i += 1
if i == 100:
tmp = self.object3_xpos[:2].copy()
break
self.object3_xpos[:2] = tmp.copy()
tmp = self.object4_xpos[:2].copy() + np.random.randn(2) * 0.005
i = 1
while la.norm(tmp - object1_qpos[:2]) < 0.05 or la.norm(tmp - object0_qpos[:2]) < 0.05 or la.norm(tmp - object2_qpos[:2]) < 0.05 or la.norm(tmp - self.object3_xpos[:2]) < \
0.05:
tmp = self.object4_xpos[:2].copy() + np.random.randn(2) * 0.005
i += 1
if i == 100:
tmp = self.object4_xpos[:2].copy()
break
self.object4_xpos[:2] = tmp.copy()
# for video
object3_qpos = self.sim.data.get_joint_qpos('object3:joint')
object4_qpos = self.sim.data.get_joint_qpos('object4:joint')
object3_qpos[:3] = self.object3_xpos
object4_qpos[:3] = self.object4_xpos
self.sim.data.set_joint_qpos('object3:joint', object3_qpos)
self.sim.data.set_joint_qpos('object4:joint', object4_qpos)
# 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)
objects_pos = []
objects_rot = []
objects_velp = []
objects_velr = []
objects_rel_pos = []
if self.has_object:
# object 0
object0_pos = self.sim.data.get_site_xpos('object0')
# rotations
object0_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0'))
# velocities
object0_velp = self.sim.data.get_site_xvelp('object0') * dt
object0_velr = self.sim.data.get_site_xvelr('object0') * dt
# gripper state
object0_rel_pos = object0_pos - grip_pos
object0_velp -= grip_velp
# object 1
object1_pos = self.sim.data.get_site_xpos('object1')
# rotations
object1_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object1'))
# velocities
object1_velp = self.sim.data.get_site_xvelp('object1') * dt
object1_velr = self.sim.data.get_site_xvelr('object1') * dt
# gripper state
object1_rel_pos = object1_pos - grip_pos
object1_velp -= grip_velp
# object 2
object2_pos = self.sim.data.get_site_xpos('object2')
if self.bias:
object2_pos[0] += 0.05
# rotations
object2_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object2'))
# velocities
object2_velp = self.sim.data.get_site_xvelp('object2') * dt
object2_velr = self.sim.data.get_site_xvelr('object2') * dt
# gripper state
object2_rel_pos = object2_pos - grip_pos
object2_velp -= grip_velp
else:
object0_pos = object0_rot = object0_velp = object0_velr = object0_rel_pos = np.zeros(0)
object1_pos = object1_rot = object1_velp = object1_velr = object1_rel_pos = np.zeros(0)
object2_pos = object2_rot = object2_velp = object2_velr = object2_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 self.n_tasks == 7:
# obs = np.concatenate([grip_pos,
# object0_pos.ravel(), object1_pos.ravel(), object2_pos.ravel(), self.object3_xpos.squeeze(), self.object4_xpos.squeeze(),
# object0_rel_pos.ravel(), object1_rel_pos.ravel(), object2_rel_pos.ravel(),
# object0_rot.ravel(), object1_rot.ravel(), object2_rot.ravel(),
# object0_velp.ravel(), object1_velp.ravel(), object2_velp.ravel(),
# object0_velr.ravel(), object1_velr.ravel(), object2_velr.ravel(),
# grip_velp, gripper_vel, gripper_state])
# else:
obs = np.concatenate([grip_pos,
object0_pos.ravel(), object1_pos.ravel(), object2_pos.ravel(),
object0_rel_pos.ravel(), object1_rel_pos.ravel(), object2_rel_pos.ravel(),
object0_rot.ravel(), object1_rot.ravel(), object2_rot.ravel(),
object0_velp.ravel(), object1_velp.ravel(), object2_velp.ravel(),
object0_velr.ravel(), object1_velr.ravel(), object2_velr.ravel(),
grip_velp, gripper_vel, gripper_state])
self.last_obs = obs.copy()
self._update_goals(obs)
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = self._compute_achieved_goal(obs.copy())
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
'mask': self.mask
}
def _get_obs(self):
""" returns the observations dict """
# positions
# grip_pos = self.sim.data.get_body_xpos('robot1:ee_link')
# dt = self.sim.nsubsteps * self.sim.model.opt.timestep
# grip_velp = self.sim.data.get_body_xvelp('robot1:ee_link') * dt
grip_pos = self.sim.data.get_body_xpos('gripper_central')
self.grip_pos = grip_pos
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_body_xvelp('gripper_central') * 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
elif self.has_cloth:
#get the positions and velocities for 4 corners of the cloth
vertices = ['CB0_0']
# Name vertices with respect to the cloth_length
vertices.append('CB'+str(self.cloth_length-1)+'_'+'0')
vertices.append('CB'+str(self.cloth_length-1)+'_'+str(self.cloth_length-1))
vertices.append('CB'+'0'+'_'+str(self.cloth_length-1))
vertice_pos, vertice_velp, vertice_velr, vertice_rel_pos = [], [], [], []
for vertice in vertices:
vertice_pos.append(self.sim.data.get_body_xpos(vertice))
vertice_velp.append(self.sim.data.get_body_xvelp(vertice) * dt)
#vertice_velr.append(self.sim.data.get_body_xvelr(vertice) * dt) #Do not need rotational velocities
vertice_rel_pos = vertice_pos.copy()
vertice_rel_pos -= grip_pos
vertice_velp -= grip_velp
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)
# if not using a fake gripper
# gripper_state = robot_qpos[-2:]
# gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
gripper_state = np.array([self.sim.model.eq_active[-1]])
# gripper_vel # Does not make sense for fake gripper
if not self.has_object and not self.has_cloth:
achieved_goal = grip_pos.copy()
elif self.has_cloth and not self.has_object:
if self.behavior=="diagonally":
achieved_goal = np.squeeze(vertice_pos[0].copy())
elif self.behavior=="sideways":
achieved_goal = np.concatenate([
vertice_pos[0].copy(), vertice_pos[1].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,
# ])
# obs = np.concatenate([
# grip_pos, gripper_state, grip_velp, gripper_vel, vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3],
# ])
# Creating dataset for Visual policy training
self._render_callback()
self.viewer = self._get_viewer('rgb_array')
HEIGHT, WIDTH = 256, 256
self.viewer.render(HEIGHT, WIDTH)
visual_data = self.viewer.read_pixels(HEIGHT, WIDTH, depth=False)
# original image is upside-down, so flip it
visual_data = cv2.UMat(visual_data[::-1, :, :])
# if self.visual_data_recording:
# name = "/home/rjangir/Pictures/kinova_dataset/" + str(vertice_pos[0])+ str(vertice_pos[1]) + ".jpg"
# cv2.imwrite(name, visual_data)
#Save vertice labels
vertice_pos_labels = [vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3]]
label = []
#convert these points to 2D
s = 1 # std for heatmap signal
output_size = [HEIGHT, WIDTH]
fov = 45 #field of view of camera
cam_name = 'camera1'
cam_pos = self.sim.data.get_camera_xpos(cam_name)
cam_ori = gym.envs.robotics.rotations.mat2euler(self.sim.data.get_camera_xmat(cam_name))
for v in vertice_pos_labels:
label.append(utils.global2label(v , cam_pos, cam_ori, output_size, fov=fov, s=s))
# for point in label:
# cv2.circle(visual_data, (int(point[0]), int(point[1])), 3, (0, 0, 255), 5)
if self.visual_data_recording:
name = "/home/rjangir/workSpace/IRI-DL/datasets/sim2real/train/" + "image_" +str(self._index) + ".jpg"
cv2.imwrite(name, visual_data)
label_data = np.array([label[0], label[1], label[2], label[3]])
self._label_matrix.append(label_data)
label_file = "/home/rjangir/workSpace/IRI-DL/datasets/sim2real/train/" + "train_ids" + ".npy"
if np.asarray(self._label_matrix).shape[0] == 1000:
print("saving the labels file")
np.save(label_file, np.asarray(self._label_matrix), allow_pickle=True )
obs = np.concatenate([
grip_pos, gripper_state, grip_velp, vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3], vertice_velp[0], vertice_velp[1], vertice_velp[2], vertice_velp[3],
])
if self.explicit_policy:
obs = np.concatenate([
obs, np.array(self.physical_params),
])
self._index += 1
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
