[zmpControl] Filters

python/sot_talos_balance/create_entities_utils.py

@@ -420,12 +420,16 @@ def create_dcm_estimator(robot, dt, robot_name='robot'):
     plug(robot.base_estimator.v, dcm_estimator.v)
     return dcm_estimator
 
-def create_zmp_estimator(robot):
+def create_zmp_estimator(robot,filter=False):
     estimator = SimpleZmpEstimator("zmpEst")
     plug(robot.dynamic.LF,estimator.poseLeft)
     plug(robot.dynamic.RF,estimator.poseRight)
-    plug(robot.device.forceLLEG,estimator.wrenchLeft)
-    plug(robot.device.forceRLEG,estimator.wrenchRight)
+    if filter and hasattr(robot,'device_filters'):
+        plug(robot.device_filters.ft_LF_filter.x_filtered,estimator.wrenchLeft)
+        plug(robot.device_filters.ft_RF_filter.x_filtered,estimator.wrenchRight)
+    else:
+        plug(robot.device.forceLLEG,estimator.wrenchLeft)
+        plug(robot.device.forceRLEG,estimator.wrenchRight)
 
     estimator.init()
     return estimator

python/sot_talos_balance/test/appli_dcmZmpControl_feedback.py

@@ -9,6 +9,9 @@ from dynamic_graph.tracer_real_time import TracerRealTime
 robot.timeStep = robot.device.getTimeStep()
 dt = robot.timeStep;
 
+# --- filters
+robot.device_filters = create_device_filters(robot,dt)
+
 # --- Dummy estimator
 robot.estimator = create_dummy_dcm_estimator(robot)
 
@@ -45,7 +48,7 @@ robot.taskCom.task.setWithDerivative(True)
 
 # -- estimator
 # -- this NEEDS to be called AFTER the operational points LF and RF are created
-robot.zmp_estimator = create_zmp_estimator(robot)
+robot.zmp_estimator = create_zmp_estimator(robot,False)
 plug(robot.zmp_estimator.zmp,robot.com_admittance_control.zmp)
 
 robot.sot = SOT('sot')