Merge pull request RobotLocomotion#2 from RussTedrake/jc-twip

closed loop system using mimoCascade and mimoFeedback
aidiyang · Aug 18, 2015 · 5b4cc0c · 5b4cc0c
2 parents 2efa669 + cf57ac0
commit 5b4cc0c
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 33 deletions.
diff --git a/drake/examples/TwoWheeledInvertedPendulum/runObserverControl.m b/drake/examples/TwoWheeledInvertedPendulum/runObserverControl.m
@@ -18,28 +18,9 @@
 controller = r.balanceLQR();
 
 %wire up the system
-sysCl = buildClosedLoopObserverControl(r, observer, controller);
+sysCl = mimoFeedback(mimoCascade(r,observer),controller);
 
-xtraj = sysCl.simulate([0, 5]);
-v.playback(xtraj)
-
-function sysCl = buildClosedLoopObserverControl(plant, observer, controller)
-    mdl_name = 'observer_control';
-    new_system(mdl_name, 'Model');
-    mdl = SimulinkModelHandle(mdl_name);
-    mdl.addSubsystem('plant', getModel(plant));
-    mdl.addSubsystem('controller', getModel(controller));
-    mdl.addSubsystem('observer', getModel(observer));
-    add_block('simulink3/Signals & Systems/Mux', [mdl_name, '/Mux']);
-    mdl.add_line('controller/1', 'Mux/1');
-    mdl.add_line('controller/1', 'plant/1');
-    mdl.add_line('plant/1', 'Mux/2');
-    mdl.add_line('Mux/1', 'observer/1');
-    mdl.add_line('observer/1', 'controller/1');
-    add_block('simulink3/Sinks/Out1',  [mdl_name, '/out']);
-    mdl.add_line('observer/1', 'out/1');
-    sysCl = SimulinkModel(mdl, 0);
-    sysCl = sysCl.setOutputFrame(observer.getOutputFrame);
-end
+ytraj = sysCl.simulate([0, 5]);
+v.playback(ytraj)
 
 end
diff --git a/drake/systems/@DynamicalSystem/mimoFeedback.m b/drake/systems/@DynamicalSystem/mimoFeedback.m
@@ -38,8 +38,7 @@
 %
 % @param output_select an optional structure with fields "system" and 
 % "output" representing the outputs from sys1 and sys2 (analogous to
-% input_select).  @default all of the unused outputs from sys1
-% followed by all of unused outputs from sys2.
+% input_select).  @default all output from sys1  
 %
 % Notes:
 %  - The inputs to a system may not be used by more than one input (from
@@ -105,16 +104,9 @@
     rangecheck(output_select(i).output,1,getNumFrames(sys{output_select(i).system}.getOutputFrame));
   end
 else
-  sys1outputs = setdiff(1:getNumFrames(sys1.getOutputFrame),[sys1_to_sys2_connection.from_output]);
-  sys2outputs = setdiff(1:getNumFrames(sys2.getOutputFrame),[sys2_to_sys1_connection.from_output]);
-  output_select=[];
-  for i=1:length(sys1outputs)
+  for i=1:getNumFrames(sys1.getOutputFrame)
     output_select(i).system=1;
-    output_select(i).output=sys1outputs(i);
-  end
-  for i=1:length(sys2outputs)
-    output_select(end+1).system=2;
-    output_select(end).output=sys2outputs(i);
+    output_select(i).output=i;
   end
 end