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!