coopPayloadSim.m 1.4 KB
Newer Older
mmroma's avatar
mmroma committed
1
%% Cooperative Payload Simulator
mmroma's avatar
mmroma committed
2
3
4
5
6
7
% Author: Matthew Romano (mmroma@umich.edu)
% Date: 1/21/2019

% This is the high level script that calls all of the necessary functions
% to run the simulation. When tuning and testing, changes should be made to
% the parameters files (called by setUpParameters() ).
mmroma's avatar
mmroma committed
8
% Run simulink model
mmroma's avatar
mmroma committed
9

mmroma's avatar
mmroma committed
10
%% Add Necessary Folders to MATLAB Path (add everything)
11
addpath(genpath(pwd));
mmroma's avatar
mmroma committed
12

mmroma's avatar
mmroma committed
13
%% Set up Parameters
14
traj_num = 1;
mmroma's avatar
mmroma committed
15
16
17
18
19
20
21
22
params = setUpParameters(traj_num);

%% Load Top Level Simulink Model (helps Simulink load ahead of time)
open('coop_payload_sim');

%% Call Simulink and Run the Simulation
simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal));

23
24
25
%% Haptic Force Estimation (post-process)
[params, simData] = hapticForceEstimation(params,simData);

mmroma's avatar
mmroma committed
26
%% Plot
27
28
29
30
31
32
plot_vec3(simData.tout, simData.Xd_load,'Xd Load');
plot_vec3(simData.tout, simData.Xd_load_control,'Xd Load Control');
plot_vec3(simData.tout, simData.payload_X,'X Load');


% plot_position(simData.tout, simData.payload_X, simData.Xd_load_control);
mmroma's avatar
mmroma committed
33
34
35
% plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_force(simData.tout, simData.q1_F_gravity, 'Q1 Gravity');
36
% plot_force(simData.tout, simData.q1_F_tether, 'Q1 Tether_G');
37
% plot_force(simData.tout, simData.q1_F_drag, 'Q1 Drag_V');
mmroma's avatar
mmroma committed
38
39
40

%% Animate
animate_simulink_data(simData, params);
41
42


mmroma's avatar
mmroma committed
43

mmroma's avatar
mmroma committed
44
45
46



47
48


49
50