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 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 compute_handle_pos(tip_pos, tip_rot):
rot_mat = rotations.euler2mat(tip_rot)
tran_mat = np.zeros((4, 4))
tran_mat[3, 3] = 1.
tran_mat[:3, 3] = tip_pos
tran_mat[:3, :3] = rot_mat
handle = np.zeros((4, ))
# handle[:3] = subgoal[:3]
handle[0] = -RakeObjectThresholds.rake_handle_x_offset
# handle[3] = 1.
handle_new = tran_mat * handle
return handle_new[:3, 0]
def render_box(viewer,
bounds: np.ndarray = None,
pose: np.ndarray = None,
size: np.ndarray = None,
label="",
opacity=0.2,
unique_id=None,
unique_label=False):
if viewer is None:
return
extra_kwargs = dict()
if unique_label:
unique_id = hash(label) % np.iinfo(np.int32).max
if unique_id is not None:
extra_kwargs['dataid'] = unique_id
with viewer._gui_lock:
existing = [
i for i, m in enumerate(viewer._markers)
if m['dataid'] == unique_id
]
for i in existing[::-1]:
del viewer._markers[i]
if bounds is not None:
assert size is None
assert bounds.shape == (3, 2)
pose = pose or np.zeros(3)
pose[:3] = bounds.mean(axis=1)
size = np.abs(bounds[:, 1] - bounds[:, 0]) / 2.0
else:
assert pose is not None
assert size is not None
pos = pose[:3]
if pose.size == 6:
mat = rotations.euler2mat(pose[3:])
elif pose.size == 7:
mat = rotations.quat2mat(pose[3:])
else:
mat = np.eye(3)
viewer.add_marker(pos=pos,
mat=mat.flatten(),
label=label,
type=mj_const.GEOM_BOX,
size=size,
rgba=np.r_[1., 0., 0., opacity],
specular=0.,
**extra_kwargs)
def kaleidoscope_robot(self,
param,
z_theta,
sym_axis='y_axis',
sym_method='y_ker'):
if sym_axis == 'y_axis':
# in linear variable, plus i; in angular variable, minus i
i = 0
elif sym_axis == 'x_axis':
i = -1
if sym_method == 'y_ker':
self.sym_plane = SYM_PLANE_Y
elif sym_method == 'x_ker':
self.sym_plane = SYM_PLANE_X
# compute the rotation transformation & its inverse.
theta = np.array([0, 0, z_theta])
rot_z_theta = r_tool.euler2mat(theta)
inv_theta = np.array([0, 0, -z_theta])
inv_rot_z_theta = r_tool.euler2mat(inv_theta)
# Determine the input is which element
param_len = len(param[0])
#goal & achieved goal
if param_len == 3:
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
elif param_len == 4: #action
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
elif param_len == 10: # observation without object
# grip pos
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
# grip vel
v_l_a = param[0][5:8]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][5:8] = s_v_l_a
elif param_len >= 25: # observation with object
# sym_grip_pos
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
# sym_obj_pos
v_l_a = param[0][3:6]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][3:6] = s_v_l_a
# sym_obj_rel_pos
param[0][6] = param[0][3] - param[0][0]
param[0][7] = param[0][4] - param[0][1]
param[0][8] = param[0][5] - param[0][2]
# sym_obj_rot_euler
theta_a = param[0][11:14]
param[0][11:14] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
# get the obj real velp
v_l_a = param[0][14:17] + param[0][20:23]
# get the sym obj real velp
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][14:17] = s_v_l_a
# get the sym grip real velp
v_l_a = param[0][20:23]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][20:23] = s_v_l_a
# get the sym obj relative velp
param[0][14:17] -= param[0][20:23]
# sym_obj_velr
theta_a = param[0][17:20]
param[0][17:20] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
if param_len == 31:
# obstacle state
# 1. pos transform
v_l_a = param[0][25:28]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][25:28] = s_v_l_a
# 2. rot transform
theta_a = param[0][28:31]
param[0][
28:31] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
return param.copy()
def kaleidoscope_robot(self,
param,
z_theta,
sym_axis='y_axis',
sym_method='y_ker'):
''' Will compute transformations of (s,a,r,s',g,ag) according to transportation.
Fetch observations have the following format:
0-2 gripper absolute position,
3-5 object absolute position,
6-8 object relative position w.r.t. the gripper position,
9-10 gripper fingers' positions (not apply KER for it),
11-13 object absolute orientation,
14-16 object relative velocity w.r.t gripper velocity,
17-19 object absolute angular velocity,
20-22 gripper absolute velocity,
23-24 gripper fingers' velocities(not apply KER to it)
'''
if sym_axis == 'y_axis':
# in linear variable, plus i; in angular variable, minus i
i = 0
elif sym_axis == 'x_axis':
i = -1
if sym_method == 'y_ker':
self.sym_plane = SYM_PLANE_Y
elif sym_method == 'x_ker':
self.sym_plane = SYM_PLANE_X
# compute the rotation transformation & its inverse.
theta = np.array([0, 0, z_theta])
rot_z_theta = r_tool.euler2mat(theta)
inv_theta = np.array([0, 0, -z_theta])
inv_rot_z_theta = r_tool.euler2mat(inv_theta)
# Determine the input is which element
param_len = len(param[0])
#goal & achieved goal
if param_len == 3:
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
elif param_len == 4: #action
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
elif param_len == 10: # observation without object
# grip pos
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
# grip vel
v_l_a = param[0][5:8]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][5:8] = s_v_l_a
elif param_len >= 25: # observation with object
# sym_grip_pos
v_l_a = param[0][0:3]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][0:3] = s_v_l_a
# sym_obj_pos
v_l_a = param[0][3:6]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][3:6] = s_v_l_a
# sym_obj_rel_pos
param[0][6] = param[0][3] - param[0][0]
param[0][7] = param[0][4] - param[0][1]
param[0][8] = param[0][5] - param[0][2]
# sym_obj_rot_euler
theta_a = param[0][11:14]
param[0][11:14] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
# get the obj real velp: objv vel is relative to the gripper. To get true velp need to add gripper vel.
v_l_a = param[0][14:17] + param[0][20:23]
# get the sym obj real velp
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][14:17] = s_v_l_a
# get the sym grip real velp
v_l_a = param[0][20:23]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
False, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][20:23] = s_v_l_a
# get the sym obj relative velp: to set the symmetryic velp need to subtract by the gripper velocity
param[0][14:17] -= param[0][20:23]
# sym_obj_velr
theta_a = param[0][17:20]
param[0][17:20] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
if param_len == 31:
# obstacle state
# 1. pos transform
v_l_a = param[0][25:28]
s_v_l_a = self.linear_vector_symmetric_with_rot_plane(
True, v_l_a, rot_z_theta, inv_rot_z_theta, i)
param[0][25:28] = s_v_l_a
# 2. rot transform
theta_a = param[0][28:31]
param[0][
28:31] = self.orientation_mat_symmetric_with_rot_plane(
theta_a, rot_z_theta, inv_rot_z_theta, i)
return param.copy()
