rotate quat

[renyu2016]

import numpy as np import quaternion

def slerp(q0, q1, t): """ Spherical Linear Interpolation (SLERP) between two quaternions.

Args:
    q0: The start quaternion.
    q1: The end quaternion.
    t: Interpolation parameter, a value between 0 and 1.

Returns:
    Interpolated quaternion.
"""
q0 = quaternion.as_float_array(q0)
q1 = quaternion.as_float_array(q1)

dot_product = np.dot(q0, q1)

# Ensure shortest path
if dot_product < 0.0:
    q1 = -q1
    dot_product = -dot_product

dot_threshold = 0.9995
if dot_product > dot_threshold:
    result = q0 + t * (q1 - q0)
    return quaternion.as_quat_array(result / np.linalg.norm(result))

theta_0 = np.arccos(dot_product)
sin_theta_0 = np.sin(theta_0)

theta = 10 * np.pi / 180  # Convert 10 degrees to radians
sin_theta = np.sin(theta)

s0 = np.cos(theta) - dot_product * sin_theta / sin_theta_0
s1 = sin_theta / sin_theta_0

return quaternion.as_quat_array((s0 * q0) + (s1 * q1))

def rotate_to_b(q0, q1): """ Rotate from quaternion q0 to quaternion q1 by 10 degrees.

Args:
    q0: The start quaternion.
    q1: The end quaternion.

Returns:
    Quaternion representing rotation from q0 to q1 by 10 degrees.
"""
# Convert to rotation matrices
R0 = quaternion.as_rotation_matrix(q0)
R1 = quaternion.as_rotation_matrix(q1)

# Calculate the angle between rotations
angle = np.arccos(np.trace(np.dot(R0.T, R1)) / 2.0)

# Check if the angle is greater than 10 degrees
if np.degrees(angle) > 10:
    # Perform SLERP with t = 0.1 (10 degrees)
    return slerp(q0, q1, 0.1)
else:
    return q1

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)

[renyu2016]

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

[renyu2016]

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

centralied_point_cloud[mask, :] = np.array([0, 0, 0])

                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])

[renyu2016]

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)