Problems aligning orientations of poses on an ellipsoid in Python

27 views Asked by At

I am trying to generate poses on an ellipsoid in Python, and I want the orientations of these poses to be aligned with the direction vectors pointing from the center of the ellipsoid to the surface points. However, I am facing issues with the orientation alignment, and I'm seeking help to understand what might be going wrong.

Here is a simplified version of my code to generate the points position and the direction vectors:

def gen_ellipsoid(centre, h_res, v_res, a, b, show=True):
    u = np.linspace(0, np.pi * 2, h_res)[1:]
    v = np.linspace(0, np.pi / 2, v_res)

    points = []
    poses = []
    dir_vers = []

    x_scale = a / (np.max([a,b]))
    y_scale = b / (np.max([a,b]))
    
    for phi in v:
        for theta in u:
            T = np.eye(4)
            x = (np.sin(phi) * np.cos(theta) * x_scale) + centre[0]
            y = (np.sin(phi) * np.sin(theta) * y_scale) + centre[1]
            z = np.cos(phi)
 
            direction_vector = np.array([centre[0] - x, centre[1] - y, - z])

I know these are correct because I plot them by generating points on these vectors using this code:

for i, point in enumerate(points):
            ax.scatter(point[0], point[1], point[2], marker='o')
            for j in np.linspace(0.1, 0.9, 10):
                tmp = dir_vers[i] * j + np.array([point[0], point[1], point[2]])
                ax.scatter(tmp[0], tmp[1], tmp[2], color='red')

And this is the result

I then would like to have a rotation matrix to then feed it to a inverse kinematics solver for my robot, this is where the problems arise, I have tried different ways, all of them without luck:

direction_vector /= np.linalg.norm(direction_vector)
            angle = np.arccos(np.dot(np.array([0, 0, 1]), direction_vector))
            axis = np.cross(np.array([0, 0, 1]), direction_vector)
            Rot_mat = R.from_rotvec(angle * axis).as_matrix()
 Rot_mat = transf.Rotation.align_vectors([[0, 0, 1]], [direction_vector])[0].as_matrix()
magnitude = np.sqrt((direction_vector[0]**2 + direction_vector[1]**2 + direction_vector[2]**2))
            norm_dir_vec = direction_vector / -magnitude
            print(norm_dir_vec)
            x_axis = np.cross([0, 0, 1], norm_dir_vec)
            if np.linalg.norm(x_axis) > 0:
                x_axis_norm = x_axis / np.linalg.norm(x_axis)
                y_axis = np.cross(norm_dir_vec, x_axis_norm)
                y_axis_norm = y_axis / np.linalg.norm(y_axis)
            else:
                x_axis_norm = np.array([0, 0, 0])
                y_axis_norm = np.array([0, 0, 0])
            Rot_mat = np.stack((x_axis_norm, y_axis_norm, norm_dir_vec), axis=1)
            Rot_mat = Rot_mat * np.eye(3)
            print(np.linalg.det(Rot_mat))
            Rot_mat = sm.SO3(trnorm(Rot_mat))

The result from the first approach are depicted here

I have used the pytransform3d library to visualize them through this loop:

for i, pose in enumerate(poses):
    tm.add_transform(f"{i}", "base", pose)

I have also tried add a correction term but it looks like some are more wrong then others so I don't know what to do. Any guidance or suggestions on how to correct the orientation alignment would be greatly appreciated. Thank you!

0

There are 0 answers