This repository has been archived by the owner on Oct 22, 2021. It is now read-only.
forked from andybarry/simflight
-
Notifications
You must be signed in to change notification settings - Fork 0
/
DeltawingPlant.m
104 lines (67 loc) · 2.65 KB
/
DeltawingPlant.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
classdef DeltawingPlant < DrakeSystem
% Defines the dynamics for the TBS Capi delta wing.
properties
parameters = {}; % extra arguments to pass to tbsc_model
end
methods
function obj = DeltawingPlant(parameters)
% @param parmaeters cell array of extra arguments to pass to
% tbsc_model
obj = obj@DrakeSystem(12,0,3,12,false,true);
obj = setDirectFeedthrough(obj,0);
obj = setOutputFrame(obj,getStateFrame(obj));
obj = obj.setInputLimits([-0.90; -0.90; 0], [0.855; 0.855; 5.33976]); % input limits in [radians radians newtons]
obj.parameters = parameters;
end
function [xdot, dxdot] = dynamics(obj, t, x, u)
options = struct();
options.grad_method = 'numerical';
tempfunc = @(t, x, u) obj.dynamics_no_grad(t, x, u);
[xdot, dxdot] = geval(tempfunc, t, x, u, options);
end
function xdot = dynamics_no_grad(obj,t,x,u)
x = ConvertToModelFrameFromDrakeWorldFrame(x);
xdot_model_frame = tbsc_model(t, x, u, obj.parameters{:});
xdot = ConvertXdotModelToDrake(x, xdot_model_frame);
end
function [y,dy] = output(obj,t,x,u)
y = x;
if (nargout>1)
dy=[zeros(obj.num_y,1),eye(obj.num_y),zeros(obj.num_y,obj.num_u)];
end
end
function x = getInitialState(obj)
x = zeros(12,1);
end
end
methods (Static)
function playback(xtraj, utraj, options)
if nargin < 3
options = struct();
end
v = DeltawingPlant.constructVisualizer;
traj_and_u = [xtraj; utraj];
fr = traj_and_u.getOutputFrame();
transform_func = @(t, x, x_and_u) [ x_and_u(1:6); x_and_u(15); x_and_u(13:14); x_and_u(7:12); zeros(3,1)];
trans = FunctionHandleCoordinateTransform(17, 0, traj_and_u.getOutputFrame(), v.getInputFrame(), true, true, transform_func, transform_func, transform_func);
fr.addTransform(trans);
warning('off', 'Drake:FunctionHandleTrajectory');
playback(v, traj_and_u, options);
warning('on', 'Drake:FunctionHandleTrajectory');
end
function playback_xtraj(xtraj, options)
if nargin < 2
options = struct();
end
utraj = ConstantTrajectory([0; 0; 0]);
DrawTrajectoryLcmGl(xtraj);
DeltawingPlant.playback(xtraj, utraj, options);
end
function v = constructVisualizer()
options.floating = true;
HudBotVisualizer.InitJava();
r = RigidBodyManipulator('TBSC_visualizer.urdf', options);
v = HudBotVisualizer(r);
end
end
end