Update auxiliaryfunctions.py and double angles (#46)

* Update auxiliaryfunctions.py Update auxiliaryfunctions.py and double angles using the following data joints_dict['Nearfrontfoot'] = ['Shoulder', 'Nearfrontfoot', 'Stifle'] x y z (liklehood) Shoulder 649.29443 355.67105 0.31627 Nearfrontfoot 604.79279 555.68231 0.92415 Stifle 519.55591 358.85684 0.03888 Existing Code q shortest rotation -0.002116213557255 -0.003361624154413 0.951158771281366 quaternion angle -0.242500855 -0.38521442 144.037938 quaternion scaler 0.22098 0.21534 0.21723 angle_rotation_quaternion 144.041017105924 degrees Modified Code q shortest rotation 0.000686756082122 0.001090930770238 -0.308673859961963 quaternion angle 0.0786972977 0.125011118 -35.9586586 quaternion scaler 0.83145863 0.83615687 0.83452721 angle_rotation_quaternion 35.9589828940757 degrees Second change in double angles reflects above plus calculating padj correctly * Update auxiliaryfunctions.py Update auxiliaryfunctions.py and double angles using the following data joints_dict['Nearfrontfoot'] = ['Shoulder', 'Nearfrontfoot', 'Stifle'] x y z (liklehood) Shoulder 649.29443 355.67105 0.31627 Nearfrontfoot 604.79279 555.68231 0.92415 Stifle 519.55591 358.85684 0.03888 Existing Code q shortest rotation -0.002116213557255 -0.003361624154413 0.951158771281366 quaternion angle -0.242500855 -0.38521442 144.037938 quaternion scaler 0.22098 0.21534 0.21723 angle_rotation_quaternion 144.041017105924 degrees Modified Code q shortest rotation 0.000686756082122 0.001090930770238 -0.308673859961963 quaternion angle 0.0786972977 0.125011118 -35.9586586 quaternion scaler 0.83145863 0.83615687 0.83452721 angle_rotation_quaternion 35.9589828940757 degrees Second change in double angles reflects above plus calculating padj correctly * Update src/dlc2kinematics/utils/auxiliaryfunctions.py --------- Co-authored-by: Jessy Lauer <[email protected]>
AdaptiveMotorControlLab · Dec 13, 2023 · dd2b036 · dd2b036
1 parent f151f42
commit dd2b036
Showing 1 changed file with 5 additions and 6 deletions.
diff --git a/src/dlc2kinematics/utils/auxiliaryfunctions.py b/src/dlc2kinematics/utils/auxiliaryfunctions.py
@@ -132,7 +132,7 @@ def jointquat_calc(pos, use4d=False):
     pos = pos.reshape((3, 3))
     # print(pos)
     u = pos[1, :] - pos[0, :]
-    v = pos[2, :] - pos[1, :]  #
+    v = pos[1, :] - pos[2, :]
     # print(u.shape)
     # print(v.shape)
     q_shortest_rotation = vector.q_shortest_rotation(u.astype(float), v.astype(float))
@@ -228,17 +228,16 @@ def doubleangle_calc(pos):
     pos = pos.reshape((3, 3))
     # print(pos)
     u = pos[1, :] - pos[0, :]
-    v = pos[2, :] - pos[1, :]
+    v = pos[1, :] - pos[2, :]
 
     rel = v - u
 
     x = rel[0]
     y = rel[1]
     z = rel[2]
-
-    yaw = np.arctan(x / z) * 180 / np.pi
-
-    padj = np.sqrt(x ** 2) + np.sqrt(z ** 2)
+    # yaw, pitch relative to the vertical plane (x-z plane)
+    yaw = np.rad2deg(np.arctan2(x, z))
+    padj = np.sqrt((x ** 2) + (z ** 2))
     pitch = np.arctan(padj / y) * 180.0 / np.pi
 
     return np.array([pitch, yaw])