Skip to content

Commit 45cff84

Browse files
xaveaglexaveagle
committed
Adjusted Kinematics to allow for zero torque in 0 position
1 parent 528e17f commit 45cff84

File tree

5 files changed

+72
-21
lines changed

5 files changed

+72
-21
lines changed

‎Kinematics/LegFPK.m

+4-5
Original file line numberDiff line numberDiff line change
@@ -23,16 +23,15 @@
2323

2424
A1 = -deg2rad(90);
2525
A2 = deg2rad(90);
26-
d1 = -.161;
2726
a1 = .161;
2827
a2 = 1.5;
2928
a3 = 1.5;
3029

3130
%math stuff
32-
x = a1*sin(q1) - d1*sin(A2)*cos(q1) + a2*cos(q2)*sin(q1) + a3*cos(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)) - a3*sin(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) + a2*cos(A2)*cos(q1)*sin(q2)
33-
y = - d1*(cos(A2)*sin(A1) + cos(A1)*sin(A2)*sin(q1)) - a1*cos(A1)*cos(q1) - a2*sin(q2)*(sin(A1)*sin(A2) - cos(A1)*cos(A2)*sin(q1)) - a3*cos(q3)*(sin(q2)*(sin(A1)*sin(A2) - cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*cos(q2)) - a3*sin(q3)*(cos(q2)*(sin(A1)*sin(A2) - cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*sin(q2)) - a2*cos(A1)*cos(q1)*cos(q2)
34-
z = d1*(cos(A1)*cos(A2) - sin(A1)*sin(A2)*sin(q1)) - a1*sin(A1)*cos(q1) + a2*sin(q2)*(cos(A1)*sin(A2) + cos(A2)*sin(A1)*sin(q1)) + a3*cos(q3)*(sin(q2)*(cos(A1)*sin(A2) + cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*(cos(A1)*sin(A2) + cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*sin(q2)) - a2*sin(A1)*cos(q1)*cos(q2)
35-
31+
x = a3*sin(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) - a2*cos(q2)*sin(q1) - a3*cos(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)) - a1*sin(q1) - a2*cos(A2)*cos(q1)*sin(q2)
32+
y = a1*cos(A1)*cos(q1) - a2*sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - a3*cos(q3)*(sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*cos(q2)) - a3*sin(q3)*(cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*sin(q2)) + a2*cos(A1)*cos(q1)*cos(q2)
33+
z = a1*sin(A1)*cos(q1) + a2*sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + a3*cos(q3)*(sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*sin(q2)) + a2*sin(A1)*cos(q1)*cos(q2)
34+
3635
%returns tip
3736
Tip = [x,y,z]
3837

‎Kinematics/LegFTK.m

+6-5
Original file line numberDiff line numberDiff line change
@@ -27,11 +27,12 @@
2727
a2 = 1.5;
2828
a3 = 1.5;
2929

30-
J = [ a3*sin(q3)*(cos(q1)*sin(q2) + cos(A2)*cos(q2)*sin(q1)) - d1*sin(A2)*sin(q1) - a2*cos(q1)*cos(q2) - a3*cos(q3)*(cos(q1)*cos(q2) - cos(A2)*sin(q1)*sin(q2)) - a1*cos(q1) + a2*cos(A2)*sin(q1)*sin(q2), a3*sin(q3)*(cos(A1)*sin(q1)*sin(q2) - cos(A1)*cos(A2)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(A1)*cos(q2)*sin(q1) + cos(A1)*cos(A2)*cos(q1)*sin(q2)) - a1*cos(A1)*sin(q1) - a2*cos(A1)*cos(q2)*sin(q1) + d1*cos(A1)*sin(A2)*cos(q1) - a2*cos(A1)*cos(A2)*cos(q1)*sin(q2), a3*sin(q3)*(sin(A1)*sin(q1)*sin(q2) - cos(A2)*sin(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(sin(A1)*cos(q2)*sin(q1) + cos(A2)*sin(A1)*cos(q1)*sin(q2)) - a1*sin(A1)*sin(q1) + d1*sin(A1)*sin(A2)*cos(q1) - a2*sin(A1)*cos(q2)*sin(q1) - a2*cos(A2)*sin(A1)*cos(q1)*sin(q2);...
31-
a2*sin(q1)*sin(q2) + a3*cos(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)) - a2*cos(A2)*cos(q1)*cos(q2), a3*sin(q3)*(sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*sin(q2)) - a2*cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - a2*cos(A1)*cos(q1)*sin(q2), a2*cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + a3*cos(q3)*(cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*sin(q2)) - a3*sin(q3)*(sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*cos(q2)) - a2*sin(A1)*cos(q1)*sin(q2);...
32-
a3*cos(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)), a3*sin(q3)*(sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*sin(q2)), a3*cos(q3)*(cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*sin(q2)) - a3*sin(q3)*(sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*cos(q2))];
33-
34-
FTK = J*T
30+
%Tranpose of Jacobian Solved in Leg Kinematics Setup
31+
Jt = [ a3*sin(q3)*(cos(q1)*sin(q2) + cos(A2)*cos(q2)*sin(q1)) - a2*cos(q1)*cos(q2) - a3*cos(q3)*(cos(q1)*cos(q2) - cos(A2)*sin(q1)*sin(q2)) - a1*cos(q1) + a2*cos(A2)*sin(q1)*sin(q2), a3*sin(q3)*(cos(A1)*sin(q1)*sin(q2) - cos(A1)*cos(A2)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(A1)*cos(q2)*sin(q1) + cos(A1)*cos(A2)*cos(q1)*sin(q2)) - a1*cos(A1)*sin(q1) - a2*cos(A1)*cos(q2)*sin(q1) - a2*cos(A1)*cos(A2)*cos(q1)*sin(q2), a3*sin(q3)*(sin(A1)*sin(q1)*sin(q2) - cos(A2)*sin(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(sin(A1)*cos(q2)*sin(q1) + cos(A2)*sin(A1)*cos(q1)*sin(q2)) - a1*sin(A1)*sin(q1) - a2*sin(A1)*cos(q2)*sin(q1) - a2*cos(A2)*sin(A1)*cos(q1)*sin(q2);...
32+
a2*sin(q1)*sin(q2) + a3*cos(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)) - a2*cos(A2)*cos(q1)*cos(q2), a3*sin(q3)*(sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*sin(q2)) - a2*cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - a2*cos(A1)*cos(q1)*sin(q2), a2*cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + a3*cos(q3)*(cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*sin(q2)) - a3*sin(q3)*(sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*cos(q2)) - a2*sin(A1)*cos(q1)*sin(q2);...
33+
a3*cos(q3)*(sin(q1)*sin(q2) - cos(A2)*cos(q1)*cos(q2)) + a3*sin(q3)*(cos(q2)*sin(q1) + cos(A2)*cos(q1)*sin(q2)), a3*sin(q3)*(sin(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) - cos(A1)*cos(q1)*cos(q2)) - a3*cos(q3)*(cos(q2)*(sin(A1)*sin(A2) + cos(A1)*cos(A2)*sin(q1)) + cos(A1)*cos(q1)*sin(q2)), a3*cos(q3)*(cos(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) - sin(A1)*cos(q1)*sin(q2)) - a3*sin(q3)*(sin(q2)*(cos(A1)*sin(A2) - cos(A2)*sin(A1)*sin(q1)) + sin(A1)*cos(q1)*cos(q2))];
34+
35+
FTK = Jt*T
3536

3637

3738
end

‎Kinematics/LegKinematicsSetup.m

+2-2
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
% | Link | a | alpha | d | theta |
99
% | Base | 0 | -90 | 0 | 0 |
1010
% | 1 | .161 | 90 | 0 | q1-pi/2 |
11-
% | 2 | 1.5 | 0 | -.161 | q2 |
11+
% | 2 | 1.5 | 0 | 0 | q2 |
1212
% | 3 | 1.5 | 0 | 0 | q3 |
1313
% _________________________________________
1414

@@ -17,7 +17,7 @@
1717
syms a1 a2 a3 A1 A2 d1 q1 q2 q3
1818
TB0 = simplify(MatrixTransform(0,A1,0,0));
1919
T00 = simplify(MatrixTransform(a1,A2,0,q1+deg2rad(90)));
20-
T01 = simplify(MatrixTransform(a2,0,d1,q2));
20+
T01 = simplify(MatrixTransform(a2,0,0,q2));
2121
T12 = simplify(MatrixTransform(a3,0,0,q3));
2222

2323
%Forward Position Kinematics

‎Kinematics/TorqueGraphs.m

+9-9
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
% | Link | a | alpha | d | theta |
77
% | Base | 0 | -90 | 0 | 0 |
88
% | 1 | .161 | 90 | 0 | q1-pi/2 |
9-
% | 2 | 1.5 | 0 | -.161 | q2 |
9+
% | 2 | 1.5 | 0 | 0 | q2 |
1010
% | 3 | 1.5 | 0 | 0 | q3 |
1111
% _________________________________________
1212

@@ -15,7 +15,7 @@
1515
syms a1 a2 a3 A1 A2 d1 q1 q2 q3
1616
TB0 = simplify(MatrixTransform(0,A1,0,0));
1717
T00 = simplify(MatrixTransform(a1,A2,0,q1+deg2rad(90)));
18-
T01 = simplify(MatrixTransform(a2,0,d1,q2));
18+
T01 = simplify(MatrixTransform(a2,0,0,q2));
1919
T12 = simplify(MatrixTransform(a3,0,0,q3));
2020

2121
%Forward Position Kinematics
@@ -29,18 +29,18 @@
2929
Jt = transpose(J);
3030
%Transpose(J)*F = torques on joints
3131

32-
var = [a1, a2, a3, A1, A2, d1, q1];
33-
val = [.161, 1.5, 1.5, -deg2rad(90), deg2rad(90), -.161, deg2rad(0)];
32+
var = [a1, a2, a3, A1, A2, q1];
33+
val = [.161, 1.5, 1.5, -deg2rad(90), deg2rad(90), deg2rad(0)];
3434

3535
Jt = subs(Jt,var, val);
3636
T = [0;0;-.5];
3737
FTK = Jt*T
3838

39-
% q2 = deg2rad(90);
40-
% q3 = deg2rad(10);
41-
% F = [-161/1000;...
42-
% - (3*sin(q2))/2 - (3*cos(q2)*sin(q3))/2 - (3*cos(q3)*sin(q2))/2;...
43-
% - (3*cos(q2)*sin(q3))/2 - (3*cos(q3)*sin(q2))/2]
39+
% FTK =
40+
%
41+
% 0
42+
% - (3*sin(q2))/4 - (3*cos(q2)*sin(q3))/4 - (3*cos(q3)*sin(q2))/4
43+
% - (3*cos(q2)*sin(q3))/4 - (3*cos(q3)*sin(q2))/4
4444

4545
U = -45: 1: 45;
4646
V = 0: 1: 90;

‎Kinematics/TorqueGraphs.m~

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
% Torque Graphs
2+
3+
%DH Table for leg
4+
5+
% _________________________________________
6+
% | Link | a | alpha | d | theta |
7+
% | Base | 0 | -90 | 0 | 0 |
8+
% | 1 | .161 | 90 | 0 | q1-pi/2 |
9+
% | 2 | 1.5 | 0 | 0 | q2 |
10+
% | 3 | 1.5 | 0 | 0 | q3 |
11+
% _________________________________________
12+
13+
%Create DH Transformations
14+
15+
syms a1 a2 a3 A1 A2 d1 q1 q2 q3
16+
TB0 = simplify(MatrixTransform(0,A1,0,0));
17+
T00 = simplify(MatrixTransform(a1,A2,0,q1+deg2rad(90)));
18+
T01 = simplify(MatrixTransform(a2,0,0,q2));
19+
T12 = simplify(MatrixTransform(a3,0,0,q3));
20+
21+
%Forward Position Kinematics
22+
T03 = simplify(TB0*T00*T01*T12);
23+
x = simplify(T03(1,4))
24+
y = simplify(T03(2,4))
25+
z = simplify(T03(3,4))
26+
27+
28+
J = jacobian([x, y, z], [q1, q2, q3]);
29+
Jt = transpose(J);
30+
%Transpose(J)*F = torques on joints
31+
32+
var = [a1, a2, a3, A1, A2, q1];
33+
val = [.161, 1.5, 1.5, -deg2rad(90), deg2rad(90), deg2rad(0)];
34+
35+
Jt = subs(Jt,var, val);
36+
T = [0;0;-.5];
37+
FTK = Jt*T
38+
39+
% FTK =
40+
%
41+
% 0
42+
% - (3*sin(q2))/4 - (3*cos(q2)*sin(q3))/4 - (3*cos(q3)*sin(q2))/4
43+
% - (3*cos(q2)*sin(q3))/4 - (3*cos(q3)*sin(q2))/4
44+
45+
% U = -45: 1: 45;
46+
% V = 0: 1: 90;
47+
% [q2,q3] = meshgrid(U,V);
48+
% F2 = - (3*sin(deg2rad(q2)))/4 - (3*cos(deg2rad(q2)).*sin(deg2rad(q3)))/4 - (3*cos(deg2rad(q3)).*sin(deg2rad(q2)))/4;
49+
% F3 = - (3*cos(deg2rad(q2)).*sin(deg2rad(q3)))/4 - (3*cos(deg2rad(q3)).*sin(deg2rad(q2)))/4;
50+
% surf(q2,q3,F2);
51+

0 commit comments

Comments
 (0)