-
Notifications
You must be signed in to change notification settings - Fork 0
/
Simulink_model.m
41 lines (37 loc) · 1.04 KB
/
Simulink_model.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
clc
clear all
load exampleMaps.mat
startLoc = [5 5];
goalLoc = [20 20];
open_system('pathPlanningSimulinkModel.slx')
map = binaryOccupancyMap(simpleMap);
simulation = sim('pathPlanningSimulinkModel.slx');
robotPose = simulation.Pose;
thetaIdx = 3;
% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;
% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;
% Plot the robot poses at every 10th step.
for k = 1:10:size(xyz, 1)
show(map)
hold on;
% Plot the start location.
plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
text(startLoc(1), startLoc(2), 2, 'Start');
% Plot the goal location.
plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
text(goalLoc(1), goalLoc(2), 2, 'Goal');
% Plot the xy-locations.
plot(robotPose(:, 1), robotPose(:, 2), '-b')
% Plot the robot pose as it traverses the path.
quat = eul2quat(thetaEuler(k, :), 'xyz');
plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
'groundvehicle.stl');
light;
drawnow;
hold off;
end