From dd2b036a43843b1798af4604a49c3b38fd3104b6 Mon Sep 17 00:00:00 2001 From: TedBrierley <90796693+TedBrierley@users.noreply.github.com> Date: Wed, 13 Dec 2023 18:47:49 +0000 Subject: [PATCH] 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 <30733203+jeylau@users.noreply.github.com> --- src/dlc2kinematics/utils/auxiliaryfunctions.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/dlc2kinematics/utils/auxiliaryfunctions.py b/src/dlc2kinematics/utils/auxiliaryfunctions.py index 26a5b5b..803f2b1 100644 --- 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])