trace com admittance test

python/sot_talos_balance/create_entities_utils.py

@@ -5,6 +5,7 @@ from sot_talos_balance.joint_admittance_controller import JointAdmittanceControl
 from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
 from sot_talos_balance.com_admittance_controller import ComAdmittanceController
 
+from time import sleep
 from dynamic_graph import plug
 
 N_JOINTS = 32;
@@ -96,3 +97,16 @@ def create_com_admittance_controller(Kp,dt,robot):
     robot.dynamic.com.recompute(0)
     controller.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
     return controller
+
+def addTrace(tracer, entity, signalName):
+    signal = '{0}.{1}'.format(entity.name, signalName)
+    filename = '{0}-{1}'.format(entity.name, signalName)
+    tracer.add(signal, filename)
+
+def dump_tracer(tracer):
+    tracer.stop()
+    sleep(0.2)
+    tracer.dump()
+    sleep(0.2)
+    tracer.close()
+

python/sot_talos_balance/test/test_comAdmittance.py

@@ -8,6 +8,11 @@ from time import sleep
 import sys
 import os
 
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+import matplotlib.pyplot as plt
+import numpy as np
+
 def main(robot):
     dt = robot.timeStep;
 
@@ -46,6 +51,17 @@ def main(robot):
     robot.sot.push(robot.taskCom.task.name)
     robot.device.control.recompute(0)
 
+    # --- TRACER
+    robot.tracer = TracerRealTime("zmp_tracer")
+    robot.tracer.setBufferSize(80*(2**20))
+    robot.tracer.open('/tmp','dg_','.dat')
+    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+
+    addTrace(robot.tracer, robot.dynamic, 'zmp')
+    addTrace(robot.tracer, robot.estimator, 'dcm')
+
+    # SIMULATION
+
     plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
     sleep(1.0)
     os.system("rosservice call \start_dynamic_graph")
@@ -53,8 +69,32 @@ def main(robot):
 
     plug(robot.estimator.dcm,robot.com_admittance_control.zmpDes)
 
-    print("Activating controller...")
     robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
     robot.com_admittance_control.Kp.value = [10.0,10.0,0.0]
 
+    robot.tracer.start()
+
     sleep(5.0)
+
+    dump_tracer(robot.tracer)
+
+	  # --- DISPLAY
+    zmp_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-zmp.dat')
+    zmpDes_data = np.loadtxt('/tmp/dg_'+robot.estimator.name+'-dcm.dat')
+
+    plt.figure()
+    plt.plot(zmp_data[:,1],'b-')
+    plt.plot(zmpDes_data[:,1],'b--')
+    plt.plot(zmp_data[:,2],'r-')
+    plt.plot(zmpDes_data[:,2],'r--')
+    plt.title('ZMP real vs desired')
+    plt.legend(['Real x','Desired x','Real y','Desired y'])
+
+    plt.figure()
+    plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
+    plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
+    plt.title('ZMP error')
+    plt.legend(['Error on x','Error on y'])
+
+    plt.show()
+