Open renyu2016 opened 7 months ago
def get_3pt_state(self, eef_state): finger_state = eef_state[7] if finger_state > 0: finger_dist = 0.005 else: finger_dist = 0.05
root_pts = np.array([0, 0, 0])
left_pts = np.array([-finger_dist, 0, 0.02])
right_pts = np.array([finger_dist, 0, 0.02])
or_pt = np.stack([root_pts, left_pts, right_pts], axis=0)
eef_Mat = np.eye(4)
eef_Mat[:3, 3] = eef_state[:3]
eef_rot = R.from_quat(eef_state[3:7])
eef_Mat[:3, :3] = eef_rot.as_matrix()
pt_state_array = (eef_Mat[:3, :3] @ or_pt.T).T + eef_Mat[:3, 3]
pt_state = pt_state_array.reshape(9)
return pt_state
if True: if self.min_bound is not None: mask = np.any(centralied_point_cloud[:, :3] < self.min_bound, axis=1) ilegal_pts = centralied_point_cloud[mask, :] replace_pts = np.random.uniform(self.min_bound, self.max_bound, size=(ilegal_pts.shape[0], 3)) centralied_point_cloud[mask, :3] = replace_pts
if self.max_bound is not None:
mask = np.any(centralied_point_cloud[:, :3] > self.max_bound, axis=1)
ilegal_pts = centralied_point_cloud[mask, :]
replace_pts = np.random.uniform(self.min_bound, self.max_bound, size=(ilegal_pts.shape[0], 3))
centralied_point_cloud[mask, :3] = replace_pts
# centralied_point_cloud[mask, :] = np.array([0, 0, 0])
per_fg_num = 0 if vaild_cat_nums > 1: per_fg_num = int(points_num 1 / (vaild_cat_nums-1)) bg_num = 0# points_num - per_fg_num (vaild_cat_nums-1)
sample_pts = []
total_num = len(cat_ids_dic.items())
for idx, (k, cat_ids) in enumerate(cat_ids_dic.items()):
cat_num = cat_ids[2]
if cap_info[k] == 0:
continue
sampled_indices = np.random.choice(cat_num, bg_num, replace=True)
else:
if idx == total_num - 1:
last_num = points_num - per_fg_num * (total_num-2)
sampled_indices = np.random.choice(cat_num, last_num, replace=True)
else:
sampled_indices = np.random.choice(cat_num, per_fg_num, replace=True)
import numpy as np import quaternion
def slerp(q0, q1, t): """ Spherical Linear Interpolation (SLERP) between two quaternions.
def rotate_to_b(q0, q1): """ Rotate from quaternion q0 to quaternion q1 by 10 degrees.
Example usage:
q0 = quaternion.from_rotation_vector(np.pi/4 np.array([1, 0, 0])) q1 = quaternion.from_rotation_vector(np.pi/4 np.array([0, 1, 0]))
result = rotate_to_b(q0, q1) print("Result quaternion:", result)