Skip to content
Snippets Groups Projects
Commit 68a6112f authored by François Bailly's avatar François Bailly
Browse files

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

parent 9f18ba34
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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()
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment