spherical.py

x, y, z, _, _, _ = kin.transform_to_pose(self.robot.transform)
# Check translation distance
# Ensure numeric stability of angle calculation
r_2 = np.linalg.norm((x, y, z))
if r_2 < 1e-5:
    return
# Aim at tip frame with givens rotations
# First about z (yaw, azimuth) - eliminate y
r_1 = np.hypot(x, y)
c_1 = x/r_1
s_1 = -y/r_1
# Then about moving y (pitch, elevation) - eliminate z
r_2 = np.hypot(r_1, z)
c_2 = r_1/r_2
s_2 = -z/r_2
# Compute rotation of aligned ("aimed") frame w.r.t root
# Note that a givens rotation on the xz coordinates and a rotation
# about the y axis have different signs on the "sin-part"
c1 = np.arctan2(-s_1, c_1)
b1 = np.arctan2(s_2, c_2)
# Distance along the x axis of the aligned frame to the origin of
# the tip frame
x2 = r_2
# Remaining rotation between aligned frame and tip frame
t_aligned = kin.TransformRPY(0, 0, 0, 0, b1, c1)
t3 = kin.invert(t_aligned) * self.robot.transform
# For this rotation, remember: fixed XYZ = moving ZYX
_, _, _, a3, b3, c3 = kin.transform_to_pose(t3)