From 68a6112faed0449d82d0418287ff734320a2bb48 Mon Sep 17 00:00:00 2001
From: fbailly <fbailly@laas.fr>
Date: Fri, 18 Jan 2019 14:39:01 +0100
Subject: [PATCH] [updated dcm_estimator] and changed the way to perform signal
introspection in SOT
---
.../talos/base_estimator_conf.py | 2 +-
.../test/test_dcm_estimator.py | 14 +++++----
python/sot_talos_balance/utils/plot_utils.py | 29 +++++++++++++++----
3 files changed, 34 insertions(+), 11 deletions(-)
diff --git a/python/sot_talos_balance/talos/base_estimator_conf.py b/python/sot_talos_balance/talos/base_estimator_conf.py
index 3b112cd..7152412 100644
--- a/python/sot_talos_balance/talos/base_estimator_conf.py
+++ b/python/sot_talos_balance/talos/base_estimator_conf.py
@@ -4,7 +4,7 @@ std_dev_zmp = 0.02
std_dev_fz = 50.
normal_force_margin = 30.
zmp_margin = 0.002
-w_imu = 1.0;
+w_imu = 0.;
beta = 0.00329
K_fb_feet_poses = 0.0; # gain used for updating foot positions
RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.075); # pos x, neg x, pos y, neg y size
diff --git a/python/sot_talos_balance/test/test_dcm_estimator.py b/python/sot_talos_balance/test/test_dcm_estimator.py
index 13c465a..a584712 100644
--- a/python/sot_talos_balance/test/test_dcm_estimator.py
+++ b/python/sot_talos_balance/test/test_dcm_estimator.py
@@ -9,6 +9,8 @@ from dynamic_graph.sot.core import SOT
from time import sleep
from IPython import embed
import os
+import numpy as np
+import matplotlib.pyplot as plt
def main(robot):
dt = robot.timeStep;
@@ -76,7 +78,7 @@ def main(robot):
robot.device_tracer.start();
robot.base_estimator_tracer.start();
robot.dcm_estimator_tracer.start();
- sleep(10.0);
+ sleep(1.0);
dump_tracer(robot.device_tracer);
dump_tracer(robot.base_estimator_tracer);
dump_tracer(robot.dcm_estimator_tracer);
@@ -89,10 +91,12 @@ def main(robot):
base_estimator_data, base_name = read_tracer_file('/tmp/dg_'+robot.base_estimator.name+'-q.dat')
dcm_estimator_data, dcm_name = read_tracer_file('/tmp/dg_'+robot.dcm_estimator.name+'-c.dat')
- plot_select_traj(device_data,[10,23,15], device_name)
- plot_select_traj(base_estimator_data,[10,23,15], base_name)
- plot_select_traj(dcm_estimator_data,[0,1,2], dcm_name)
+ # plot_select_traj(device_data,[10,23,15], device_name)
+ # plot_select_traj(base_estimator_data,[np.linspace(1,7,7,dtype=int)], base_name)
+ # plot_select_traj(dcm_estimator_data,[0,1,2], dcm_name)
+
+ # plt.show()
write_pdf_graph('/tmp/')
-² embed()
+
diff --git a/python/sot_talos_balance/utils/plot_utils.py b/python/sot_talos_balance/utils/plot_utils.py
index b5e1bd5..668210c 100644
--- a/python/sot_talos_balance/utils/plot_utils.py
+++ b/python/sot_talos_balance/utils/plot_utils.py
@@ -1,6 +1,9 @@
-import numpy as np
-import matplotlib.pyplot as plt
-from dynamic_graph import writeGraph
+import numpy as np
+import matplotlib.pyplot as plt
+from dynamic_graph import writeGraph
+from sot_talos_balance.create_entities_utils import create_tracer, dump_tracer
+from time import sleep
+from IPython import embed
import os
def read_tracer_file(filename):
@@ -10,12 +13,11 @@ def read_tracer_file(filename):
def plot_select_traj(traj,idxs,name):
''' plot selected idx of ND array'''
+ plt.figure()
nb_plots = np.size(idxs)
for idx in idxs:
plt.plot(traj[:,idx])
plt.title(name)
- plt.show()
- return
def write_pdf_graph(path):
''' outputs a pdf of the graph to the specified path '''
@@ -23,3 +25,20 @@ def write_pdf_graph(path):
os.system('dot -Tpdf '+path+'graph.dot -o '+path+'graph.pdf')
return
+def dump_sot_sig(robot,entity,signal_name,duration):
+ '''dumps a sot signal in /tmp
+ ex: dump_sot_sig(robot,robot.entity,'signal_name',1.)'''
+ full_sig_name = entity.name +'.'+signal_name
+ robot.tmp_tracer = create_tracer(robot,entity,'tmp_tracer', [signal_name])
+ robot.device.after.addSignal(full_sig_name)
+ robot.tmp_tracer.start()
+ sleep(duration)
+ dump_tracer(robot.tmp_tracer)
+ robot.tmp_tracer.clear()
+
+def plot_sot_sig(filename,idxs):
+ '''plots a dumped signal'''
+ filename = '/tmp/dg_'+filename+'.dat'
+ data, name = read_tracer_file(filename)
+ plot_select_traj(data,idxs,name)
+ return
\ No newline at end of file
--
GitLab