From f1c9feae0e6209bd1fd943013c7646877f3e6f5e Mon Sep 17 00:00:00 2001
From: Alex Elias <aelias36@yahoo.com>
Date: Sun, 31 Mar 2024 18:31:48 -0400
Subject: [PATCH] Create dh_to_kin.m

Moved from 7-DOF repo
---
 matlab/robot_IK_helpers/dh_to_kin.m | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100644 matlab/robot_IK_helpers/dh_to_kin.m

diff --git a/matlab/robot_IK_helpers/dh_to_kin.m b/matlab/robot_IK_helpers/dh_to_kin.m
new file mode 100644
index 0000000..71c15a5
--- /dev/null
+++ b/matlab/robot_IK_helpers/dh_to_kin.m
@@ -0,0 +1,26 @@
+function kin = dh_to_kin(alpha_vec, a_vec, d_vec)
+N = length(alpha_vec);
+kin.joint_type = zeros(1,N);
+kin.H = sym(NaN(3,N));
+kin.P = sym(NaN(3,N+1));
+
+kin.P(:,1) = [0;0;0];
+kin.H(:,1) = [0;0;1];
+R = eye(3);
+
+for i = 1:N
+    % Translate d_i along z_{i-1}   
+    % Move a along x_{i-1}
+    kin.P(:,i+1) = d_vec(i)*R(:,3) + a_vec(i)*R(:,1);
+
+    % Rotate by alpha along x_{i-1}
+    R = rot(R(:,1), alpha_vec(i))*R;
+
+    if i == N
+        kin.RT = rot(R(:,1), alpha_vec(i));
+    else
+        kin.H(:,i+1) = R(:,3); % Joint axis is z axis
+    end
+end
+
+end
\ No newline at end of file