From 73f6aad6c6c73e8a53c6eef9232ed493f74b8c06 Mon Sep 17 00:00:00 2001 From: TedBrierley <90796693+TedBrierley@users.noreply.github.com> Date: Tue, 14 Nov 2023 07:59:12 +0000 Subject: [PATCH 1/3] 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 --- src/dlc2kinematics/utils/auxiliaryfunctions.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/dlc2kinematics/utils/auxiliaryfunctions.py b/src/dlc2kinematics/utils/auxiliaryfunctions.py index 26a5b5b..013de9b 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[2, :] - pos[2, :] rel = v - u x = rel[0] y = rel[1] z = rel[2] - + # yaw, pitch relative to the vertical plane (x-z plane) yaw = np.arctan(x / z) * 180 / np.pi - - padj = np.sqrt(x ** 2) + np.sqrt(z ** 2) + padj = np.sqrt((x ** 2) + (z ** 2)) pitch = np.arctan(padj / y) * 180.0 / np.pi return np.array([pitch, yaw]) From 1b98e64387f39a47ea34e739be577eeed694f1bc Mon Sep 17 00:00:00 2001 From: TedBrierley <90796693+TedBrierley@users.noreply.github.com> Date: Tue, 14 Nov 2023 08:02:57 +0000 Subject: [PATCH 2/3] 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 --- src/dlc2kinematics/utils/auxiliaryfunctions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dlc2kinematics/utils/auxiliaryfunctions.py b/src/dlc2kinematics/utils/auxiliaryfunctions.py index 013de9b..dcbfd51 100644 --- a/src/dlc2kinematics/utils/auxiliaryfunctions.py +++ b/src/dlc2kinematics/utils/auxiliaryfunctions.py @@ -228,7 +228,7 @@ def doubleangle_calc(pos): pos = pos.reshape((3, 3)) # print(pos) u = pos[1, :] - pos[0, :] - v = pos[2, :] - pos[2, :] + v = pos[1, :] - pos[2, :] rel = v - u From e8b3169ef603be589b71a411593e36ff9ca6bdd8 Mon Sep 17 00:00:00 2001 From: Jessy Lauer <30733203+jeylau@users.noreply.github.com> Date: Wed, 13 Dec 2023 19:43:34 +0100 Subject: [PATCH 3/3] Update src/dlc2kinematics/utils/auxiliaryfunctions.py --- src/dlc2kinematics/utils/auxiliaryfunctions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dlc2kinematics/utils/auxiliaryfunctions.py b/src/dlc2kinematics/utils/auxiliaryfunctions.py index dcbfd51..803f2b1 100644 --- a/src/dlc2kinematics/utils/auxiliaryfunctions.py +++ b/src/dlc2kinematics/utils/auxiliaryfunctions.py @@ -236,7 +236,7 @@ def doubleangle_calc(pos): y = rel[1] z = rel[2] # yaw, pitch relative to the vertical plane (x-z plane) - yaw = np.arctan(x / z) * 180 / np.pi + yaw = np.rad2deg(np.arctan2(x, z)) padj = np.sqrt((x ** 2) + (z ** 2)) pitch = np.arctan(padj / y) * 180.0 / np.pi