updated dcm estimator scripts

python/sot_talos_balance/test/appli_dcm_estimator.py

@@ -60,8 +60,8 @@ robot.dcm_estimator           = create_dcm_estimator(robot, dt)
 robot.publisher = create_rospublish(robot, 'robot_publisher')
 create_topic(robot.publisher, robot.base_estimator, 'q', robot = robot, data_type='vector')
 create_topic(robot.publisher, robot.base_estimator, 'q_imu', robot = robot, data_type='vector')
-create_topic(robot.publisher, robot.device_filters.gyro_filter, 'x', robot = robot, data_type='vector')
 create_topic(robot.publisher, robot.device_filters.gyro_filter, 'x_filtered', robot = robot, data_type='vector')
+create_topic(robot.publisher, robot.device_filters.acc_filter, 'x_filtered', robot = robot, data_type='vector')
 
 
 # --- ROS SUBSCRIBER
@@ -71,11 +71,11 @@ robot.subscriber = RosSubscribe("base_subscriber")
 robot.subscriber.add("vector","position","/sot/torso_2_link/position")
 robot.subscriber.add("vector","q_est","/sot/base_estimator/q")
 robot.subscriber.add("vector","q_imu","/sot/base_estimator/q_imu")
-robot.subscriber.add("vector","gyro","/sot/gyro_filter/x")
 robot.subscriber.add("vector","gyro_f","/sot/gyro_filter/x_filtered")
+robot.subscriber.add("vector","acc_f","/sot/acc_filter/x_filtered")
 
 robot.device.after.addSignal('{0}.position'.format(robot.subscriber.name)) # force recalculation
-robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
+# robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
 # robot.device.after.addSignal('{0}.q_est'.format(robot.subscriber.name))        # force recalculation
 # robot.device.after.addSignal('{0}.q_imu'.format(robot.subscriber.name))        # force recalculation
 # robot.device.after.addSignal('{0}.gyro'.format(robot.subscriber.name))        # force recalculation

python/sot_talos_balance/test/test_dcm_estimator.py

@@ -7,7 +7,7 @@ import numpy                                as np
 from IPython import embed
 
 # pub_base = GazeboLinkStatePublisher('base_link',1000)
-pub_torso = GazeboLinkStatePublisher('torso_2_link',1000,True)
+pub_torso = GazeboLinkStatePublisher('torso_2_link',1000,local_frame = True)
 
 print("Starting Gazebo link state publisher...")
 # pub_base.start()