From 41bfe6e27d6f86e30c8b9c3efd833058ad39fd21 Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Tue, 19 Feb 2019 16:21:08 +0100
Subject: [PATCH] reimplemented test_zmpEstimator.py
---
CMakeLists.txt | 1 +
.../test/appli_zmpEstimator.py | 60 ++++++++
.../test/test_zmpEstimator.py | 140 +++++++-----------
3 files changed, 111 insertions(+), 90 deletions(-)
create mode 100644 python/sot_talos_balance/test/appli_zmpEstimator.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 699c3fc..34cdcf9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -178,6 +178,7 @@ IF(BUILD_PYTHON_INTERFACE)
python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_ffSubscriber.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_zmpEstimator.py
+ python/${SOTTALOSBALANCE_PYNAME}/test/appli_zmpEstimator.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmComControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComZmpControl.py
diff --git a/python/sot_talos_balance/test/appli_zmpEstimator.py b/python/sot_talos_balance/test/appli_zmpEstimator.py
new file mode 100644
index 0000000..3db9b21
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_zmpEstimator.py
@@ -0,0 +1,60 @@
+from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
+from sot_talos_balance.create_entities_utils import create_zmp_estimator
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+
+dt = robot.timeStep;
+robot.comTrajGen = create_com_trajectory_generator(dt,robot);
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 10
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+robot.contactLF.feature.frame('desired')
+robot.contactLF.gain.setConstant(100)
+robot.contactLF.keep()
+locals()['contactLF'] = robot.contactLF
+
+robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+robot.contactRF.feature.frame('desired')
+robot.contactRF.gain.setConstant(100)
+robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+# -- estimator
+# -- this NEEDS to be called AFTER the operational points LF and RF are created
+robot.zmp_estimator = create_zmp_estimator(robot)
+
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+plug(robot.sot.control,robot.device.control)
+plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
+
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+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))
+robot.device.after.addSignal('{0}.zmp'.format(robot.zmp_estimator.name))
+robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name))
+
+addTrace(robot.tracer, robot.zmp_estimator, 'zmp')
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.dynamic, 'zmp')
+
+robot.tracer.start()
+
diff --git a/python/sot_talos_balance/test/test_zmpEstimator.py b/python/sot_talos_balance/test/test_zmpEstimator.py
index e4d605a..4666cf8 100644
--- a/python/sot_talos_balance/test/test_zmpEstimator.py
+++ b/python/sot_talos_balance/test/test_zmpEstimator.py
@@ -1,96 +1,56 @@
-from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
-from sot_talos_balance.create_entities_utils import create_zmp_estimator
-from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
-from dynamic_graph.sot.core.matrix_util import matrixToTuple
-from dynamic_graph import plug
-from dynamic_graph.sot.core import SOT
-from numpy import eye
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
from time import sleep
-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;
- robot.comTrajGen = create_com_trajectory_generator(dt,robot);
-
- # --- COM
- robot.taskCom = MetaTaskKineCom(robot.dynamic)
- robot.dynamic.com.recompute(0)
- robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
- robot.taskCom.task.controlGain.value = 10
-
- # --- CONTACTS
- #define contactLF and contactRF
- robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
- robot.contactLF.feature.frame('desired')
- robot.contactLF.gain.setConstant(100)
- robot.contactLF.keep()
- locals()['contactLF'] = robot.contactLF
-
- robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
- robot.contactRF.feature.frame('desired')
- robot.contactRF.gain.setConstant(100)
- robot.contactRF.keep()
- locals()['contactRF'] = robot.contactRF
-
- # -- estimator
- # -- this NEEDS to be called AFTER the operational points LF and RF are created
- robot.zmp_estimator = create_zmp_estimator(robot)
-
- robot.sot = SOT('sot')
- robot.sot.setSize(robot.dynamic.getDimension())
- plug(robot.sot.control,robot.device.control)
-
- robot.sot.push(robot.contactRF.task.name)
- robot.sot.push(robot.taskCom.task.name)
- robot.sot.push(robot.contactLF.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))
- robot.device.after.addSignal('{0}.zmp'.format(robot.zmp_estimator.name)) # force recalculation
-
- addTrace(robot.tracer, robot.zmp_estimator, 'zmp')
- addTrace(robot.tracer, robot.dynamic, 'com')
-
- plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN);
-
- robot.tracer.start()
-
- sleep(1.0);
- os.system("rosservice call \start_dynamic_graph")
- sleep(2.0);
- robot.comTrajGen.move(1,-0.025,4.0);
- sleep(20.0);
- robot.comTrajGen.startSinusoid(1,0.05,8.0);
- sleep(5.0);
-
- dump_tracer(robot.tracer)
-
- # --- DISPLAY
- zmp_data = np.loadtxt('/tmp/dg_'+robot.zmp_estimator.name+'-zmp.dat')
- com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
-
- plt.figure()
- plt.plot(com_data[:,1],'b-')
- plt.plot(com_data[:,2],'r-')
- plt.plot(com_data[:,3],'g-')
- plt.title('COM')
- plt.legend(['x','y','z'])
-
- plt.figure()
- plt.plot(zmp_data[:,1],'b-')
- plt.plot(zmp_data[:,2],'r-')
- plt.plot(zmp_data[:,3],'g-')
- plt.title('ZMP estimate')
- plt.legend(['x','y','z'])
-
- plt.show()
+run_test('appli_zmpEstimator.py')
+
+sleep(2.0)
+runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+sleep(20.0)
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+sleep(20.0)
+
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+zmpEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.zmp_estimator.name') + '-zmp.dat')
+zmpDyn_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat')
+com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
+
+plt.ion()
+
+plt.figure()
+plt.plot(zmpEst_data[:,1],'b-')
+plt.plot(zmpDyn_data[:,1],'b--')
+plt.plot(com_data[:,1],'b:')
+plt.plot(zmpEst_data[:,2],'r-')
+plt.plot(zmpDyn_data[:,2],'r--')
+plt.plot(com_data[:,2],'r:')
+plt.title('ZMP estimate vs dynamic vs CoM (planar)')
+plt.legend(['x estimate', 'x dynamic', 'x CoM', 'y estimate', 'y dynamic', 'y CoM'])
+
+plt.figure()
+plt.plot(com_data[:,1],'b-')
+plt.plot(com_data[:,2],'r-')
+plt.plot(com_data[:,3],'g-')
+plt.title('COM')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(zmpDyn_data[:,1],'b-')
+plt.plot(zmpDyn_data[:,2],'r-')
+plt.plot(zmpDyn_data[:,3],'g-')
+plt.title('ZMP dynamic')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(zmpEst_data[:,1],'b-')
+plt.plot(zmpEst_data[:,2],'r-')
+plt.plot(zmpEst_data[:,3],'g-')
+plt.title('ZMP estimate')
+plt.legend(['x','y','z'])
+
+raw_input("Wait before leaving the simulation")
--
GitLab