def point_down(env):
current_pos = env._right_hand_pos
target_rot_X = T.rotation_matrix(0, np.array([1, 0, 0]), point=current_pos)
target_rot_Y = T.rotation_matrix(math.pi,
np.array([0, 1, 0]),
point=current_pos)
target_rot_Z = T.rotation_matrix(math.pi,
np.array([0, 0, 1]),
point=current_pos)
target_rot = np.matmul(target_rot_Z, np.matmul(target_rot_Y, target_rot_X))
target_quat = T.mat2quat(target_rot)
done_task = False
while not done_task:
current_pos = env._right_hand_pos
current_quat = env._right_hand_quat
dquat = T.quat_slerp(current_quat, target_quat, 0.01)
dquat = T.quat_multiply(dquat, T.quat_inverse(current_quat))
if np.abs(dquat[3] - 1.0) < 1e-4:
done_task = True
grasp = -1
dpos = np.zeros(3)
action = np.concatenate([dpos, dquat, [grasp]])
obs, reward, done, info = env.step(action)
if RENDER:
env.render()
def move_orient(self, env, q1):
q0 = env.observation_spec()['eef_quat']
x_target = env.observation_spec()['eef_pos']
fraction_size = 50
for i in range(fraction_size):
q_target = T.quat_slerp(q0, q1, fraction=(i + 1) / fraction_size)
steps = 0
lamda = 0.01
current = env._right_hand_orn
target_rotation = T.quat2mat(q_target)
drotation = current.T.dot(target_rotation)
while (np.linalg.norm(T.mat2euler(drotation)) > 0.01):
current = env._right_hand_orn
target_rotation = T.quat2mat(q_target)
drotation = current.T.dot(target_rotation)
dquat = T.mat2quat(drotation)
x_current = env.observation_spec()['eef_pos']
d_pos = np.clip((x_target - x_current) * lamda, -0.05, 0.05)
d_pos = [0, 0, 0]
action = np.concatenate((d_pos, dquat, [self.grasp]))
env.step(action)
env.render()
steps += 1
if (steps > 20):
break
return
def get_interpolated_goal(self):
"""
Provides the next step in interpolation given the remaining steps.
NOTE: If this interpolator is for orientation, it is assumed to be receiving either euler angles or quaternions
Returns:
np.array: Next position in the interpolated trajectory
"""
# Grab start position
x = np.array(self.start)
# Calculate the desired next step based on remaining interpolation steps
if self.ori_interpolate is not None:
# This is an orientation interpolation, so we interpolate linearly around a sphere instead
goal = np.array(self.goal)
if self.ori_interpolate == "euler":
# this is assumed to be euler angles (x,y,z), so we need to first map to quat
x = T.mat2quat(T.euler2mat(x))
goal = T.mat2quat(T.euler2mat(self.goal))
# Interpolate to the next sequence
x_current = T.quat_slerp(x,
goal,
fraction=(self.step + 1) /
self.total_steps)
if self.ori_interpolate == "euler":
# Map back to euler
x_current = T.mat2euler(T.quat2mat(x_current))
else:
# This is a normal interpolation
dx = (self.goal - x) / (self.total_steps - self.step)
x_current = x + dx
# Increment step if there's still steps remaining based on ramp ratio
if self.step < self.total_steps - 1:
self.step += 1
# Return the new interpolated step
return x_current
current_quat = env._right_hand_quat
target_rot_X = T.rotation_matrix(0,
np.array([1, 0, 0]),
point=target_pos)
target_rot_Y = T.rotation_matrix(math.pi,
np.array([0, 1, 0]),
point=target_pos)
target_rot_Z = T.rotation_matrix(math.pi + target_angle,
np.array([0, 0, 1]),
point=target_pos)
target_rot = np.matmul(target_rot_Z,
np.matmul(target_rot_Y, target_rot_X))
target_quat = T.mat2quat(target_rot)
dquat = T.quat_slerp(current_quat, target_quat, 0.01)
dquat = T.quat_multiply(dquat, T.quat_inverse(current_quat))
action = np.concatenate([dpos, dquat, [grasp]])
obs, reward, done, info = env.step(action)
pos_traj.append(current_pos - table_top_center)
angle_traj.append(T.mat2euler(T.quat2mat(current_quat)))
# env.render()
time = 0
done_task = False
while not done_task:
time += 1
if time > 2000: