Function to transform from 3D XYZ to exponential map
DungxNguyen opened this issue · 6 comments
Hello,
Do you know how to transform from 3D XYZ coordinates of joints to the exponential map of the joint angle?
Is it a simple way to transform them?
Best,
I have the same doubt. Can anyone help with it? Also, the definitions of offset and RotInd are little confusing from the code in forward_kinematics.py. Can anyone clarify?
Thanks,
Mukund
I have the same doubt. Can anyone help with it? Also, the definitions of offset and RotInd are little confusing from the code in forward_kinematics.py. Can anyone clarify?
Thanks,
Mukund
Hi @MukundSeethamraju I have the same question right now, have you work out, need your help.
I implemented this a while ago, so please test if it works.
def xyz2expmap(J0, J1, J2):
return rotmat2expmap(xzy2rotmat(J0, J1, J2))
def skew(x):
return np.array([[0, -x[2], x[1]],
[x[2], 0, -x[0]],
[-x[1], x[0], 0]])
def norm_vec(u):
return u / np.sqrt(np.dot(u, u))
def xzy2rotmat(J0, J1, J2):
"""
Converts an angle to rotation matrix
Args
J0, J1, J2: 3 3x1 vectors of the positions of 3 joints.
J0 is joint in between. J1 is a parent of J0 and J2 is a child of J0 in a human kinematic tree
Returns
R: a 3x3 rotation matrix
"""
U = J1 - J0
V = J2 - J0
axis = norm_vec(np.cross(U, V))
cosine_theta = np.dot(U, V) / (np.sqrt(np.dot(U, U)) * np.sqrt(np.dot(V, V)))
sine_theta = np.sqrt(1 - cosine_theta ** 2)
R = cosine_theta * np.identity(3) + sine_theta * skew(axis) + (1 - cosine_theta) * np.outer(axis, axis)
return R
I have been doing some testing of @DungxNguyen implementation but I can't obtain similar exponential map angles to the starting point.
From this skeleton (in exponential map format):
expmap = np.asarray([0.0000000,0.0000000,0.0000000, -0.0000000,-0.0000001,0.0000001, 0.3230213,-0.4831149,-0.0673216, -0.0632727,-0.0000000,-0.0000000, -0.0513711,0.5105966,-0.0717521, 0.3066142,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, 0.3265765,0.3157165,-0.0472587, -0.1024184,-0.0000000,-0.0000000, 0.0504554,-0.3802582,-0.0118426, 0.2074713,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, 0.4935718,0.0160918,-0.1225905, -0.0910649,0.0319310,0.0919449, -0.5382497,0.0032421,-0.3011716, 1.3627026,-0.0115682,0.1639829, -0.0000000,-0.0000000,-0.0000000, -0.1539563,-0.0657004,2.0724664, -0.1472652,-0.2662845,0.7816223, -0.1515370,-0.0000000,-0.0000000, 0.2704134,-0.5088616,0.1476415, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, 0.0520571,0.0791909,-2.1037283, -0.1340761,0.4897867,-0.8238848, -0.3317853,-0.0000000,-0.0000000, 0.1473034,-0.1267874,-0.0072563, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000, -0.0000000,-0.0000000,-0.0000000])
I apply the forward kinematics step and obtain the following skeleton in xyz format:
xyz = np.asarray([[[-1.00000000e-07, 0.00000000e+00, 1.00000000e-07], [-1.32948591e+02, 7.21395214e-06, 7.31395214e-06], [-7.12205344e+01, 1.28042418e+02, -4.19464443e+02], [-2.08359262e+01, 2.35085875e+02, -8.57992190e+02], [-2.53277722e+01, 3.95477933e+02, -8.30654812e+02], [-3.11122313e+01, 4.62083302e+02, -7.96666868e+02], [1.32948826e+02, -7.21396489e-06, -7.11396489e-06], [1.30721063e+02, 1.42910366e+02, -4.19198198e+02], [1.42957362e+02, 2.46973147e+02, -8.61153893e+02], [1.53058057e+02, 4.02895814e+02, -8.15554193e+02], [1.57114238e+02, 4.68896673e+02, -7.80164559e+02], [-9.45738785e-08, -1.47096310e-16, 1.00000100e-01], [-2.64849004e+01, -1.10510074e+02, 2.03947543e+02], [-3.34620788e+01, -2.10663311e+02, 4.40610978e+02], [-7.46627281e+01, -1.94861037e+02, 5.53422624e+02], [-7.36184535e+01, -3.02745210e+02, 5.93240023e+02], [-3.34620788e+01, -2.10663311e+02, 4.40610978e+02], [9.99888314e+01, -1.72456896e+02, 3.81092444e+02], [1.83446705e+02, -7.75122410e+01, 1.32504683e+02], [2.41767447e+02, 4.07991330e+01, -8.19033898e+01], [2.41767447e+02, 4.07991330e+01, -8.19033898e+01], [1.65187940e+02, 1.04061455e+02, -9.34524740e+01], [2.71241200e+02, 5.93647979e+01, -1.75640516e+02], [2.71241200e+02, 5.93647979e+01, -1.75640516e+02], [-3.34620788e+01, -2.10663311e+02, 4.40610978e+02], [-1.59760834e+02, -1.98654521e+02, 3.58666470e+02], [-2.06843483e+02, -1.27348351e+02, 9.31859735e+01], [-2.20514309e+02, 9.48864841e+00, -1.17660069e+02], [-2.20514309e+02, 9.48864841e+00, -1.17660069e+02], [-1.97607651e+02, 9.87210857e+01, -7.87648976e+01], [-2.33021616e+02, 6.68880774e+01, -2.41978593e+02], [-2.33021616e+02, 6.68880774e+01, -2.41978593e+02]]])
When I use xyz2expmap function on the first three joints, I obtain the following angles:
[9.69321432e-08 1.36862317e+00 4.17775132e-01]
Which are not similar to the original first joint angles:
-0.0000000,-0.0000001,0.0000001
Am I doing something wrong?
Hi all there, while this can be a bit late, can I know if anyone have solved this problem?
From my understanding now, it seems that rotation matrix cannot be uniquely recovered from only the two vectors