diff --git a/IK_Last_version/Damped_LS.m b/IK_Last_version/Damped_LS.m
deleted file mode 100644
index 99c1201..0000000
--- a/IK_Last_version/Damped_LS.m
+++ /dev/null
@@ -1,28 +0,0 @@
-function [q, q_dot] = Damped_LS(q_0, link_lengths, p_global)
-deltaT = 1;
-k = 1000;
-
-u = sqrt(0.001);
-%% Calculating the jacobian
-J = Jacobian(q_0, link_lengths);
-
-
-%% Calculating the forward kinematics
-[~, ~, ~, ~, ~, ~, ~, cur_pos] = FK(q_0, link_lengths);
-
-%% Getting the r vector
-r = p_global - cur_pos;
-
-%% Numerical differentiation
-r_dot = r./k; % To decrease the step that is taken by the velocity, k is some large constant
-
-%% Calculate the J_inverse
-J_DLS= J'/(J*J' + u^2*eye(6));
-
-%% Calculating the q_dot
-q_dot = J_DLS * r_dot;
-
-%% Integration
-q = q_0+ (q_dot .* deltaT)';
-end
-
diff --git a/IK_Last_version/FK.m b/IK_Last_version/FK.m
index afdff81..8faf694 100644
--- a/IK_Last_version/FK.m
+++ b/IK_Last_version/FK.m
@@ -1,6 +1,5 @@
-function cur_pos = FK(q,r, leg)
-global L_sh L_th_f L_h L_th_h L_th_h_mid L_b W_b
-
+function pos = FK(q,r)
+global L_b W_b
r_p = r(1:3); % pelvis position
x_p = r_p(1);
y_p = r_p(2);
@@ -15,69 +14,32 @@
R_p_z = rot(3,psi_p,4);
R0 = R_p_z*R_p_y*R_p_x;
P_p = trans([],r_p);
+P_shd_hr = trans([],[-L_b/2,-W_b/2,0]);
+
+q1_hl = q(1);
+q2_hl = q(2);
+q3_hl = q(3);
+q4_hl = q(4);
+
+Rq1_hr = rot(1,q1_hl,4);
+Rq2_hr = rot(2,q2_hl,4);
+Rq3_hr = rot(2,q3_hl,4);
+Rq4_hr = rot(2,q4_hl,4);
+
+L_h = [53.26, 61.34, 0.17]/1000;
+L_th_h_mid = [0.0, 92.88, 201.0]/1000;
+L_th_h = [0.0, 52.62, 246.5]/1000;
+% Adjusted L_sh to put foot under frame 4 in zero pose, as discussed.
+L_sh = [0.0, 0.0, norm([77.21, 190.62])]/1000;
+P_hip_hr = trans([],[-L_h(1);-L_h(2);-L_h(3)]);
+P_knee_mid_hr = trans([],[-L_th_h_mid(1);-L_th_h_mid(2);-L_th_h_mid(3)]);
+P_knee_hr = trans([],[-L_th_h(1);-L_th_h(2);-L_th_h(3)]);
+P_ankle_hr = trans([],[-L_sh(1);-L_sh(2);-L_sh(3)]);
-if leg==1
- q1_hl = q(1);
- q2_hl = q(2);
- q3_hl = q(3);
- q4_hl = q(4);
-
- Rq1_hl = rot(1,q1_hl,4);
- Rq2_hl = rot(2,q2_hl,4);
- Rq3_hl = rot(2,q3_hl,4);
- Rq4_hl = rot(2,q4_hl,4);
-
- P_shd_hl = trans([],[-L_b/2,W_b/2,0]);
- P_hip_hl = trans([],[-L_h(1);L_h(2);-L_h(3)]);
- P_knee_mid_hl = trans([],[-L_th_h_mid(1);L_th_h_mid(2);-L_th_h_mid(3)]);
- P_knee_hl = trans([],[-L_th_h(1);L_th_h(2);-L_th_h(3)]);
- P_ankle_hl = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]);
- cur_pos = P_p*R0*P_shd_hl*Rq1_hl*P_hip_hl*Rq2_hl*P_knee_mid_hl*Rq3_hl*P_knee_hl*Rq4_hl*P_ankle_hl*[0;0;0;1];
-elseif leg==2
- q1_hr = q(1);
- q2_hr = q(2);
- q3_hr = q(3);
- q4_hr = q(4);
-
- Rq1_hr = rot(1,q1_hr,4);
- Rq2_hr = rot(2,q2_hr,4);
- Rq3_hr = rot(2,q3_hr,4);
- Rq4_hr = rot(2,q4_hr,4);
-
- P_shd_hr = trans([],[-L_b/2,W_b/2,0]);
- P_hip_hr = trans([],[-L_h(1);L_h(2);-L_h(3)]);
- P_knee_mid_hr = trans([],[-L_th_h_mid(1);L_th_h_mid(2);-L_th_h_mid(3)]);
- P_knee_hr = trans([],[-L_th_h(1);L_th_h(2);-L_th_h(3)]);
- P_ankle_hr = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]);
- cur_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*Rq4_hr*P_ankle_hr*[0;0;0;1];
-elseif leg==0
- q1_fr = q(1);
- q2_fr = q(2);
- q3_fr = q(3);
-
- Rq1_fr = rot(1,q1_fr,4);
- Rq2_fr = rot(2,q2_fr,4);
- Rq3_fr = rot(2,q3_fr,4);
-
- P_shd_fr = trans([],[L_b/2,-W_b/2,0]);
- P_hip_fr = trans([],[L_h(1);-L_h(2);-L_h(3)]);
- P_knee_fr = trans([],[L_th_f(1);-L_th_f(2);-L_th_f(3)]);
- P_ankle_fr = trans([],[-L_sh(1);-L_sh(2);-L_sh(3)]);
- cur_pos= P_p*R0*P_shd_fr*Rq1_fr*P_hip_fr*Rq2_fr*P_knee_fr*Rq3_fr*P_ankle_fr*[0;0;0;1];
-else
- q1_fl = q(1);
- q2_fl = q(2);
- q3_fl = q(3);
-
- Rq1_fl = rot(1,q1_fl,4);
- Rq2_fl = rot(2,q2_fl,4);
- Rq3_fl = rot(2,q3_fl,4);
-
- P_shd_fl = trans([],[L_b/2,W_b/2,0]);
- P_hip_fl = trans([],[L_h(1);L_h(2);-L_h(3)]);
- P_knee_fl = trans([],[L_th_f(1);L_th_f(2);-L_th_f(3)]);
- P_ankle_fl = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]);
-
- cur_pos= P_p*R0*P_shd_fl*Rq1_fl*P_hip_fl*Rq2_fl*P_knee_fl*Rq3_fl*P_ankle_fl*[0;0;0;1];
-end
+shoulder_pos = P_p*R0*P_shd_hr*[0;0;0;1];
+hip_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*[0;0;0;1];
+knee_mid_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*[0;0;0;1];
+knee_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*[0;0;0;1];
+anlke_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*Rq4_hr*P_ankle_hr*[0;0;0;1];
+pos = [shoulder_pos(1:3), hip_pos(1:3), knee_mid_pos(1:3), knee_pos(1:3), anlke_pos(1:3)];
end
\ No newline at end of file
diff --git a/IK_Last_version/FK_old.m b/IK_Last_version/FK_old.m
deleted file mode 100644
index 520cd77..0000000
--- a/IK_Last_version/FK_old.m
+++ /dev/null
@@ -1,29 +0,0 @@
-function Pos = FK(q)
-global L1 L2 L3 L4
-
-q1 = q(1);
-q2 = q(2);
-q3 = q(3);
-q4 = q(4);
-%% Rotation
-Rq1 = rot(1,q1,4);
-Rq2 = rot(2,q2,4);
-Rq3 = rot(2,q3,4);
-Rq4 = rot(2,q4,4);
-
-%% Translation
-P_shd = trans([],[0;0;0]);
-P_hip = trans([],-L1);
-P_knee = trans(3,-L2);
-P_knee_mid = trans(3,-L3);
-P_ankle = trans([],-L4);
-
-%% Transformations
-r_shd = P_shd*[0;0;0;1];
-r_hip = P_shd*Rq1*P_hip*[0;0;0;1];
-r_knee = P_shd*Rq1*P_hip*Rq2*P_knee*[0;0;0;1];
-r_knee_mid = P_shd*Rq1*P_hip*Rq2*P_knee*Rq3*P_knee_mid*[0;0;0;1];
-r_ankle = P_shd*Rq1*P_hip*Rq2*P_knee*Rq3*P_knee_mid*Rq4*P_ankle*[0;0;0;1];
-
-Pos = [r_shd(1:3);r_hip(1:3);r_knee(1:3);r_knee_mid(1:3);r_ankle(1:3)];
-end
diff --git a/IK_Last_version/Knee_pos.m b/IK_Last_version/Knee_pos.m
deleted file mode 100644
index 4a4c1e0..0000000
--- a/IK_Last_version/Knee_pos.m
+++ /dev/null
@@ -1,7 +0,0 @@
-function Pos = Knee_pos(ankle_pos,alpha)
-global L4
-y = ankle_pos(2)+L4(2);
-x = ankle_pos(1)+L4(1)*cos(alpha)-L4(3)*sin(alpha);
-z = ankle_pos(3)+L4(1)*sin(alpha)+L4(3)*cos(alpha);
-Pos = [x,y,z];
-end
diff --git a/IK_Last_version/LICENSE b/IK_Last_version/LICENSE
deleted file mode 100644
index b86b789..0000000
--- a/IK_Last_version/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2022 Walid Shaker
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/IK_Last_version/README.md b/IK_Last_version/README.md
deleted file mode 100644
index a4c9117..0000000
--- a/IK_Last_version/README.md
+++ /dev/null
@@ -1,50 +0,0 @@
-[[Demo]](https://youtu.be/QU4yxvBdSNA)
-
-## 7DOF-KUKA-Linear-Axis-Forward-and-Inverse-Kinematics
-KUKA on linear axis includes a further axis to the robot, so it is considerably extending the workspace of the robot. The advantages of the redundant robots are increasing manipulability in specified directions, uniform distribution of velocities and accelerations, minimizing energy consumption, optimizing execution time, etc. However, there are also such drawbacks, as complicated calculations for IK and motion control, and greater structural complexity of construction.
-
-A manipulator is kinematically redundant when it possesses more degrees of freedom than it is needed to execute a given task. In other words, there are more local coordinate variables than global. The Redundancy Resolution is necessary because it allows to avoid singularity, obstacles and to smooth manipulation around the workspace. The solution for the redundancy is a cost function optimization, where the cost function can be i) energy-based or ii) minimizing the distance. The Redundancy Resolution includes three methods, which are
-- Jacobian-based (Damped Least Square and Weighted Pseudoinverse)
-- Null Space
-- Task Augmentation
-
-In this repository, the implementation of forward and inverse kinematics by redundancy resolution is presented.
-
-
- 
- 
-
-
-### Forward Kinematics can be written as follows:
-
-
-## Redundancy Resolution
-
- 
- 
-
-
-
- 
- 
-
-
-
- 
- 
-
-
-Full video is attached [Demo](https://youtu.be/QU4yxvBdSNA)
-
-## Accuracy and Execution Time
-
- 
- 
-
-
-## How to run
-First open **IK.m** file and select the preferable redundancy resolution approach as shown below.
-
-
-Then run **main_redundancy_resolution.m** file for calculations and visualization.
-
diff --git a/IK_Last_version/Visualize_Robot.m b/IK_Last_version/Visualize_Robot.m
index ef750b3..f54232d 100644
--- a/IK_Last_version/Visualize_Robot.m
+++ b/IK_Last_version/Visualize_Robot.m
@@ -1,10 +1,15 @@
-function [] = Visualize_Robot(Pos, p_global, color_list)
+function [] = Visualize_Robot(body_pos, foot_Pos, p_global, color_list)
limit = 1.3;
-r_shd = Pos(1:3);
-r_hip = Pos(4:6);
-r_knee_mid = Pos(7:9);
-r_knee = Pos(10:12);
-r_ankle = Pos(13:15);
+r_shd = foot_Pos(1:3);
+r_hip = foot_Pos(4:6);
+r_knee_mid = foot_Pos(7:9);
+r_knee = foot_Pos(10:12);
+r_ankle = foot_Pos(13:15);
+
+r_shd_hr = body_pos(1:3);
+r_shd_hl = body_pos(4:6);
+r_shd_fr = body_pos(7:9);
+r_shd_fl = body_pos(10:12);
%% Plotting
hold on
@@ -19,7 +24,6 @@
config_joint_3 = r_knee;
config_joint_4 = r_ankle;
-% if flag==0
line([config_joint_0(1) config_joint_1(1)],[config_joint_0(2) config_joint_1(2)],...
[config_joint_0(3) config_joint_1(3)],'linewidth', 1,'Color',color_list{1}) %, 'LineStyle', '--'
line([config_joint_1(1) config_joint_2(1)],[config_joint_1(2) config_joint_2(2)],...
@@ -28,16 +32,6 @@
[config_joint_2(3) config_joint_3(3)],'linewidth', 1,'Color',color_list{3})
line([config_joint_3(1) config_joint_4(1)],[config_joint_3(2) config_joint_4(2)],...
[config_joint_3(3) config_joint_4(3)],'linewidth', 1,'Color',color_list{4})
-% else
-% line([config_joint_0(1) config_joint_1(1)],[config_joint_0(2) config_joint_1(2),],...
-% [config_joint_0(3) config_joint_1(3)],'linewidth', 1,'Color',color_list{1},'LineStyle', '--') %, 'LineStyle', '--'
-% line([config_joint_1(1) config_joint_2(1)],[config_joint_1(2) config_joint_2(2)],...
-% [config_joint_1(3) config_joint_2(3)],'linewidth', 1,'Color',color_list{2},'LineStyle', '--')
-% line([config_joint_2(1) config_joint_3(1)],[config_joint_2(2) config_joint_3(2)],...
-% [config_joint_2(3) config_joint_3(3)],'linewidth', 1,'Color',color_list{3},'LineStyle', '--')
-% line([config_joint_3(1) config_joint_4(1)],[config_joint_3(2) config_joint_4(2)],...
-% [config_joint_3(3) config_joint_4(3)],'linewidth', 1,'Color',color_list{4},'LineStyle', '--')
-% end
plot3(config_joint_0(1),config_joint_0(2),config_joint_0(3),'go','linewidth', 4,'MarkerSize',4)
plot3(config_joint_1(1),config_joint_1(2),config_joint_1(3),'go','linewidth', 4,'MarkerSize',4)
@@ -53,28 +47,12 @@
plot3(p_global(1),p_global(2),p_global(3),'r*')
-% text(1.5, 0, 2.8, 'Joint state', 'color', 'red', 'FontSize', 14)
-% str = {string(q(1)), string(q(2)), string(q(3)), string(q(4))};
-% text(1.6, 0, 1.5, str)
-%
-% text(2.5, 0, 2.4,'Joint state velocities', 'color', 'blue', 'FontSize', 14)
-% str = {string(q_dot(1)), string(q_dot(2)), string(q_dot(3)), string(q_dot(4))};
-% text(2.8, 0, 1.05, str)
-% %
-% % text(1.5, 0, 0, {'End-effector','position'}, 'color', 'red', 'FontSize', 14)
-% % str = {string(cur_pos(1)), string(cur_pos(2)), string(cur_pos(3)), string(cur_pos(4)), string(cur_pos(5)), string(cur_pos(6))};
-% % text(1.65, 0, -1.35, str)
-%
-% text(2.6, 0, -0.35, {'End-effector','desired position'}, 'color', 'blue', 'FontSize', 14)
-% str = {string(p_global(1)), string(p_global(2)), string(p_global(3))};
-% text(2.9, 0, -1.75, str)
-
+plot3([r_shd_hr(1) r_shd_fr(1) r_shd_fl(1) r_shd_hl(1) r_shd_hr(1)],[r_shd_hr(2) r_shd_fr(2) r_shd_fl(2) r_shd_hl(2) r_shd_hr(2)],[r_shd_hr(3) r_shd_fr(3) r_shd_fl(3) r_shd_hl(3) r_shd_hr(3)],'k','LineWidth',2);
xlim([-3,3])
ylim([-3,3])
zlim([-3,3])
xlabel('x')
ylabel('y')
zlabel('z')
-%title('KUKA on Linear Axis', 'FontSize', 14)
end
diff --git a/IK_Last_version/angle_joints.m b/IK_Last_version/angle_joints.m
deleted file mode 100644
index 288440c..0000000
--- a/IK_Last_version/angle_joints.m
+++ /dev/null
@@ -1,32 +0,0 @@
-function q = angle_joints(q1,p,dy,dz)
-
-global L1 L2 L3
-P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]);
-
-%% Front Right
-Rq1 = rot(1,q1,4);
-r_0_hip= Rq1*P_sho_hip*[0;0;0;1];
-
-delta_x= r_0_hip(1)-p(1);
-delta_y= r_0_hip(2)-p(2);
-delta_z= r_0_hip(3)-p(3);
-r_0 = sqrt(dy^2+dz^2-L1(2)^2);
-
-h_1 = sqrt(delta_x^2+r_0^2);
-phi = asin(delta_x/h_1);
-if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1
- ratio = 1;
-else
- ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2);
-end
-q2 = phi-acos(ratio);
-if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1
- ratio1 = -1;
-else
- ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3);
-end
-q3 = pi-acos(ratio1);
-
-q = [q2,q3];
-% pause
-end
\ No newline at end of file
diff --git a/IK_Last_version/angle_joints_old.m b/IK_Last_version/angle_joints_old.m
deleted file mode 100644
index 11ce586..0000000
--- a/IK_Last_version/angle_joints_old.m
+++ /dev/null
@@ -1,64 +0,0 @@
-function q = angle_joints(q1,p,dy,dz)
-
-global L1 L2 L3
-
-% %% Rotation Matrix
-% % Pelvis
-% R_p_x = rot(1,alpha_p,4);
-% R_p_y = rot(2,beta_p,4);
-% R_p_z = rot(3,gamma_p,4);
-% R0=R_p_y*R_p_z*R_p_x; % Pelvis Rotation Matrix
-
-% % Front Right
-% Rq1 = rot(1,q1,4);
-% %% Translation Matrix
-% P_p = trans([],r_p);
-% % Front Right
-% P_b_sho_fr = trans([],0.5*[L_b,-W_b,0]);
-% P_b_sho_fl = trans([],0.5*[L_b,W_b,0]);
-% P_b_sho_hr = trans([],0.5*[-L_b,-W_b,0]);
-% P_b_sho_hl = trans([],0.5*[-L_b,W_b,0]);
-P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]);
-% P_sho_hip_fl = trans([],[L_h(1),L_h(2),-L_h(3)]);
-% P_sho_hip_hr = trans([],[-L_h(1),-L_h(2),-L_h(3)]);
-% P_sho_hip_hl = trans([],[-L_h(1),L_h(2),-L_h(3)]);
-
-% r_0_sho_fr = P_p*R0*P_b_sho_fr*[0;0;0;1];
-% r_0_sho_fl = P_p*R0*P_b_sho_fl*[0;0;0;1];
-% r_0_sho_hr = P_p*R0*P_b_sho_hr*[0;0;0;1];
-% r_0_sho_hl = P_p*R0*P_b_sho_hl*[0;0;0;1];
-%% Front Right
-% delta_y = -p(2);
-% delta_z = -p(3);
-%
-% h = sqrt(delta_y^2+delta_z^2);
-% q1= (atan(delta_y/delta_z)-asin(L1(2)/h));
-% r_0 = sqrt(delta_y^2+delta_z^2-L1(2)^2);
-
-Rq1 = rot(1,q1,4);
-r_0_hip= Rq1*P_sho_hip*[0;0;0;1];
-
-delta_x= r_0_hip(1)-p(1);
-delta_y= r_0_hip(2)-p(2);
-delta_z= r_0_hip(3)-p(3);
-r_0 = sqrt(dy^2+dz^2-L1(2)^2);
-
-% delta_x_hip = -r_0_hip(1)+p(1);
-h_1 = sqrt(delta_x^2+r_0^2);
-phi = asin(delta_x/h_1);
-if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1
- ratio = 1;
-else
- ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2);
-end
-q2 = phi-acos(ratio);
-if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1
- ratio1 = -1;
-else
- ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3);
-end
-q3 = pi-acos(ratio1);
-
-q = [q2,q3];
-% pause
-end
\ No newline at end of file
diff --git a/IK_Last_version/angle_joints_stefan.m b/IK_Last_version/angle_joints_stefan.m
deleted file mode 100644
index 9035d8e..0000000
--- a/IK_Last_version/angle_joints_stefan.m
+++ /dev/null
@@ -1,265 +0,0 @@
-function q = angle_joints(p)
-r_p = p(1:3); % Base position
-x_p = r_p(1);
-y_p = r_p(2);
-z_p = r_p(3);
-
-q_p = p(4:6); % Base rotation
-theta_p = q_p(1);
-phi_p = q_p(2);
-psi_p = q_p(3);
-
-r_fr = p(7:9); % Front Right foot position
-x_fr = r_fr(1);
-y_fr = r_fr(2);
-z_fr = r_fr(3);
-
-r_fl = p(10:12); % Front Left foot position
-x_fl = r_fl(1);
-y_fl = r_fl(2);
-z_fl = r_fl(3);
-
-r_hr = p(13:15); % Hind Right foot position
-x_hr = r_hr(1);
-y_hr = r_hr(2);
-z_hr = r_hr(3);
-
-r_hl = p(16:18); % Hind Left foot position
-x_hl = r_hl(1);
-y_hl = r_hl(2);
-z_hl = r_hl(3);
-
-%% Global
-global L_h L_b W_b % Base (Body) dimensions
-global redundant
-% global L_sh L_th_f L_th_h L_th_h_mid
-%% Rotation Matrix
-% Body Rotation Matrix
-R_p_x = rot(1,theta_p,4);
-R_p_y = rot(2,phi_p,4);
-R_p_z = rot(3,psi_p,4);
-R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix
-
-%% Translation Matrix
-% Base in the World frame
-P_p = trans([],r_p);
-
-% Translation from Base to Shoulder joint
-P_b_sho_fr = trans([],0.5*[L_b,-W_b,0]);
-P_b_sho_fl = trans([],0.5*[L_b,W_b,0]);
-P_b_sho_hr = trans([],0.5*[-L_b,-W_b,0]);
-P_b_sho_hl = trans([],0.5*[-L_b,W_b,0]);
-
-% Translation from Shoulder to Hip joint
-P_sho_hip_fr = trans([],[L_h(1),-L_h(2),-L_h(3)]);
-P_sho_hip_fl = trans([],[L_h(1),L_h(2),-L_h(3)]);
-P_sho_hip_hr = trans([],[-L_h(1),-L_h(2),-L_h(3)]);
-P_sho_hip_hl = trans([],[-L_h(1),L_h(2),-L_h(3)]);
-% Position of Shoulder joint in the World frame
-r_0_sho_fr = P_p*R0*P_b_sho_fr*[0;0;0;1];
-r_0_sho_fl = P_p*R0*P_b_sho_fl*[0;0;0;1];
-r_0_sho_hr = P_p*R0*P_b_sho_hr*[0;0;0;1];
-r_0_sho_hl = P_p*R0*P_b_sho_hl*[0;0;0;1];
-%% Front Right
-L2_f = 246.5/1000; % Vertical distance between Shoulder and Knee Joints
-L3_f = 280.67/1000; % Vertical distance between Knee and Foot Joints
-% off_y = 143.34/1000;
-
-delta_y = r_0_sho_fr(2)-y_fr; % SV: removed abs()
-% delta_y = abs(r_0_sho_fr(2)-y_fr);
-delta_z = r_0_sho_fr(3)-z_fr;
-
-h_fr = sqrt(delta_y^2+delta_z^2);
-q1_fr= -atan2(delta_y,delta_z)+asin(L_h(2)/h_fr); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different)
-% q1_fr = atan(delta_y/delta_z)-asin(L_h(2)/h);
-r_0 = sqrt(h_fr^2-L_h(2)^2);
-
-Rq1_fr = rot(1,q1_fr,4);
-r_0_hip= P_p*R0*P_b_sho_fr*Rq1_fr*P_sho_hip_fr*[0;0;0;1];
-
-delta_x_hip = x_fr-r_0_hip(1);
-
-h_1 = sqrt(delta_x_hip^2+r_0^2);
-phi = asin(delta_x_hip/h_1);
-q2_fr = acos((h_1^2+L2_f^2-L3_f^2)/(2*h_1*L2_f))-phi;
-q3_fr = acos((L3_f^2+L2_f^2-h_1^2)/(2*L3_f*L2_f))-pi;
-%% Front Left
-delta_y = r_0_sho_fl(2)-y_fl; % SV: removed abs()
-% delta_y = abs(r_0_sho_fl(2)-y_fl);
-delta_z = r_0_sho_fl(3)-z_fl;
-
-h_fl = sqrt(delta_y^2+delta_z^2);
-q1_fl= -atan2(delta_y,delta_z)-asin(L_h(2)/h_fl); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different)
-% q1_fl = atan(delta_y/delta_z)-asin(L_h(2)/h_fl);
-% q1_fl =0;
-r_0 = sqrt(h_fl^2-L_h(2)^2);
-
-Rq1_fl = rot(1,q1_fl,4);
-r_0_hip= P_p*R0*P_b_sho_fl*Rq1_fl*P_sho_hip_fl*[0;0;0;1];
-
-delta_x_hip = x_fl-r_0_hip(1);
-
-h_1 = sqrt(delta_x_hip^2+r_0^2);
-phi = asin(delta_x_hip/h_1);
-q2_fl = acos((h_1^2+L2_f^2-L3_f^2)/(2*h_1*L2_f))-phi;
-q3_fl = acos((L3_f^2+L2_f^2-h_1^2)/(2*L3_f*L2_f))-pi;
-
-%% Hind Right
-if redundant == 1 % Hind legs with redundancy
- alpha = 10*pi/180; % alpha = q2+q3+q4
- L2_h = 201/1000; % Vertical distance between Shoulder and Knee Joint (Hind leg)
- L3_h = 246.5/1000; % Vertical distance between Knee and Ankle Joint (Hind leg)
-
- delta_y = r_0_sho_hr(2)-y_hr; % SV: removed abs()
-% delta_y = abs(r_0_sho_hr(2)-y_hr);
- delta_z = abs(r_0_sho_hr(3)-z_hr);
-
- h = sqrt(delta_y^2+delta_z^2);
- q1_hr= -atan2(delta_y,delta_z)+asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different)
-% q1_hr= atan(delta_y/delta_z)-asin((L_h(2))/h);
- Rq1_hr = rot(1,q1_hr,4);
- r_0_hip= P_p*R0*P_b_sho_hr*Rq1_hr*P_sho_hip_hr*[0;0;0;1];
-
- L4 = [0 0 190.62]/1000; % Vertical distance from Knee to foot
-
- % SV: new x-r plane calcs
- dx = x_hr - r_0_hip(1); % SV: hip to foot dx
- dy = y_hr - r_0_hip(2); % SV: hip to foot dy
- dz = z_hr - r_0_hip(3); % SV: hip to foot dz
-
- dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip
-
- dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx
- dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr
-
- h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane
- phi = asin(dx_ankle/h_1); % angle to h_1 from r
-
- % knee_hr = Knee_pos(r_hr,alpha,1,L4);
- % delta_x= r_0_hip(1)-knee_hr(1);
- % dy=abs(r_0_sho_hr(2)-knee_hr(2));
- % dz=abs(r_0_sho_hr(3)-knee_hr(3));
- % r_0 = sqrt(dy^2+dz^2-L_h(2)^2);
- % h_1 = sqrt(delta_x^2+r_0^2);
- % phi = asin(delta_x/h_1);
- % L2 = 201/1000;
- % L3 = 246.5/1000;
-
- if abs((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))>1
- ratio = 1;
- else
- ratio = (h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h);
- end
- q2_hr = -phi-acos(ratio); % SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip
- % q2_hr = phi-acos(ratio);
- if abs((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))>1
- ratio1 = 1; % SV: changed sign, should be >0 now like ratio above
- else
- ratio1 = (L3_h^2+L2_h^2-h_1^2)/(2*L2_h*L3_h);
- end
- q3_hr = pi-acos(ratio1);
- q4_hr= -q2_hr-q3_hr-alpha;
- %% Hind Left
- delta_y = r_0_sho_hl(2)-y_hl; % SV: removed abs()
-% delta_y = abs(r_0_sho_hl(2)-y_hl);
- delta_z = abs(r_0_sho_hl(3)-z_hl);
-
- h = sqrt(delta_y^2+delta_z^2);
- q1_hl = -atan2(delta_y,delta_z)-asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different)
-% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h);
- Rq1_hl = rot(1,q1_hl,4);
- r_0_hip= P_p*R0*P_b_sho_hl*Rq1_hl*P_sho_hip_hl*[0;0;0;1];
-
- % SV: new x-r plane calcs
- dx = x_hl - r_0_hip(1); % SV: hip to foot dx
- dy = y_hl - r_0_hip(2); % SV: hip to foot dy
- dz = z_hl - r_0_hip(3); % SV: hip to foot dz
-
- dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip
-
- dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx
- dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr
-
- h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane
- phi = asin(dx_ankle/h_1); % angle to h_1 from r
-
- % knee_hl = Knee_pos(r_hl,alpha,0,L4);
- % delta_x= r_0_hip(1)-knee_hl(1);
- % dy=abs(r_0_sho_hl(2)-knee_hl(2));
- % dz=abs(r_0_sho_hl(3)-knee_hl(3));
- % r_0 = sqrt(dy^2+dz^2-L_h(2)^2);
- % h_1 = sqrt(delta_x^2+r_0^2);
- % phi = asin(delta_x/h_1);
- % L2 = 201/1000;
- % L3 = 246.5/1000;
- if abs((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))>1
- ratio = 1; % SV: changed sign, should be >0 now like right leg above
- % ratio = -1;
- else
- ratio = (h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h);
- end
- q2_hl = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip
- % q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%%
- if abs((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))>1
- ratio1 = 1;
- else
- ratio1 = (L3_h^2+L2_h^2-h_1^2)/(2*L2_h*L3_h);
- end
- q3_hl = pi-acos(ratio1);
- q4_hl= -q2_hl-q3_hl-alpha;
-else
- %% Hind Right
- L2_h = 246.5/1000;
- L3_h = 190.62/1000;
-% off_h = 206.84/1000;
-% off_h = 145/1000;
-
- delta_y = r_0_sho_hr(2)-y_hr; % SV: removed abs()
-% delta_y = abs(r_0_sho_hr(2)-y_hr);
- delta_z = abs(r_0_sho_hr(3)-z_hr);
-
- h = sqrt(delta_y^2+delta_z^2);
- q1_hr= -atan2(delta_y,delta_z)+asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different)
-% q1_hr= atan(delta_y/delta_z)-asin((L_h(2))/h);
- r_0 = sqrt(delta_y^2+delta_z^2-(L_h(2))^2);
-
- Rq1_hr = rot(1,q1_hr,4);
- r_0_hip= P_p*R0*P_b_sho_hr*Rq1_hr*P_sho_hip_hr*[0;0;0;1];
-
- delta_x_hip = x_hr-r_0_hip(1);
-
- h_1 = sqrt(delta_x_hip^2+r_0^2);
- phi = asin(delta_x_hip/h_1);
- q3_hr = acos((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))-phi+pi/2; %
- q4_hr = acos((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))-pi;
- q2_hr= -pi/2;
- %% Hind Left
- delta_y = r_0_sho_hl(2)-y_hl; % SV: removed abs()
-% delta_y = abs(r_0_sho_hl(2)-y_hl);
- delta_z = r_0_sho_hl(3)-z_hl;
-
- h = sqrt(delta_y^2+delta_z^2);
- q1_hl = -atan2(delta_y,delta_z)-asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different)
-% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h);
- r_0 = sqrt(delta_y^2+delta_z^2-(L_h(2))^2);
-
- Rq1_hl = rot(1,q1_hl,4);
- r_0_hip= P_p*R0*P_b_sho_hl*Rq1_hl*P_sho_hip_hl*[0;0;0;1];
-
- delta_x_hip = x_hl-r_0_hip(1);
-
- h_1 = sqrt(delta_x_hip^2+r_0^2);
- phi = asin(delta_x_hip/h_1);
- q3_hl = acos((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))-phi+pi/2;
- q4_hl = acos((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))-pi;
- q2_hl= -pi/2;
-
-end
-%% Output
-q_fr = [q1_fr q2_fr q3_fr]'; % Right Angles
-q_fl = [q1_fl q2_fl q3_fl]'; % Right Angles
-q_hr = [q1_hr q2_hr q3_hr q4_hr]'; % Right Angles
-q_hl = [q1_hl q2_hl q3_hl q4_hl]'; % Right Angles
-
-q = [q_fr;q_fl;q_hr;q_hl];
-end
\ No newline at end of file
diff --git a/IK_Last_version/check_security.m b/IK_Last_version/check_security.m
deleted file mode 100644
index cf10dc5..0000000
--- a/IK_Last_version/check_security.m
+++ /dev/null
@@ -1,16 +0,0 @@
-function y = check_security(q)
-y= zeros(2,4);
-global lower_joint_limits upper_joint_limits
-for i=1:4
- if (q(i) - upper_joint_limits(i) >= 0)
- y(1,i) = 0;
- else
- y(1,i)=1;
- end
- if (q(i) - lower_joint_limits(i) <= 0)
- y(2,i) = 0;
- else
- y(2,i)=1;
- end
-end
-end
\ No newline at end of file
diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m
index dd80b85..506a5c6 100644
--- a/IK_Last_version/main.m
+++ b/IK_Last_version/main.m
@@ -1,74 +1,147 @@
-%% Redundancy Resolution
-clear all;
-close all;
-clc;
-
-global L1 L2 L3 L4
-
-L1= [53.26 61.84 0.1]/1000; % Length of hip
-L2= 201/1000; % Length of hind thigh [0 46.5 201]
-L3= 246.5/1000; % Length of middle hind thigh [0 63.5 246.5]
-% L4=[-77.18 -145 -280.52]/1000; % Length of shank
-L4=[0 0 190.62]/1000; % Length of shank 280.52 190.62
-
-color_list = {'blue', 'red','green', 'black'};
-
-% p_global = [-53.2600 -61.8400 -638.2200]'/1000; % zero position -728.12
-% p_global = [-130.61 -95.84 -620.05]'/1000;
-% p_global = [-226.55 -153.84 -463.83]'/1000;
-p_global = [192.46 -206.84 18.8]'/1000;
-
-r_0_sho = [-0,-0,0]'/1000;
-alpha =10*pi/180;
-offset_y = 145/1000;
-
-delta_y = r_0_sho(2)-p_global(2); % SV: removed abs()
-% delta_y = abs(r_0_sho_hl(2)-y_hl);
-delta_z = abs(r_0_sho(3)-p_global(3));
-
-h = sqrt(delta_y^2+delta_z^2);
-q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different)
-% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h);
-Rq1 = rot(1,q1,4);
-P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]);
-
-r_0_hip= Rq1*P_sho_hip*[0;0;0;1];
-
-% SV: new x-r plane calcs
-dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx
-dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy
-dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz
-
-dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip
-
-dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx
-dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr
-
-h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane
-phi = asin(dx_ankle/h_1); % angle to h_1 from r
-
-
-if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1
- ratio = 1; % SV: changed sign, should be >0 now like right leg above
- q2 =0;
-else
- ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2);
- q2 = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip
-end
-% q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%%
-if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1
- ratio1 = 1;
- q3=0;
-else
- ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3);
- q3 = pi-acos(ratio1);
-end
-
-q4= -q2-q3-alpha;
-q_new =[q1,q2,q3,q4];
-Pos = FK(q_new); %q_new
-figure
-view(3)
-Visualize_Robot(Pos, p_global, color_list) %Pos(:,3)
-pause(0.1);
-
+%% Redundancy Resolution
+clear all;
+close all;
+clc;
+
+global L1 L2 L3 L4 L_b W_b
+
+L1 = [53.26 206.84 0.1]/1000; % Length of hip
+L2 = 201/1000; % Length of hind thigh
+L3 = 246.5/1000; % Length of middle hind thigh
+% SV: As discussed, will set the zero position to the point with frame 5
+% directly under frame 4 (on z4 axis). Length between frame 4 and 5 then equal
+% to sqrt(77.21^2 + 190.62^2) = 205.66 mm.
+L4 = norm([77.21 0 190.62])/1000; % Length of shank dx = 77.21/1000
+L_b = 527/1000; % Length of body
+W_b = 245.81/1000; % Width of body
+
+color_list = {'blue', 'red','green', 'black'};
+
+%% Position of the base
+r_p = [0.0 0.0 0.0]'/1000;
+angle_p = [0.0 0.0 0.0]'*pi/180;
+R_p_x = rot(1,angle_p(1),4);
+R_p_y = rot(2,angle_p(2),4);
+R_p_z = rot(3,angle_p(3),4);
+R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix
+
+base_pos = [r_p, angle_p];
+%%
+P_p = trans([],r_p);
+
+r_b_sho_hr = trans([],[-L_b/2,-W_b/2,0]);
+r_b_sho_hl = trans([],[-L_b/2,W_b/2,0]);
+r_b_sho_fr = trans([],[L_b/2,-W_b/2,0]);
+r_b_sho_fl = trans([],[L_b/2,W_b/2,0]);
+
+r_w_sho_hr = P_p*R0*r_b_sho_hr*[0;0;0;1];
+r_w_sho_hl = P_p*R0*r_b_sho_hl*[0;0;0;1];
+r_w_sho_fr = P_p*R0*r_b_sho_fr*[0;0;0;1];
+r_w_sho_fl = P_p*R0*r_b_sho_fl*[0;0;0;1];
+
+body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)];
+%% Position of foot
+p_global = [-393.96+77.21 -329.74 -638.29-(norm([77.21, 190.62])-190.62)]'/1000; % adjusted zero position
+% p_global = [-393.96+77.21 -329.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000;
+% p_global = [-393.96+77.21 -329.74 -138.29-(norm([77.21, 190.62])-190.62)]'/1000;
+% p_global = [-393.96+77.21 -429.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000;
+% p_global = [-193.96+77.21 -329.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000;
+% p_global = [-393.96+77.21 -329.74 600.00]'/1000;
+
+% p_global = [-393.96 -329.74 -638.29]'/1000; % old zero position
+% p_global = [-393.96 -229.74 -638.29]'/1000;
+% p_global = [-323.96 -229.74 -438.29]'/1000;
+% p_global = [-393.96 -329.74 -38.29]'/1000;
+% p_global = [-323.96 -229.74 -188.29]'/1000;
+
+%%
+
+alpha = 0*pi/180:-0.2*pi/180:-75*pi/180;
+% TODO: cleanup commented code after testing
+% delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs()
+% delta_z = abs(r_w_sho_hr(3)-p_global(3));
+
+% vector from joint 1 to 5 (foot) in joint 1_hr frame:
+p_1_5_hr = transpose(R0(1:3,1:3)) * (p_global - r_w_sho_hr(1:3));
+
+% h = sqrt(delta_y^2+delta_z^2);
+% q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign
+% => q1 should be positive when hip above shoulder (right and left now different)
+
+% h using joint 1 frame instead of delta_y & z:
+h = sqrt(p_1_5_hr(2)^2 + p_1_5_hr(3)^2);
+
+% hr q1 using joint 1 frame and with q1 positive when hip above shoulder
+% (right and left now different). atan2 discontinuity rotated to an
+% unattainable point (along y_1 axis) (by -pi/2):
+q1 = atan2(-p_1_5_hr(3), -p_1_5_hr(2))+ asin(L1(2)/h) - pi/2;
+
+% Renamed several for consistency with writeup notation & clarity.
+R_1_2_hr = rot(1,q1,4);
+R_0_2_hr = R0 * R_1_2_hr;
+
+P_1_2_hr = trans([],[-L1(1),-L1(2),-L1(3)]);
+p_0_2_hr = P_p*R0*r_b_sho_hr*R_1_2_hr*P_1_2_hr*[0;0;0;1];
+
+% Vector from frame 2 to 5 in joint 2 frame:
+p_2_5_hr = transpose(R_0_2_hr(1:3,1:3)) * (p_global - p_0_2_hr(1:3));
+
+% % SV: new x-r plane calcs
+% dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx
+% dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy
+% dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz
+% dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip
+
+%%
+for i = 1: length(alpha)
+ % Vector from frame 5 to 4 (notice order) in joint 2 frame as a function of alpha:
+ p_2_5_4 = [-L4*sin(alpha(i)); 0; L4*cos(alpha(i))];
+
+ p_2_4_hr = p_2_5_hr + p_2_5_4; % Vector from frame 2 to 4 in joint 2 frame.
+
+% Angle between z_2 axis and p_2_4_hr, with dextral sign. Renamed from
+% phi to avoid confusion with body rotation angle.
+ beta = atan2(-p_2_4_hr(1), -p_2_4_hr(3));
+
+% dx_ankle = dx - L4(3)*sin(alpha(i)) + L4(1)*cos(alpha((i))); % hip to ankle dx
+% dr_ankle = dr_foot - L4(3)*cos(alpha(i)); % hip to ankle dr L4(3)*cos(alpha)
+
+% h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane
+% phi = asin(dx_ankle/h_1); % angle to h_1 from r
+
+% norm(p_2_4_hr) is the magnitude (2-norm) of the vector p_2_4_hr,
+% equivalent to h_1 previously used:
+% Also, I don't know if the domain check is still necessary:
+ if abs((norm(p_2_4_hr)^2+L2^2-L3^2)/(2*norm(p_2_4_hr)*L2))>1
+ ratio = 1;
+ q2 = 0;
+ else
+ ratio = (norm(p_2_4_hr)^2+L2^2-L3^2)/(2*norm(p_2_4_hr)*L2);
+ q2 = -acos(ratio) + beta; % Note change in sign because of orientation of beta.
+ end
+ % q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%%
+ if abs((L3^2+L2^2-norm(p_2_4_hr)^2)/(2*L3*L2))>1
+ ratio1 = 1;
+ q3 = 0;
+ else
+ ratio1 = (L3^2+L2^2-norm(p_2_4_hr)^2)/(2*L3*L2);
+ q3 = -acos(ratio1) + pi;
+ end
+
+ q4= -q2-q3-alpha(i);
+ q_new(i,:) =[q1,q2,q3,q4];
+ foot_Pos = FK(q_new(i,:), base_pos); %q_new
+ error(i) = sqrt((foot_Pos(1,5)-p_global(1))^2+(foot_Pos(2,5)-p_global(2))^2+(foot_Pos(3,5)-p_global(3))^2); % root square error
+end
+plot(alpha*180/pi, error)
+[minError, minIndex] = min(error);
+minAlpha = alpha(minIndex);
+fprintf("alpha = %.2f deg\nerror = %.2e m\n", minAlpha*180/pi, minError)
+foot_Pos_min = FK(q_new(minIndex,:), base_pos);
+
+figure
+view(3)
+Visualize_Robot(body_pos,foot_Pos_min, p_global, color_list) %Pos(:,3)
+pause(0.1);
+
+
diff --git a/IK_Last_version/quad_properties.m b/IK_Last_version/quad_properties.m
deleted file mode 100644
index 14ea7ca..0000000
--- a/IK_Last_version/quad_properties.m
+++ /dev/null
@@ -1,8 +0,0 @@
-% Biped Properties
-global L1 L2 L3 L4
-%% Length OK
-L1= [53.26 61.84 0.1]/1000; % Length of hip
-L2= 201/1000; % Length of hind thigh [0 46.5 201]
-L3= 246.5/1000; % Length of middle hind thigh [0 63.5 246.5]
-%L4=[77.18 145 280.52]/1000; % Length of shank
-L4=[77.18 145 190.62]/1000; % Length of shank
diff --git a/IK_Last_version/untitled1.zip b/IK_Last_version/untitled1.zip
deleted file mode 100644
index 3f8a86b..0000000
Binary files a/IK_Last_version/untitled1.zip and /dev/null differ