[updated dcm_estimator] and changed the way to perform signal introspection in SOT

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 

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()
+

python/sot_talos_balance/utils/plot_utils.py

-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