publish state manually to keep up with qp_input

juanfxlopez · Mar 13, 2015 · ee45bff · ee45bff
1 parent 6ea6ef9
commit ee45bff
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 5 deletions.
diff --git a/examples/Atlas/+atlasControllers/AtlasPlanEvalAndControlSystem.m b/examples/Atlas/+atlasControllers/AtlasPlanEvalAndControlSystem.m
@@ -21,6 +21,7 @@
   properties(SetAccess=protected)
     control
     plan_eval;
+    robot;
     options;
     lc
     monitor
@@ -52,6 +53,7 @@
       obj.control = control;
       obj.plan_eval = plan_eval;
       obj.options = options;
+      obj.robot = r;
 
       if isempty(obj.plan_eval) || isempty(obj.control)
         obj.lc = lcm.lcm.LCM.getSingleton();
@@ -104,6 +106,10 @@
           t0 = tic();
         end
         encodeQPInputLCMMex(qp_input);
+        out_frame = obj.robot.getOutputFrame();
+        if isprop(out_frame, 'lcmcoder')
+          obj.lc.publish('EST_ROBOT_STATE', out_frame.lcmcoder.encode(t, x));
+        end
         if ~obj.quiet
           lcm_time = toc(t0);
           fprintf(1, 'lcm_serialize: %f, ', lcm_time);

diff --git a/examples/Atlas/runAtlasWalkingAsync.m b/examples/Atlas/runAtlasWalkingAsync.m
@@ -57,13 +57,13 @@ function runAtlasWalkingAsync(example_options)
 % walking_plan_data = StandingPlan.from_standing_state(x0, r);
 planeval = atlasControllers.AtlasPlanEval(r, walking_plan_data);
 
+% Make our Atlas listen for ATLAS_COMMAND over LCM and publish EST_ROBOT_STATE
+r = r.setInputFrame(drcFrames.AtlasInput(r));
+r = r.setOutputFrame(drcFrames.AtlasState(r));
+
 % Wrap the planeval in a Drake system
 planeval_sys = atlasControllers.AtlasPlanEvalAndControlSystem(r, [], planeval);
-% Make sure planeval_sys can publish EST_ROBOT_STATE over LCM
-planeval_sys = planeval_sys.setOutputFrame(drcFrames.AtlasState(r));
-
-% Make our Atlas listen for ATLAS_COMMAND over LCM
-r = r.setInputFrame(drcFrames.AtlasInput(r));
+planeval_sys = planeval_sys.setInputFrame(r.getOutputFrame);
 
 sys = cascade(r, planeval_sys);