WIP for LQR in body frame.

BuildTrajectoryLibrary.m

@@ -112,7 +112,7 @@
 lib = AddLqrControllersToLib('right-turn', lib, xtraj_turn2, utraj_turn2, gains);
 
 max_climb = 1.0; % m/s
-lib = FindClimbTrimDrake(p, max_climb, lib);
+[x0, u0, lib] = FindClimbTrimDrake(p, max_climb, lib);
 
 return;
 

ConvertXdotModelToStateEstimatorFrame.m

@@ -0,0 +1,16 @@
+function xdot_state_est = ConvertXdotModelToStateEstimatorFrame(x_model_frame, xdot_model_frame)
+
+  % in this case, the only difference between the two frames is the 180
+  % degree rotation about the x axis
+
+
+  % TODO: CHECKME
+  rotm(1:3,1:3) = [ 1,  0,  0;
+                    0, -1,  0;
+                    0,  0, -1];
+
+  rotm_full = blkdiag(rotm, rotm, rotm, rotm);
+
+  xdot_state_est = rotm_full * xdot_model_frame;
+
+end

DeltawingPlantStateEstFrame.m

@@ -0,0 +1,76 @@
+classdef DeltawingPlantStateEstFrame < DrakeSystem
+% Defines the dynamics for the TBS Capi delta wing in the state estimator
+% frame.  Use this for TVLQR but NOT FOR ANYTHING ELSE!
+
+  properties
+    p = {}; % DeltawingPlant
+  end
+
+  methods
+    function obj = DeltawingPlantStateEstFrame(p)
+      % @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(p.umin, p.umax); % input limits in [radians radians newtons]
+
+      obj.p = p;
+
+    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_drake_frame = ConvertStateEstimatorToDrakeFrame(x);
+
+      xdot_drake_frame = obj.p.dynamics(t, x_drake_frame, u);
+
+      xdot = ConvertXdotDrakeToStateEstimatorFrame(x_drake_frame, xdot_drake_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)
+      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.');
+
+    end
+
+
+    function v = constructVisualizer()
+        error('be super careful, you are attempting playback with a state-estimator-frame plant.  Probably you have plants mixed up.');
+    end
+
+  end  
+
+end

FindClimbTrimDrake.m

@@ -174,5 +174,5 @@
     gains.K_pd_aggressive_yaw = K_pd_aggressive_yaw;
 
 
-    lib = AddTiqrControllers(lib, 'tilqr', p, A, B, x0, u0, gains);
+    lib = AddTiqrControllers(lib, 'tilqr-climb', p, A, B, x0, u0, gains);
 end

HudBotVisualizer.m

@@ -52,7 +52,7 @@ function draw(obj, t, y)
       obj.pose_msg.utime = int64(t*1000000);
 
       obj.pose_msg.pos = y(1:3);
-      obj.pose_msg.vel = y(10:12);
+      obj.pose_msg.vel = [xdot; 0; y(12)];
 
       obj.pose_msg.orientation = rpy2quat([roll pitch yaw]);
 

TrajectoryInLibrary.m

@@ -212,11 +212,20 @@ function WriteToFile(obj, filename_prefix, dt, overwrite_files)
       %
       % @retval converted object
 
-      xtraj_convert = obj.xtraj.inFrame(obj.body_coordinate_frame);
-      lqrsys_convert = obj.lqrsys.inInputFrame(obj.body_coordinate_frame);
+%       xtraj_convert = obj.xtraj.inFrame(obj.body_coordinate_frame);
+%       lqrsys_convert = obj.lqrsys.inInputFrame(obj.body_coordinate_frame);
+%       
+%       converted_traj = TrajectoryInLibrary(xtraj_convert, obj.utraj, lqrsys_convert, obj.state_frame);
+%       
+
+      old_K = obj.lqrsys.D.eval(0);
+      x_drake = obj.xtraj.eval(0);
 
-      converted_traj = TrajectoryInLibrary(xtraj_convert, obj.utraj, lqrsys_convert, obj.state_frame);
+      x_body = ConvertDrakeFrameToEstimatorFrame(x_drake);
 
+      new_K = old_K * x_drake * x_body';
+
+
     end
 
     function converted_traj = ConvertToDrakeFrame(obj)

tbsc_model_for_turn.m

@@ -13,6 +13,8 @@
   %   2: roll ddot
   %   3: pitch ddot
 
+  full_state(6) = 1;
+
   full_state(4) = small_state(1);
   full_state(5) = small_state(2);
   full_state(7) = small_state(3);