def orientation_mat_symmetric_with_rot_plane(self, theta_a, rot_z_theta,
inv_rot_z_theta, i):
# Point 'a' orientation euler angle = theta_a
# euler to rot matrix for transform
v_r_a = r_tool.euler2mat(theta_a)
# transform to the O cordinate from S cordinate
v_r_a_hat = np.matmul(v_r_a, inv_rot_z_theta)
# Rot matrix to euler for sym
v_r_a_hat = r_tool.mat2euler(v_r_a_hat)
# Sym on transformed S's xoz plane (y axis)
v_r_a_hat[0 - i] = -v_r_a_hat[0 - i]
v_r_a_hat[2] = -v_r_a_hat[2]
# euler to rot matrix for transform
v_r_a_hat = r_tool.euler2mat(v_r_a_hat)
s_v_r_a = np.matmul(v_r_a_hat, rot_z_theta)
# Rot matrix to euler for return
s_v_r_a = r_tool.mat2euler(s_v_r_a)
#another method --- by ray
z_theta = r_tool.mat2euler(rot_z_theta)
inv_z_theta = -z_theta
theta_a = theta_a - z_theta
theta_a[0 - i] = -theta_a[0 - i]
theta_a[2] = -theta_a[2]
theta_a = theta_a - inv_z_theta
return s_v_r_a.copy()
def orientation_mat_symmetric_with_rot_plane(self, theta_a, rot_z_theta,
inv_rot_z_theta, i):
'''
Imagine an object that stands on the left side, but always touches and moves with the plane of symmetry. The plane
rotates from 0 to 90 degrees. The orientation of this object is the identity. Now think of the reflection.
At z=0, they are parallel, but as theta grows, the reflected object must yaw about +z-axis. When theta is 90, we effectively yaw not 90 degress but 180.
'''
# Point 'a' orientation euler angle = theta_a
# euler to rot matrix for transform
v_r_a = r_tool.euler2mat(theta_a)
# transform to the O cordinate from S cordinate
v_r_a_hat = np.matmul(v_r_a, inv_rot_z_theta)
# Rot matrix to euler for sym
v_r_a_hat = r_tool.mat2euler(v_r_a_hat)
# Sym on transformed S's xoz plane (y axis)
v_r_a_hat[0 - i] = -v_r_a_hat[0 - i]
v_r_a_hat[2] = -v_r_a_hat[2]
# euler to rot matrix for transform
v_r_a_hat = r_tool.euler2mat(v_r_a_hat)
s_v_r_a = np.matmul(v_r_a_hat, rot_z_theta)
# Rot matrix to euler for return
s_v_r_a = r_tool.mat2euler(s_v_r_a)
return s_v_r_a.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
pusher_pos = self.sim.data.get_site_xpos('pusher')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
pusher_velp = self.sim.data.get_site_xvelp('pusher') * dt
robot_qpos, robot_qvel = utils.ur5_get_obs(self.sim)
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
# pusher state
object_rel_pos = object_pos - pusher_pos
object_velp -= pusher_velp
# achieved_goal = np.squeeze(object_pos.copy())
achieved_goal = np.concatenate([object_pos.ravel(), object_rot.ravel()])
obs = np.concatenate([
pusher_pos, object_pos.ravel(), object_rot.ravel(),
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
ft = self.sim.data.sensordata.copy()
if self.start_flag:
ft = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.start_flag = False
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rpy = normalize_rad(rotations.mat2euler(x_mat))
obs = numpy.concatenate([
rot_mat.dot(x_pos-self.goal[:3]),
rot_mat.dot(normalize_rad(rpy-self.goal[3:])),
ft.copy()
])
if self.save_data:
self.fts.append([ft[0], ft[1], ft[2], ft[3], ft[4], ft[5],])
self.obs.append(obs)
self.fxs.append(ft[0])
self.fys.append(ft[1])
self.fzs.append(ft[2])
self.poses.append(numpy.concatenate(
[x_pos-self.goal[:3],
normalize_rad(rpy-self.goal[3:])]))
self.last_obs = obs
return obs
def get_target_rot(self, pad=True) -> np.ndarray:
"""
Get target rotation in euler angles for all objects.
"""
return self._get_target_obs(
lambda n: rotations.mat2euler(self.mj_sim.data.get_body_xmat(n)),
pad=pad)
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)
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
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
ft = self.sim.data.sensordata.copy()
if self.ft_noise:
ft += numpy.random.randn(6, ) * self.ft_std
if self.ft_drift:
ft += self.ft_drift_val
ft[1] -= 0.350 * 9.81 # nulling in initial position
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_pos += numpy.random.uniform(-self.pos_std[:3], self.pos_std[:3])
x_pos += self.pos_drift_val[:3]
#x_quat = self.sim.data.get_body_xquat("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rpy = normalize_rad(
rotations.mat2euler(x_mat) +
numpy.random.uniform(-self.pos_std[3:], self.pos_std[3:]) +
self.pos_drift_val[3:])
obs = np.concatenate([
rot_mat.dot(x_pos - self.goal[:3]),
rot_mat.dot(normalize_rad(rpy - self.goal[3:])),
ft.copy()
])
self.last_obs = obs
return obs
def _get_obs(self):
obs = fetch_env.FetchEnv._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
hook_pos = self.sim.data.get_site_xpos('hook')
hook_pos = self._noisify_obs(hook_pos, noise=0.025)
# rotations
hook_rot = rotations.mat2euler(self.sim.data.get_site_xmat('hook'))
hook_rot = self._noisify_obs(hook_rot, noise=0.025)
# velocities
hook_velp = self.sim.data.get_site_xvelp('hook') * dt
hook_velr = self.sim.data.get_site_xvelr('hook') * dt
# gripper state
hook_rel_pos = hook_pos - grip_pos
hook_velp -= grip_velp
hook_observation = np.concatenate(
[hook_pos, hook_rot, hook_velp, hook_velr, hook_rel_pos])
obs['observation'] = np.concatenate(
[obs['observation'], hook_observation])
obs['observation'][3:5] = self._noisify_obs(obs['observation'][3:5],
noise=0.025)
obs['observation'][6:9] = obs['observation'][3:6] - obs[
'observation'][:3] #object_pos - grip_pos
obs['observation'][12:15] = self._noisify_obs(obs['observation'][6:9],
noise=0.025)
return obs
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):
# print("trying to get obs")
ee_pos = self.sim.data.get_site_xpos('grip')
obj_pos = self.sim.data.get_site_xpos('box')[:self.space_dim]
target_pos = self.sim.data.get_site_xpos('target')[:self.space_dim]
# print("targetpos", target_pos, "qpos", self.data.get_joint_qpos('target'))
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
# ee vel
ee_velp = self.sim.data.get_site_xvelp('grip') * dt
# gripper state
gripper_state_l = self.data.get_joint_qpos("l_gripper_l_finger_joint")
gripper_state_r = self.data.get_joint_qpos("l_gripper_r_finger_joint")
# obj rel pos
obj_rel_pos = obj_pos - ee_pos
# obj rotation
object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('box'))
# obj vel
object_velp = self.sim.data.get_site_xvelp('box') * dt
object_velr = self.sim.data.get_site_xvelr('box') * dt
# gripper
gripper_state = np.array([gripper_state_l, gripper_state_r])
gripper_vel = gripper_state * dt
# things missing obj_rel_pos, obj_rot, obj_velr
state = np.concatenate([
ee_pos, obj_pos, obj_rel_pos, gripper_state, object_rot,
object_velp, object_velr, ee_velp, gripper_vel, target_pos
])
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)
# 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):
# 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):
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
ft = self.sim.data.sensordata.copy()
if self.start_flag:
ft = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.start_flag = False
ft[1] -= self.gripper_mass * 9.81
ft[:3] += numpy.random.normal(self.f_mean_si, self.f_std_si, 3)
ft[:3] += numpy.random.uniform(-self.f_var_dr_uncor,
self.f_var_dr_uncor, 3)
ft[:3] += self.f_corr
ft[3:] += numpy.random.normal(self.t_mean_si, self.t_std_si, 3)
ft[3:] += numpy.random.uniform(-self.t_var_dr_uncor,
self.t_var_dr_uncor, 3)
ft[3:] += self.t_corr
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_pos += numpy.random.normal(self.pos_mean_si, self.pos_std_si, 3)
x_pos += numpy.random.uniform(-self.pos_var_dr_uncor,
self.pos_var_dr_uncor, 3)
x_pos += self.pos_corr
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
#print(rotations.mat2euler(x_mat))
rpy = normalize_rad(
rotations.mat2euler(x_mat) +
numpy.random.normal(self.rot_mean_si, self.rot_std_si, 3) *
(numpy.pi / 180) + numpy.random.uniform(-self.rot_var_dr_uncor,
self.rot_var_dr_uncor, 3) *
(numpy.pi / 180) + self.rot_corr * (numpy.pi / 180))
obs = numpy.concatenate([
rot_mat.dot(x_pos - self.goal[:3]),
rot_mat.dot(normalize_rad(rpy - self.goal[3:])),
ft.copy()
])
if self.save_data:
self.fts.append([
ft[0],
ft[1],
ft[2],
ft[3],
ft[4],
ft[5],
])
self.obs.append(obs)
self.fxs.append(ft[0])
self.fys.append(ft[1])
self.fzs.append(ft[2])
self.poses.append(
numpy.concatenate([
x_pos - self.goal[:3],
normalize_rad(rpy - self.goal[3:])
]))
self.last_obs = obs
return obs
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
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
ft = self.sim.data.sensordata.copy()
#if self.ft_noise:
# ft += numpy.random.randn(6,) * self.ft_std
#if self.ft_drift:
# ft += self.ft_drift_val
ft[1] -= 0.588 * 9.81 # nulling in initial position
ft_si_noise = numpy.concatenate([
numpy.random.normal(0.0, self.f_std_si, 3),
numpy.random.normal(0.0, self.t_std_si, 3)
])
ft_dr_uncor_noise = numpy.concatenate([
numpy.random.uniform(-self.f_var_dr_uncor, self.f_var_dr_uncor, 3),
+numpy.random.uniform(-self.t_var_dr_uncor, self.t_var_dr_uncor, 3)
])
ft_noise = ft_si_noise + ft_dr_uncor_noise + self.ft_dr_cor_noise
ft += ft_noise
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
pos_si_noise = numpy.random.normal(0.0, self.pos_std_si, 3)
pos_dr_uncor_noise = numpy.random.uniform(-self.pos_var_dr_uncor,
self.pos_var_dr_uncor, 3)
pos_noise = pos_si_noise + pos_dr_uncor_noise + self.pos_dr_cor_noise
x_pos += pos_noise
#x_pos += numpy.random.uniform(-self.pos_std[:3], self.pos_std[:3])
#x_pos += self.pos_drift_val[:3]
#x_quat = self.sim.data.get_body_xquat("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rot_si_noise = numpy.random.normal(0.0, self.rot_std_si,
3) * (numpy.pi / 180)
rot_dr_uncor_noise = numpy.random.uniform(-self.rot_var_dr_uncor,
self.rot_var_dr_uncor,
3) * (numpy.pi / 180)
rot_noise = rot_si_noise + rot_dr_uncor_noise + self.rot_dr_cor_noise
rpy = normalize_rad(rotations.mat2euler(x_mat) + rot_noise)
obs = np.concatenate([
rot_mat.dot(x_pos - self.goal[:3]),
rot_mat.dot(normalize_rad(rpy - self.goal[3:])),
ft.copy()
])
self.last_obs = obs
return obs
def _is_success(self, achieved_goal, desired_goal):
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rpy = normalize_rad(rotations.mat2euler(x_mat))
obs = np.concatenate([
rot_mat.dot(x_pos-self.goal[:3]), rot_mat.dot(normalize_rad(rpy-self.goal[3:]))
])
d = goal_distance(obs, desired_goal)
self.distances.append(d)
return (d < self.distance_threshold).astype(np.float32)
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_len(self):
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
# positions
grip_pos = self.sim.data.get_site_xpos('robot0:grip')
grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt
robot_qpos, robot_qvel = self.sim.data.qpos, self.sim.data.qvel
object_pos, object_rot, object_velp, object_velr = ([]
for _ in range(4))
object_rel_pos = []
if self.n_objects > 0:
for n_o in range(self.n_objects):
oname = 'object{}'.format(n_o)
this_object_pos = self.sim.data.get_site_xpos(oname)
# rotations
this_object_rot = rotations.mat2euler(
self.sim.data.get_site_xmat(oname))
# velocities
this_object_velp = self.sim.data.get_site_xvelp(oname) * dt
this_object_velr = self.sim.data.get_site_xvelr(oname) * dt
# gripper state
this_object_rel_pos = this_object_pos - grip_pos
this_object_velp -= grip_velp
object_pos = np.concatenate([object_pos, this_object_pos])
object_rot = np.concatenate([object_rot, this_object_rot])
object_velp = np.concatenate([object_velp, this_object_velp])
object_velr = np.concatenate([object_velr, this_object_velr])
object_rel_pos = np.concatenate(
[object_rel_pos, this_object_rel_pos])
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.array(
np.zeros(3))
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,
object_pos.ravel(),
object_rel_pos.ravel(),
gripper_state,
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
grip_velp,
gripper_vel,
])
print("obs len: ", len(obs))
return len(obs)
def _get_obs(self):
obs = fetch_env.FetchEnv._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
hook_pos = self.sim.data.get_site_xpos('hook')
# rotations
hook_rot = rotations.mat2euler(self.sim.data.get_site_xmat('hook'))
# velocities
hook_velp = self.sim.data.get_site_xvelp('hook') * dt
hook_velr = self.sim.data.get_site_xvelr('hook') * dt
# gripper state
hook_rel_pos = hook_pos - grip_pos
hook_velp -= grip_velp
hook_observation = np.concatenate(
[hook_pos, hook_rot, hook_velp, hook_velr, hook_rel_pos])
obs['observation'] = np.concatenate(
[obs['observation'], hook_observation])
hook_pos = self.sim.data.get_site_xpos('hooktwo')
# rotations
hook_rot = rotations.mat2euler(self.sim.data.get_site_xmat('hooktwo'))
# velocities
hook_velp = self.sim.data.get_site_xvelp('hooktwo') * dt
hook_velr = self.sim.data.get_site_xvelr('hooktwo') * dt
# gripper state
hook_rel_pos = hook_pos - grip_pos
hook_velp -= grip_velp
hook_observation = np.concatenate(
[hook_pos, hook_rot, hook_velp, hook_velr, hook_rel_pos])
obs['observation'] = np.concatenate(
[obs['observation'], hook_observation])
return obs
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 _set_action(self, action):
assert action.shape == (self.n_actions,)
self.action = action
# energy correction
h = self.sim.data.get_site_xpos('tar')[2]
if h < 0.8:
self.energy_corrected = False
# each time the target crosses the z == 0.8 line its energy gets corrected
if not self.energy_corrected:
if h > 0.8:
self.sim.data.set_joint_qvel('tar:z', self.sqrt_2_g * np.sqrt(self.initial_h - h))
self.give_reflection_reward = True
self.energy_corrected = True
if not self.no_movement:
a = np.zeros([8], dtype=np.float32)
a[0] = 0.015 * action[0]
a[1] = 0.015 * action[1]
euler = rotations.mat2euler(self.sim.data.get_site_xmat('robot0:grip'))
a_4 = euler[0]
a_5 = euler[1]
# adjusting z position to a plane (otherwise the arm has a downwards drift)
if self.sim.data.get_site_xpos('robot0:grip')[2] < 0.705:
a[2] = 0.665 - self.sim.data.get_site_xpos('robot0:grip')[2]
else:
a[2] = 0.665 - self.sim.data.get_site_xpos('robot0:grip')[2]
a[4] = 0.015 * action[2]
a[5] = 0.015 * action[3]
# Restricting rotation action:
if a_4 > 0.3 and action[2] > 0.:
a[4] = 0.
if a_4 < -0.3 and action[2] < 0.:
a[4] = 0.
if a_5 > 0.3 and action[3] > 0.:
a[5] = 0.
if a_5 < -0.3 and action[3] < 0.:
a[5] = 0.
# Apply action to simulation.
utils.ctrl_set_action(self.sim, a)
utils.mocap_set_action(self.sim, a)
else:
pass
def _get_obs(self):
# positions
ft = self.sim.data.sensordata
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_quat = self.sim.data.get_body_xquat("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rpy = normalize_rad(rotations.mat2euler(x_mat))
obs = np.concatenate(
[x_pos - self.goal[:3],
normalize_rad(rpy - self.goal[3:]), ft])
self.last_obs = obs
return obs
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):
# positions
rot_mat = self.sim.data.get_body_xmat('gripper_dummy_heg')
ft = self.sim.data.sensordata.copy()
x_pos = self.sim.data.get_body_xpos("gripper_dummy_heg")
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
rpy = normalize_rad(rotations.mat2euler(x_mat))
obs = np.concatenate([
rot_mat.dot(x_pos-self.goal[:3]), rot_mat.dot(normalize_rad(rpy-self.goal[3:])), ft.copy()
])
#self.sim_poses.append(numpy.concatenate([x_pos-self.goal[:3], normalize_rad(rpy-self.goal[3:])]))
self.last_obs = obs
return obs
def publish_observations(self):
'''Function that computes and publishes the observations given the pose, rotation matrix and force torque data'''
msg = CustomVector()
x_mat = self.sim.data.get_body_xmat("gripper_dummy_heg")
pos = self.final_pose
rpy = normalize_rad(rotations.mat2euler(x_mat))
msg.data = numpy.concatenate([
x_mat.dot(pos),
x_mat.dot(normalize_rad(rpy - self.goal[3:])),
self.ft_values.copy()
])
self.observation_publisher.publish(msg)
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):
gripper_pos = self.gripper_position
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('grip') * dt
qpos = self.sim.data.qpos
qvel = self.sim.data.qvel
object_pos = self.object_position
object_rot = rotations.mat2euler(
self.sim.data.get_site_xmat('object0'))
object_velp = self.sim.data.get_site_xvelp('object0') * dt
object_velr = self.sim.data.get_site_xvelr('object0') * dt
object_rel_pos = object_pos - gripper_pos
object_velp -= grip_velp
grasped = self.has_object
obs = np.concatenate([
gripper_pos,
object_pos.ravel(), # TODO remove object_pos (reveals task id)
object_rel_pos.ravel(),
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
grip_velp,
qpos,
qvel,
[float(grasped), self.gripper_state],
])
achieved_goal = self._achieved_goal_fn(self)
desired_goal = self._desired_goal_fn(self)
achieved_goal_qpos = np.concatenate((achieved_goal, [1, 0, 0, 0]))
self.sim.data.set_joint_qpos('achieved_goal:joint', achieved_goal_qpos)
desired_goal_qpos = np.concatenate((desired_goal, [1, 0, 0, 0]))
self.sim.data.set_joint_qpos('desired_goal:joint', desired_goal_qpos)
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal,
'desired_goal': desired_goal,
'gripper_state': self.gripper_state,
'gripper_pos': gripper_pos.copy(),
'has_object': grasped,
'object_pos': object_pos.copy(),
'joints': self.sim.data.qpos.copy()
}
def get_current_obs(self):
grip_pos = self.sim.data.get_site_xpos('grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('grip') * dt
qpos = self.sim.data.qpos
qvel = self.sim.data.qvel
object_pos = self.sim.data.get_geom_xpos('object0')
object_rot = rotations.mat2euler(
self.sim.data.get_geom_xmat('object0'))
object_velp = self.sim.data.get_geom_xvelp('object0') * dt
object_velr = self.sim.data.get_geom_xvelr('object0') * dt
object_rel_pos = object_pos - grip_pos
object_velp -= grip_velp
achieved_goal = np.squeeze(object_pos.copy())
if self._control_method == 'position_control':
obs = np.concatenate([
grip_pos,
object_pos.ravel(),
object_rel_pos.ravel(),
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
grip_velp,
])
elif self._control_method == 'torque_control':
obs = np.concatenate([
object_pos.ravel(),
object_rel_pos.ravel(),
object_rot.ravel(),
object_velp.ravel(),
object_velr.ravel(),
qpos,
qvel,
])
else:
raise NotImplementedError
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self._goal,
'gripper_pos': grip_pos,
}
