-
Notifications
You must be signed in to change notification settings - Fork 12
/
exp2_EBR.m
205 lines (183 loc) · 9.34 KB
/
exp2_EBR.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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
%% initialize
close all
clear all
format shorte
clc
pause( 1 )
addpath('./hll');
%% parameters
n_l = 3 ; % number of segments
l_s = 44e-3 ; %44e-3 ; % manipulator length
r_s1 = 4.5e-3 ; % manipulator inner radius
r_s2 = 12.5e-3 ; % manipulator outer radius
r_p1 = 2.5e-3 ; % 2.25e-3 pressure chamber inner radius
r_p2 = 3e-3 ; % pressure chamber outer radius
r_t = 0.2e-3 ; % braid diameter
gamma = pi / 2 - acos( 1 - r_t ^ 2 / 2 / r_p2 ^ 2 ) ;
r_o = 8.5e-3 ; %8.4e-3
phi_o = acos( 1 - r_p2 ^ 2 / 2 / r_o ^ 2 ) ;
r_od = [ cos( 3*pi/6 + phi_o ) sin( 3*pi/6 + phi_o ) 0 ; % according to Ali's inputs
cos( 3*pi/6 - phi_o ) sin( 3*pi/6 - phi_o ) 0 ;
cos( -pi/6 + phi_o ) sin( -pi/6 + phi_o ) 0 ;
cos( -pi/6 - phi_o ) sin( -pi/6 - phi_o ) 0 ;
cos( 7*pi/6 + phi_o ) sin( 7*pi/6 + phi_o ) 0 ;
cos( 7*pi/6 - phi_o ) sin( 7*pi/6 - phi_o ) 0 ] ;
r_od = [ r_od(:,2) -r_od(:,1) r_od(:,3) ] ;
m_f = 13.47e-3 ; % 11e-3 ; % force sensor and its mount mass
l_f = 14.5e-3; % 36.5e-3 ; % 36.5-23=14.5e-3 ; %17e-3 ;
r_f = 7.5e-3 ;
g_x = 0 ; g_y = 0 ; g_z = 9.81 ; % gravity
sigma_s = 1.4 * 1300 ; % 1.4 * ; % silicon density
E_s = 120e3 ; % 130e3, 205e3
mu_pow = 1 ; % < 1: rate thining, > 1: rate thikening
mu_x = 1e-1 ; % 1e-1
mu_y = 1e-1 ;
mu_z = 1e-1 ;
mu_xx = 1e-3 ; % 1e-4
mu_yy = 1e-3 ;
mu_zz = 1e-3 ;
%% variables
% symbolic parameters to be changed after EOM derivation
syms sigma_s_ E_s_ r_s1_ r_s2_ r_p1_ l_s_ ...
mu_x_ mu_y_ mu_z_ mu_xx_ mu_yy_ mu_zz_ mu_pow_ ...
g_x_ g_y_ g_z_ lambda_ m_f_ l_f_ r_f_ r_o_ gamma_ ...
p1_ p2_ p3_ p4_ p5_ p6_ ...
f_tx_ f_ty_ f_tz_ tau_tx_ tau_ty_ tau_tz_ real
vars = [ sigma_s_ E_s_ r_s1_ r_s2_ l_s_ mu_x_ mu_y_ mu_z_ ...
mu_xx_ mu_yy_ mu_zz_ mu_pow_ g_x_ g_y_ g_z_ m_f_ l_f_ r_f_ ...
r_p1_ r_o_ gamma_ p1_ p2_ p3_ p4_ p5_ p6_ f_tx_ f_ty_ ...
f_tz_ tau_tx_ tau_ty_ tau_tz_ lambda_ ] ;
% parameter values, can be changed after derivations
var_vals = [ sigma_s, E_s, r_s1, r_s2, l_s, mu_x, mu_y, mu_z, mu_xx, mu_yy, ...
mu_zz, mu_pow, g_x, g_y, g_z, m_f, l_f, r_f, r_p1, r_o, gamma, zeros(1,12) , 1 ] ;
% simplification
mu_pow_ = mu_pow ;
%% exp. readings
exp_case = 1 ; % experiment scenario
[ p , tip_pos , tip_RQ , f_ex , t_exp , t_exp_equil ] = sample_exp_data( var_vals , exp_case ); % motor input and robot tip position from a sample experiment
% user_pars contains other data sets such as inputs, experimental data, etc.
user_pars.p = p ;
user_pars.tip_pos = tip_pos ;
user_pars.tip_RQ = tip_RQ ;
user_pars.f_ex = f_ex ;
user_pars.n_l = n_l ;
%% preprocess
l_ss = l_s_ / n_l ; % segment length
a_aw = pi * ( r_s2_.^ 2 - r_s1_.^ 2 - 6 * r_p1_.^ 2 ) ; % no change
m_s = a_aw * l_s_ * sigma_s_ ; % manipulator mass
r_ov = r_o_ * r_od / sqrt( lambda_ ) ;
r_p1_ = r_p1_ * sqrt( 1 - lambda_.^ 2 * cos( gamma_ ) ^ 2 ) / sin( gamma_ ) ;
a_p = pi * r_p1_.^ 2 ;
a_aw = pi * ( ( r_s2_.^ 2 - r_s1_.^ 2 ) / lambda_ - 6 * r_p1_.^ 2 ) ;
m_ss = m_s / n_l ; % manipulator segment mass
G_s = E_s_ / 3 ;
I_aw = [ 1 1 2 ] * 1 / 4 * pi * ( ( r_s2_.^ 4 - r_s1_.^ 4 ) / lambda_.^ 2 - 6 * r_p1_.^ 4 ) ...
- a_p * [ diag( r_ov(:,1:2).' * r_ov(:,1:2) ).' r_o_.^2*lambda_ ] ;
K_v = diag( [ G_s G_s E_s_ ] ) * a_aw / l_ss / lambda_ ; % ( l_ss / lambda_ ) to get deformation not strain
K_u = diag( [ E_s_ E_s_ G_s ] ) * diag( I_aw ) / l_ss / lambda_ ; % ( l_ss / lambda_ ) to get angles not curvatures/torsion
I_ss = diag( ... % COM (joint location) is mid-link
sigma_s_ * pi * ( r_s2_.^2 - r_s1_.^2- 6 * r_p1_.^2 ) * l_ss * ( ... % minimal implementation of the holes
[ 1/12*(3*(r_s2_.^2+r_s1_.^2)/lambda_+(l_ss*lambda_).^2) , ...
1/12*(3*(r_s2_.^2+r_s1_.^2)/lambda_+(l_ss*lambda_).^2) , ...
1/2*(r_s2_.^2+r_s1_.^2)/lambda_ ] ) ) ; % manipulator segment inertia
I_f = diag( [ 1/12*m_f_*(3*r_f_^2+l_f_.^2) 1/12*m_f_*(3*r_f_.^2+l_f_.^2) 1/2*m_f_*r_f_.^2 ]...
+ m_f_ * (l_f_/2).^2 * [ 1 1 0 ] ) ; % tip force sensor segment inertia
p = [ p1_ p2_ p3_ p4_ p5_ p6_ ] ;
f_p = [ 0 0 p*ones(6,1)*a_p ] ;
tau_p = p * a_p * r_ov ;
K_uv = [ diag( K_v ).' diag( K_u ).' ] ;
mu_uv = [ mu_x_ mu_y_ mu_z_ mu_xx_ mu_yy_ mu_zz_ ] ;
ftau_uv = [ f_p tau_p ] ;
%% robot
% break DSL down to smaller pieces to prevent Matlab from complaining about too complex arguments!
results = ...
tmtdyn()...
.simulation()... % simulation
.variables(vars, var_vals)...
.user_parameters(user_pars)...
.derive_eom('assume_small_velocities')... % 'full_system', 'assume_small_velocities', 'no'
.optimize_code()...
.analysis()...
.static_sim('generate_mex_file', t_exp_equil)... % edited_m_file, generate_m_file, generate_mex_file, old_mex_file, generate_mex_from_edited_m_file, generate_mex_from_c_files
.dynamic_sim('generate_mex_file', 'radau', t_exp(1), t_exp(end))... % matlab, sundials_ode, sundials_dae, radau, radau_mex, external
.results_sample_rate(1e2)...
.report_time_intervals(1)... % show sim progress in terminal
.post_process()...
.animate('tube', 'show_frame', 30, 20, 20)... % ({null 'none', 'show_frame', 'side_views', 'show_frame_side_views'}, fps, axis edges, cross-section edges)
.record_video()...
.number_of_reports(1)...
.run_user_code() ;
results = results...
.world()... % world
.g([g_x_, g_y_, g_z_])...
.robot('series_rigid_link_arm')... % robot
.body('rigid_link')...
.with_mass(m_ss)...
.with_inertia(I_ss)...
.with_center_of_mass_at( [ 0 0 l_ss/2 ] )...
.with_tip_at( [ 0 0 l_ss ] )...
.with_cross_section_radius(1/sqrt(lambda_)*r_s2_)...
.connected_to_repeated_joint(1:n_l)...
.with_transformation_from_body(1,0:n_l-1)... % leave empty for connection to ground
.trans_z()... % axial translation w.r.t. previous joint copy
.base_value( [0, ones(1,n_l-1)].' * l_ss )... % all links have l_ss offset along z except the 1st link
.translation([inf,inf,inf])... % axial translation w.r.t. previous joint copy
.dof(1)...
.direct_input('force', ftau_uv(1))...
.parallel_spring()...
.coefficient(K_uv(1))...
.parallel_damper()...
.viscosity(mu_uv(1))...
.power(mu_pow_)...
.dof(2)...
.direct_input('force', ftau_uv(2))...
.parallel_spring()...
.coefficient(K_uv(2))...
.parallel_damper()...
.viscosity(mu_uv(2))...
.power(mu_pow_)...
.dof(3)...
.direct_input('force', ftau_uv(3))...
.parallel_spring()...
.coefficient(K_uv(3))...
.parallel_damper()...
.viscosity(mu_uv(3))...
.power(mu_pow_) ;
results = results...
.rot_strain([inf, inf, inf], [1 0 0 0 ])...
.dof(1)...
.direct_input('force', ftau_uv(4))...
.parallel_spring()...
.coefficient(K_uv(4))...
.parallel_damper()...
.viscosity(mu_uv(4))...
.dof(2)...
.direct_input('force', ftau_uv(5))...
.parallel_spring()...
.coefficient(K_uv(5))...
.parallel_damper()...
.viscosity(mu_uv(5))...
.power(mu_pow_)...
.dof(3)...
.direct_input('force', ftau_uv(6))...
.parallel_spring()...
.coefficient(K_uv(6))...
.parallel_damper()...
.viscosity(mu_uv(6))...
.power(mu_pow_) ;
results = results...
.body('tip_force_sensor')...
.with_mass(m_f_)...
.with_inertia(I_f)...
.with_center_of_mass_at( [ 0 0 l_f_/2 ] )...
.with_tip_at( [ 0 0 l_f_ ] )...
.with_cross_section_radius(r_f_)...
.connected_to_joint()...
.with_transformation_from_body(1,n_l)... % mass at the last link tip with no extra transformation
.trans_z(l_ss)...
.exload('tip_load')...
.exerted_on_body_with_transformation(2)... % >> transformation
.translation([0 0 l_f_])... % axial translation w.r.t. previous joint copy
.force([f_tx_, f_ty_, f_tz_])...
.moment([tau_tx_, tau_ty_, tau_tz_])...
.run() ;