[base_estimator] fixed velocity estimation

include/sot/talos_balance/base-estimator.hh

@@ -247,7 +247,7 @@ namespace dynamicgraph {
         Eigen::VectorXd   m_q_sot;            /// robot configuration according to SoT convention
         Eigen::VectorXd   m_v_pin;            /// robot velocities according to pinocchio convention
         Eigen::VectorXd   m_v_sot;            /// robot velocities according to SoT convention
-        Matrix3           m_oRchest;          /// chest orientation in the world from angular fusion
+        Matrix3           m_oRtorso;          /// chest orientation in the world from angular fusion
         Matrix3           m_oRff;             /// base orientation in the world
 
         /* Filter buffers*/

python/sot_talos_balance/test/appli_dcm_estimator.py

@@ -67,25 +67,28 @@ create_topic(robot.publisher, robot.device_filters.acc_filter, 'x_filtered', rob
 create_topic(robot.publisher, robot.dcm_estimator, 'c', robot = robot, data_type='vector')
 create_topic(robot.publisher, robot.dcm_estimator, 'dc', robot = robot, data_type='vector')
 create_topic(robot.publisher, robot.base_estimator, 'v', robot = robot, data_type='vector')
+create_topic(robot.publisher, robot.base_estimator, 'v_gyr', robot = robot, data_type='vector')
 
 
 # --- ROS SUBSCRIBER
 robot.subscriber = RosSubscribe("base_subscriber")
-# robot.subscriber.add("vector","position","/sot/base_link/position")
-# robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
-robot.subscriber.add("vector","position","/sot/torso_2_link/position")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
 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_f","/sot/gyro_filter/x_filtered")
 robot.subscriber.add("vector","acc_f","/sot/acc_filter/x_filtered")
 robot.subscriber.add("vector","com","/sot/dcm_estimator/c")
 robot.subscriber.add("vector","vcom","/sot/dcm_estimator/dc")
-robot.subscriber.add("vector","v_torso","/sot/base_estimator/v")
+robot.subscriber.add("vector","v_base","/sot/base_estimator/v")
+robot.subscriber.add("vector","v_gyr","/sot/base_estimator/v_gyr")
+
 
 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}.com'.format(robot.subscriber.name)) # force recalculation
 robot.device.after.addSignal('{0}.vcom'.format(robot.subscriber.name)) # force recalculation
-robot.device.after.addSignal('{0}.v_torso'.format(robot.subscriber.name)) # force recalculation
+robot.device.after.addSignal('{0}.v_base'.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

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,local_frame = False)
+pub_torso = GazeboLinkStatePublisher('base_link',1000,local_frame = False)
 
 print("Starting Gazebo link state publisher...")
 # pub_base.start()
@@ -18,9 +18,9 @@ raw_input("Wait before running the test")
 run_test('appli_dcm_estimator.py')
 
 sleep(2.0)
-runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+runCommandClient('robot.comTrajGen.move(1,-0.025,3.0)')
 sleep(5.0)
-runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,1.0)')
 sleep(5.0)
 runCommandClient("write_pdf_graph('/tmp/')")