Skip to content

SV IK updates #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
TheIndoorDad wants to merge 13 commits into
base: main
Choose a base branch
from
Open

SV IK updates #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
b43ad3c
removing zip file
Boogard2021 Sep 6, 2024
0463fb2
updating the coordinate systems for solving IK
Boogard2021 Sep 6, 2024
a9658d1
add a for loop for checking the best alpha value
Boogard2021 Sep 10, 2024
cb807f4
SV comments on frame changes
TheIndoorDad Sep 10, 2024
4ba0c47
Update q1 calc to joint 1 frame
TheIndoorDad Sep 10, 2024
853d37b
Correction to q1 comp - R0 size.
TheIndoorDad Sep 10, 2024
47ad69c
Initial update of IK using new method - issue
TheIndoorDad Sep 10, 2024
0f9fecf
Modify L4 so zero pose with foot directly below frame 4.
TheIndoorDad Sep 11, 2024
6a57b34
Remove SV comment resolved after discussion.
TheIndoorDad Sep 11, 2024
90ae6c6
Adjusted L_sh to put foot under frame 4 in zero pose.
TheIndoorDad Sep 11, 2024
28a6d5c
Corrected beta calc.
TheIndoorDad Sep 17, 2024
23a3c3f
Corrected zero position and added printout.
TheIndoorDad Sep 17, 2024
86c560a
Minor update.
TheIndoorDad Sep 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 0 additions & 28 deletions IK_Last_version/Damped_LS.m
View file
Open in desktop

This file was deleted.

96 changes: 29 additions & 67 deletions IK_Last_version/FK.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
Expand All @@ -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
29 changes: 0 additions & 29 deletions IK_Last_version/FK_old.m
View file
Open in desktop

This file was deleted.

7 changes: 0 additions & 7 deletions IK_Last_version/Knee_pos.m
View file
Open in desktop

This file was deleted.

21 changes: 0 additions & 21 deletions IK_Last_version/LICENSE
View file
Open in desktop

This file was deleted.

50 changes: 0 additions & 50 deletions IK_Last_version/README.md
View file
Open in desktop

This file was deleted.

46 changes: 12 additions & 34 deletions IK_Last_version/Visualize_Robot.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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)],...
Expand All @@ -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)
Expand All @@ -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

32 changes: 0 additions & 32 deletions IK_Last_version/angle_joints.m
View file
Open in desktop

This file was deleted.

Loading