Adds simulation for stabilization trajectories with the controller in…

… the state-estimator frame (body frame) and all the drake components in the Drake frame (so visualization still works).

AddTiqrControllers.m

@@ -1,4 +1,4 @@
-function lib = AddTiqrControllers(lib, name, p, A, B, x0, u0, gains)
+function lib = AddTiqrControllers(lib, name, A, B, x0, u0, gains)
 
   Q = gains.Q;
   R = gains.R;
@@ -7,6 +7,7 @@
   K_pd_aggressive_yaw = gains.K_pd_aggressive_yaw;
 
   number = 10000;
+  p = lib.p;
 
   % first just use the K_pd's and build trajectories
 
@@ -29,7 +30,7 @@
   
   number = number + 1;
   %}
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K_pd, 'pd-no-yaw');
+  lib = lib.AddStabilizationTrajectory(x0, u0, K_pd, 'pd-no-yaw');
 
   %{
   
@@ -48,7 +49,7 @@
   
   number = number + 1;
   %}
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K_pd_yaw, 'pd-yaw');
+  lib = lib.AddStabilizationTrajectory(x0, u0, K_pd_yaw, 'pd-yaw');
 
 
   %{
@@ -67,7 +68,7 @@
   
   number = number + 1;
   %}
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K_pd_aggressive_yaw, 'pd-aggressive-yaw');
+  lib = lib.AddStabilizationTrajectory(x0, u0, K_pd_aggressive_yaw, 'pd-aggressive-yaw');
 
 
 
@@ -106,7 +107,7 @@
   comments = sprintf('%s\n\n%s', [trajname, prettymat('Parameters', cell2mat(p.parameters), 3) ...
       prettymat('Q', Q, 5) prettymat('R', R)]);
 
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K0, trajname, comments);
+  lib = lib.AddStabilizationTrajectory(x0, u0, K0, trajname, comments);
 
 
 
@@ -135,7 +136,7 @@
   comments = sprintf('%s\n\n%s', [trajname, prettymat('Parameters', cell2mat(p.parameters), 3) ...
       prettymat('Q', Q, 5) prettymat('R', R)]);
 
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K1, trajname, comments);
+  lib = lib.AddStabilizationTrajectory(x0, u0, K1, trajname, comments);
 
 
 
@@ -161,7 +162,7 @@
   comments = sprintf('%s\n\n%s', [trajname, prettymat('Parameters', cell2mat(p.parameters), 3) ...
       prettymat('Q', Q, 5) prettymat('R', R)]);
 
-  lib = lib.AddStabilizationTrajectory(p, x0, u0, K2, trajname, comments);
+  lib = lib.AddStabilizationTrajectory(x0, u0, K2, trajname, comments);
 
 
 end

BuildTrajectoryLibrary.m

@@ -6,7 +6,9 @@
 
 [parameters, gains] = GetDefaultGains();
 
-p = DeltawingPlant(parameters);
+deltawing_plant = DeltawingPlant(parameters);
+p = DeltawingPlantStateEstFrame(deltawing_plant);
+
 
 [x0, u0, lib] = FindTrimDrake(p);
 

DeltawingPlantStateEstFrame.m

@@ -4,6 +4,7 @@
 
   properties
     p = {}; % DeltawingPlant
+    parameters;
   end
 
   methods
@@ -18,6 +19,22 @@
       obj = obj.setInputLimits(p.umin, p.umax); % input limits in [radians radians newtons]
 
       obj.p = p;
+      obj.parameters = obj.p.parameters;
+
+      % tell the DeltawingPlant how to convert from its frame to this frame
+      drake_to_est_func = @(t, x, u) ConvertDrakeFrameToEstimatorFrame(u);
+      est_to_drake_func = @(t, x, u) ConvertStateEstimatorToDrakeFrame(u);
+      trans_drake_to_state_est = FunctionHandleCoordinateTransform(12, 0, obj.p.getStateFrame(), obj.getStateFrame(), false, true, drake_to_est_func, drake_to_est_func, drake_to_est_func);
+      trans_state_est_to_drake = FunctionHandleCoordinateTransform(12, 0, obj.getStateFrame(), obj.p.getStateFrame(), false, true, est_to_drake_func, est_to_drake_func, est_to_drake_func);
+      obj.p.getStateFrame.addTransform(trans_drake_to_state_est);
+      obj.getStateFrame.addTransform(trans_state_est_to_drake);
+
+      % also for the input frames
+      trans_u_drake_to_est = AffineTransform(obj.p.getInputFrame(), obj.getInputFrame(), eye(3), zeros(3,1));
+      trans_u_est_to_drake = AffineTransform(obj.getInputFrame(), obj.p.getInputFrame(), eye(3), zeros(3,1));
+
+      obj.p.getInputFrame.addTransform(trans_u_drake_to_est);
+      obj.getInputFrame.addTransform(trans_u_est_to_drake);
 
     end
 
@@ -53,25 +70,32 @@
       x = zeros(12,1);
     end
 
-  end
-
-  methods (Static)
-
-    function playback(xtraj, utraj, options)
-      error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
+    function playback(obj, xtraj, utraj, options)
+      if nargin < 4
+        options = struct();
+      end
+      obj.p.playback(xtraj, utraj, options);
+      %error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
 
     end
 
-    function playback_xtraj(xtraj, options)
-      error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
+    function playback_xtraj(obj, xtraj, options)
+      if nargin < 3
+        options = struct();
+      end
+
+      obj.p.playback_xtraj(xtraj, options);
+      %error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
 
     end
 
 
-    function v = constructVisualizer()
-        error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
+    function v = constructVisualizer(obj)
+      v = obj.p.constructVisualizer();
+      %error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
     end
 
-  end  
+  end
+
 
 end

FindClimbTrimDrake.m

@@ -75,6 +75,8 @@
     x0(5) = x(1);
     x0(7) = x(2);
     x0(9) = x(3);
+
+    x0 = ConvertDrakeFrameToEstimatorFrame(x0);
 
     u0 = zeros(3,1);
 
@@ -174,5 +176,5 @@
     gains.K_pd_aggressive_yaw = K_pd_aggressive_yaw;
 
 
-    lib = AddTiqrControllers(lib, 'tilqr-climb', p, A, B, x0, u0, gains);
+    lib = AddTiqrControllers(lib, 'tilqr-climb', A, B, x0, u0, gains);
 end

FindTrimDrake.m

@@ -107,10 +107,8 @@
 
     %R = diag([35 35 35]);
     %R_values = [35 50 25];
-
-    p_body_frame = DeltawingPlantStateEstFrame(p);
 
-    [A, B, C, D, xdot0, y0] = p_body_frame.linearize(0, x0, u0);
+    [A, B, C, D, xdot0, y0] = p.linearize(0, x0, u0);
     %% check linearization
 
     %(A*(x0-x0) + B*(u0-u0) + xdot0) - p.dynamics(0, x0, u0)
@@ -160,5 +158,5 @@
     gains.K_pd_aggressive_yaw = K_pd_aggressive_yaw;
 
 
-    lib = AddTiqrControllers(lib, 'tilqr', p, A, B, x0, u0, gains);
+    lib = AddTiqrControllers(lib, 'tilqr', A, B, x0, u0, gains);
 end

TrajectoryLibrary.m

@@ -18,6 +18,8 @@
       %
       % @param plant plant that the library is built for (some kind of DrakeSystem)
 
+      assert(isa(plant, 'DeltawingPlantStateEstFrame'), 'plant is not a DeltawingPlantStateEstFrame.');
+
       obj.p = plant;
     end
 
@@ -78,27 +80,31 @@ function playback(obj, traj_num_from_filename)
       traj.playback(obj.p);
     end
 
-    function [ytraj, xtraj] = SimulateStabilizationTrajectory(obj, traj_num_from_filename, tf)
+    function [ytraj, xtraj] = SimulateStabilizationTrajectory(obj, traj_num_from_filename, tf, x0)
 
       traj = obj.GetTrajectoryByNumber(traj_num_from_filename);
 
-      x0 = traj.xtraj.eval(0);
+      if nargin < 4
+        x0_est = traj.xtraj.eval(0);
+        x0 = ConvertStateEstimatorToDrakeFrame(x0_est);
+      end
 
       lqrsys = traj.lqrsys;
 
+      fb_sys = feedback(obj.p.p, lqrsys);
 
-
-      fb_sys = feedback(obj.p, lqrsys);
-
-
+      disp(['Simulating: ' traj.name '...']);
       [ytraj, xtraj] = fb_sys.simulate([0 tf], x0);
+      disp('done.');
+
+      obj.p.playback_xtraj(xtraj, struct('slider', true));
 
     end
 
-    function obj = AddStabilizationTrajectory(obj, p, x0, u0, K_to_be_negated, trajname, comments)
+    function obj = AddStabilizationTrajectory(obj, x0, u0, K_to_be_negated, trajname, comments)
 
       if (nargin < 7)
-        comments = sprintf('%s\n\n%s', [trajname, prettymat('Parameters', cell2mat(p.parameters), 3)]);
+        comments = sprintf('%s\n\n%s', [trajname, prettymat('Parameters', cell2mat(obj.p.parameters), 3)]);
       end
 
       xtraj = ConstantTrajectory(x0);
@@ -109,19 +115,19 @@ function playback(obj, traj_num_from_filename)
 
       lqrsys = AffineSystem([],[],[],[],[], [], [], ktraj, affine_traj);
 
-      lqrsys = setInputFrame(lqrsys,CoordinateFrame([p.getStateFrame.name,' - ', mat2str(x0,3)],length(x0),p.getStateFrame.prefix));
+      lqrsys = setInputFrame(lqrsys,CoordinateFrame([obj.p.getStateFrame.name,' - ', mat2str(x0,3)],length(x0), obj.p.getStateFrame.prefix));
 
-      p.getStateFrame.addTransform(AffineTransform(p.getStateFrame,lqrsys.getInputFrame,eye(length(x0)),-x0));
-      lqrsys.getInputFrame.addTransform(AffineTransform(lqrsys.getInputFrame,p.getStateFrame,eye(length(x0)),+x0));
+      obj.p.getStateFrame.addTransform(AffineTransform(obj.p.getStateFrame,lqrsys.getInputFrame,eye(length(x0)),-x0));
+      lqrsys.getInputFrame.addTransform(AffineTransform(lqrsys.getInputFrame,obj.p.getStateFrame,eye(length(x0)),+x0));
 
 
-      lqrsys = setOutputFrame(lqrsys,CoordinateFrame([p.getInputFrame.name,' + ',mat2str(u0,3)],length(u0),p.getInputFrame.prefix));
-      lqrsys.getOutputFrame.addTransform(AffineTransform(lqrsys.getOutputFrame,p.getInputFrame,eye(length(u0)),u0));
-      p.getInputFrame.addTransform(AffineTransform(p.getInputFrame,lqrsys.getOutputFrame,eye(length(u0)),-u0));
+      lqrsys = setOutputFrame(lqrsys,CoordinateFrame([obj.p.getInputFrame.name,' + ',mat2str(u0,3)],length(u0),obj.p.getInputFrame.prefix));
+      lqrsys.getOutputFrame.addTransform(AffineTransform(lqrsys.getOutputFrame,obj.p.getInputFrame,eye(length(u0)),u0));
+      obj.p.getInputFrame.addTransform(AffineTransform(obj.p.getInputFrame,lqrsys.getOutputFrame,eye(length(u0)),-u0));
 
 %       lqrsys = lqrsys.setInputFrame(obj.p.getOutputFrame());
 
-      traj = TrajectoryInLibrary(xtraj, utraj, lqrsys, p.getStateFrame(), trajname, comments);
+      traj = TrajectoryInLibrary(xtraj, utraj, lqrsys, obj.p.getStateFrame(), trajname, comments);
 
       obj.stabilization_trajectories{end+1} = traj;