-
Notifications
You must be signed in to change notification settings - Fork 1
/
generate_trajectory.m
119 lines (67 loc) · 2.33 KB
/
generate_trajectory.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
%2 link model with horizontal joints
mdl_twolink_mdh
%plot with both links at 0
%twolink.plot([0,0])
%transmission of joint 60:1
JOINT_1_TRANSMISSION = 60
%transmission of joint 51:1
JOINT_2_TRANSMISSION = 51
%transmission of joint 50:1
JOINT_3_TRANSMISSION = 50
%set link 2 position
%distance joint 1 to joint 2
twolink.links(2).a = 0.355
%distance joint 2 to tool
twolink.tool(1,4) = 0.22
%distance floor to link 1 height (currently not really needed)
twolink.base(3,4) = 0.54
valid_zero_offset = pi-pi/4
%set joint limits
twolink.qlim = [-valid_zero_offset, valid_zero_offset; -valid_zero_offset, valid_zero_offset]
%generate trajectory
%create circle
t = linspace(0,2*pi,50);
r = 0.1
x = r*cos(t)
y = r*sin(t)
x = x.'
y = y.'
%path = [x,y,zeros(length(x),1)]
%not wokring anymore no ik found: path = [ 1 0 0; 1 0 0; 0 0 0; 0 2 0; 1 2 0; 1 2 0; 0 1 0; 0 1 0; 1 1 0; 1 1 0];
%square
%path = [0.45 0 0; 0.45 0.1 0; 0.35 0.01 0; 0.35 0 0; 0.45 0 0];
%line
path = [0.57 0.1 0; 0.30 0.1 0; 0.57 0.1 0; 0.57 0.1 0; 0.30 0.1 0; 0.57 0.1 0];
%plot3(path(:,1), path(:,2), path(:,3), 'color', 'k', 'LineWidth', 2)
% x y z speedlimit, empty, xyz initial position, timestep, time acceleration is
% applied (strange parameter as smaller values give higher acc/speed?)
%p =
%mstraj(path, [2000 2000 2000], [], [0 0 0], 0.1, 0.9)
timestep = 0.1
%plots the trajectory
mstraj(path, [0.1 0.1 0.1], [], [path(1,1) path(1,2) 0], timestep, 0.1)
p = mstraj(path, [0.1 0.1 0.1], [], [path(1,1) path(1,2) 0], timestep, 0.1)
%scale the trajectory
Tp = transl(p);
%transform the trajectory
Tp = homtrans( transl(0, 0, 0), Tp)
disp(['Calculating IK...'])
%calc ikine for the trajectory, start point, mask which dofs to use/ignore
%q_out = twolink.ikine(Tp,[0 0], [1 1 0 0 0 0], 'plot')
[q_out,err,exitflag] = twolink.ikcon(Tp)
%simulate the trajectory
%twolink.plot(q_out)
%get rid of first value
q_out = q_out(2:end,:)
%conversion from radian angles to encoder steps
q_out(:,1) = radians_to_encoder_steps(q_out(:,1),JOINT_1_TRANSMISSION)
q_out(:,2) = radians_to_encoder_steps(q_out(:,2),JOINT_2_TRANSMISSION)
total_duration_in_s = 10.0
t = 0:total_duration_in_s/length(q_out):total_duration_in_s
t = t(2:end)'
wave1.signals.values = [q_out(:,1)]
wave1.time = t
wave2.signals.values = [q_out(:,2)]
wave2.time = []
disp(['Done'])
twolink.fkine([0 0])