diff --git a/python/sot_talos_balance/test/test_COMTraj_tracer.py b/python/sot_talos_balance/test/test_COMTraj_tracer.py index 972fd6ac01d3bd2d37ad1f7444ac06de2b838f0e..bad9e33e3e01b18093b18bd2d040c1a335ff1490 100644 --- a/python/sot_talos_balance/test/test_COMTraj_tracer.py +++ b/python/sot_talos_balance/test/test_COMTraj_tracer.py @@ -2,7 +2,7 @@ from sot_talos_balance.create_entities_utils import * from sot_talos_balance.utils.plot_utils import * import sot_talos_balance.talos_conf as conf import sot_talos_balance.motor_parameters as motor_params -import sot_talos_balance.control_manager_conf as control_manager_conf +import sot_talos_balance.control_manager_conf as param_server_conf 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 @@ -48,8 +48,9 @@ def main(robot): robot.device.control.recompute(0) # --- ESTIMATION - robot.ctrl_manager = create_ctrl_manager(control_manager_conf, motor_params, dt); - #~ robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt) + robot.param_server = create_parameter_server(param_server_conf,dt) + # robot.ctrl_manager = create_ctrl_manager(param_server_conf,motor_params,dt) + # robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt) robot.device_filters = create_device_filters(robot, dt) robot.imu_filters = create_imu_filters(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, conf) @@ -60,7 +61,8 @@ def main(robot): robot.device_tracer = create_tracer(robot,robot.device, 'device_tracer', outputs) outputs = ['q'] robot.estimator_tracer = create_tracer(robot,robot.base_estimator, 'estimator_tracer', outputs) - + robot.device.after.addSignal('base_estimator.q') + # --- RUN SIMULATION plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN); sleep(1.0); @@ -77,8 +79,7 @@ def main(robot): dump_tracer(robot.device_tracer); dump_tracer(robot.estimator_tracer); print 'data dumped' - write_pdf_graph('/tmp/') - return + # --- DISPLAY device_data = read_tracer_file('/tmp/dg_'+robot.device.name+'-robotState.dat')