Contents
clear all; clc;
syms t1 t2 t3 L1 L2 L3 d d1 d2 d3
syms t4 t5 t6 d4 L4
problemnumber=0;
for i = 1:8
problemnumber=i;
p = problemnumber;
if p == 1
Problem #1
DH = [0 0 0 0 0 0;...
0 L1 0 0 0 0;...
0 L2 0 0 0 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 t2 0])
T1_2 = Robot1.A([2],[t1 t2 0])
T2_t = Robot1.A([3],[t1 t2 0])
Robot1.A([1 2 3],[t1 t2 0])
Robot1 =
noname (3 axis, RRR, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| 0| L1| 0| 0|
| 3| q3| 0| L2| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_2 =
[ cos(t2), -sin(t2), 0, L1]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T2_t =
[ 1, 0, 0, L2]
[ 0, 1, 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
ans =
[ cos(t1)*cos(t2) - sin(t1)*sin(t2), - cos(t1)*sin(t2) - cos(t2)*sin(t1), 0, L2*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) + L1*cos(t1)]
[ cos(t1)*sin(t2) + cos(t2)*sin(t1), cos(t1)*cos(t2) - sin(t1)*sin(t2), 0, L2*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) + L1*sin(t1)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
end
if p == 2
Problem #2
DH = [0 0 0 0 0 0;...
0 L1 0 0 0 0;...
0 L2 0 0 0 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 t2 t3])
T1_2 = Robot1.A([2],[t1 t2 t3])
T2_3 = Robot1.A([3],[t1 t2 t3])
Robot1.A([1 2 3],[t1 t2 t3])
Robot1 =
noname (3 axis, RRR, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| 0| L1| 0| 0|
| 3| q3| 0| L2| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_2 =
[ cos(t2), -sin(t2), 0, L1]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T2_3 =
[ cos(t3), -sin(t3), 0, L2]
[ sin(t3), cos(t3), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
ans =
[ cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) - sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)), - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) - sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)), 0, L2*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) + L1*cos(t1)]
[ cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)), cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) - sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)), 0, L2*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) + L1*sin(t1)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
end
if p == 3
Problem #3
DH = [0 0 0 0 0 0;...
-90 0 0 0 0 0;...
0 L2 0 0 0 0;...
0 L3 0 0 0 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 t2 t3 0])
T1_2 = Robot1.A([2],[t1 t2 t3 0])
T2_3 = Robot1.A([3],[t1 t2 t3 0])
T2_4 = Robot1.A([4],[t1 t2 t3 0])
Robot1.A([1 2 3 4],[t1 t2 t3 0])
Robot1 =
noname (4 axis, RRRR, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| 0| 0| -pi/2| 0|
| 3| q3| 0| L2| 0| 0|
| 4| q4| 0| L3| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_2 =
[ cos(t2), -sin(t2), 0, 0]
[ 0, 0, 1, 0]
[ -sin(t2), -cos(t2), 0, 0]
[ 0, 0, 0, 1]
T2_3 =
[ cos(t3), -sin(t3), 0, L2]
[ sin(t3), cos(t3), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T2_4 =
[ 1, 0, 0, L3]
[ 0, 1, 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
ans =
[ cos(t1)*cos(t2)*cos(t3) - cos(t1)*sin(t2)*sin(t3), - cos(t1)*cos(t2)*sin(t3) - cos(t1)*cos(t3)*sin(t2), -sin(t1), L2*cos(t1)*cos(t2) - L3*(cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3))]
[ cos(t2)*cos(t3)*sin(t1) - sin(t1)*sin(t2)*sin(t3), - cos(t2)*sin(t1)*sin(t3) - cos(t3)*sin(t1)*sin(t2), cos(t1), L2*cos(t2)*sin(t1) - L3*(sin(t1)*sin(t2)*sin(t3) - cos(t2)*cos(t3)*sin(t1))]
[ - cos(t2)*sin(t3) - cos(t3)*sin(t2), sin(t2)*sin(t3) - cos(t2)*cos(t3), 0, - L3*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) - L2*sin(t2)]
[ 0, 0, 0, 1]
end
if p == 4
Problem #4
DH = [0 0 0 0 0 0;...
90 0 L2 0 0 0;...
-90 0 d3 0 1 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 t2 d])
T1_3 = Robot1.A([2],[t1 t2 d])
T3_4 = Robot1.A([3],[t1 t2 d])
Robot1.A([1 2 3],[t1 t2 d])
Robot1 =
noname (3 axis, RRP, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| L2| 0| pi/2| 0|
| 3| 0| q3| 0| -pi/2| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_3 =
[ cos(t2), -sin(t2), 0, 0]
[ 0, 0, -1, -L2]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 0, 1]
T3_4 =
[ 1, 0, 0, 0]
[ 0, 0, 1, d]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
ans =
[ cos(t1)*cos(t2), -sin(t1), -cos(t1)*sin(t2), L2*sin(t1) - d*cos(t1)*sin(t2)]
[ cos(t2)*sin(t1), cos(t1), -sin(t1)*sin(t2), - L2*cos(t1) - d*sin(t1)*sin(t2)]
[ sin(t2), 0, cos(t2), d*cos(t2)]
[ 0, 0, 0, 1]
end
if p == 5
Problem #5
DH = [0 0 0 0 0 0;...
0 0 d2 90 1 0;...
90 0 d3 0 1 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 d2 d3])
T1_2 = Robot1.A([2],[t1 d2 d3])
T2_3 = Robot1.A([3],[t1 d2 d3])
Robot1.A([1 2 3],[t1 d2 d3])
Robot1 =
noname (3 axis, RPP, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| pi/2| q2| 0| 0| 0|
| 3| 0| q3| 0| pi/2| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_2 =
[ 0, -1, 0, 0]
[ 1, 0, 0, 0]
[ 0, 0, 1, d2]
[ 0, 0, 0, 1]
T2_3 =
[ 1, 0, 0, 0]
[ 0, 0, -1, -d3]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]
ans =
[ -sin(t1), 0, cos(t1), d3*cos(t1)]
[ cos(t1), 0, sin(t1), d3*sin(t1)]
[ 0, 1, 0, d2]
[ 0, 0, 0, 1]
end
if p == 6
Problem #6
DH = [0 0 0 t4 0 0;...
90 0 0 t5 0 0;...
-90 0 0 t6 0 0];
Robot1 = DH2Robot(DH,0)
T3_4 = Robot1.A([1],[t4 t5 t6])
T4_5 = Robot1.A([2],[t4 t5 t6])
T5_6 = Robot1.A([3],[t4 t5 t6])
Robot1.A([1 2 3],[t4 t5 t6])
Robot1 =
noname (3 axis, RRR, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| 0| 0| pi/2| 0|
| 3| q3| 0| 0| -pi/2| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T3_4 =
[ cos(t4), -sin(t4), 0, 0]
[ sin(t4), cos(t4), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T4_5 =
[ cos(t5), -sin(t5), 0, 0]
[ 0, 0, -1, 0]
[ sin(t5), cos(t5), 0, 0]
[ 0, 0, 0, 1]
T5_6 =
[ cos(t6), -sin(t6), 0, 0]
[ 0, 0, 1, 0]
[ -sin(t6), -cos(t6), 0, 0]
[ 0, 0, 0, 1]
ans =
[ cos(t4)*cos(t5)*cos(t6) - sin(t4)*sin(t6), - cos(t6)*sin(t4) - cos(t4)*cos(t5)*sin(t6), -cos(t4)*sin(t5), 0]
[ cos(t4)*sin(t6) + cos(t5)*cos(t6)*sin(t4), cos(t4)*cos(t6) - cos(t5)*sin(t4)*sin(t6), -sin(t4)*sin(t5), 0]
[ cos(t6)*sin(t5), -sin(t5)*sin(t6), cos(t5), 0]
[ 0, 0, 0, 1]
end
if p == 7
Problem #7
DH = [0 0 0 t1 0 0;...
0 L1 0 t2 0 0;...
0 L2 0 t3 0 0;...
0 0 -d4 0 1 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[t1 t2 t3 d4])
T1_2 = Robot1.A([2],[t1 t2 t3 d4])
T2_3 = Robot1.A([3],[t1 t2 t3 d4])
T3_4 = Robot1.A([4],[t1 t2 t3 d4])
Robot1.A([1 2 3 4],[t1 t2 t3 d4])
Robot1 =
noname (4 axis, RRRP, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0| 0| 0| 0|
| 2| q2| 0| L1| 0| 0|
| 3| q3| 0| L2| 0| 0|
| 4| 0| q4| 0| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T1_2 =
[ cos(t2), -sin(t2), 0, L1]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T2_3 =
[ cos(t3), -sin(t3), 0, L2]
[ sin(t3), cos(t3), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T3_4 =
[ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, d4]
[ 0, 0, 0, 1]
ans =
[ cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) - sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)), - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) - sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)), 0, L2*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) + L1*cos(t1)]
[ cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)), cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)) - sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)), 0, L2*(cos(t1)*sin(t2) + cos(t2)*sin(t1)) + L1*sin(t1)]
[ 0, 0, 1, d4]
[ 0, 0, 0, 1]
end
if p == 8
Problem #8
DH = [0 0 d1 0 1 0;...
0 0 L1 t2 0 0;...
0 L2 0 t3 0 0;...
0 L3 0 t4 0 0;...
0 0 -L4 0 0 0];
Robot1 = DH2Robot(DH,0)
T0_1 = Robot1.A([1],[d1 t2 t3 t4 0])
T1_2 = Robot1.A([2],[d1 t2 t3 t4 0])
T2_3 = Robot1.A([3],[d1 t2 t3 t4 0])
T3_4 = Robot1.A([4],[d1 t2 t3 t4 0])
T4_5 = Robot1.A([5],[d1 t2 t3 t4 0])
Robot1.A([1 2 3 4 5],[d1 t2 t3 t4 0])
Robot1 =
noname (5 axis, PRRRR, modDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| 0| q1| 0| 0| 0|
| 2| q2| L1| 0| 0| 0|
| 3| q3| 0| L2| 0| 0|
| 4| q4| 0| L3| 0| 0|
| 5| q5| -L4| 0| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
grav = 0 base = 1 0 0 0 tool = 1 0 0 0
0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T0_1 =
[ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, d1]
[ 0, 0, 0, 1]
T1_2 =
[ cos(t2), -sin(t2), 0, 0]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 1, L1]
[ 0, 0, 0, 1]
T2_3 =
[ cos(t3), -sin(t3), 0, L2]
[ sin(t3), cos(t3), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T3_4 =
[ cos(t4), -sin(t4), 0, L3]
[ sin(t4), cos(t4), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T4_5 =
[ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, -L4]
[ 0, 0, 0, 1]
ans =
[ cos(t4)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - sin(t4)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)), - cos(t4)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) - sin(t4)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)), 0, L3*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) + L2*cos(t2)]
[ cos(t4)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + sin(t4)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)), cos(t4)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - sin(t4)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)), 0, L3*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + L2*sin(t2)]
[ 0, 0, 1, L1 - L4 + d1]
[ 0, 0, 0, 1]
end
end