how to using serow on a quadruped robot

[MUXIANGRU]

Hi,

Thank you for sharing your work.I want to use serow on my quadruped robot.I have change some parameters in serow/config/estimation_params_centauro.yaml to fit my roboT thesedays.My robot in gazebo can provide imu_topic ,joint_states and the contact_force.But the result is not very good.I wonder that how can I set the parameters in the yaml ? There is picture of my trial. I'm looking forward to your reply.Thank you.

[MUXIANGRU]

The yaml:

Robot Frames (CHECK URDF)

isQuadruped: true base_link: "base" LFfoot: "LF_FOOT" RFfoot: "RF_FOOT" LHfoot: "LH_FOOT" RHfoot: "RH_FOOT" modelname: "/home/mxr/catkin_odom/src/simple_dog_simulation-wheel_test/simpledog_description/urdf/simpledog.urdf.xacro"

useLegOdom: false

ROS Topic Names

odom_topic: "/gazebo/odom" #only if usePoseUpdate is false and useLegOdom is false imu_topic: "/imu" joint_state_topic: "/joint_states"

geometry_msgs/WrenchStamped

note:this force isn't true how to solve?

LFfoot_force_torque_topic: "/test/lf_contact_force" RFfoot_force_torque_topic: "/test/rf_contact_force" LHfoot_force_torque_topic: "/test/lh_contact_force" RHfoot_force_torque_topic: "/test/rh_contact_force"

Ground-Truth #Not Mandatory - For comparison only

ground_truth: false ground_truth_odom_topic: "/gazebo/odom" ground_truth_com_topic: "/gazebo/odom" T_B_GT: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 , 1, 0, 0, 0 ,0, 1]

T_B_GT: [0, 0, 1, 0, 1, 0, 0, 0, 0, -1 , 0, 0, 0, 0 ,0, 1]

comp_with: false comp_with_odom0_topic: "/gazebo/odom"

If external Support Foot Detection is provided

support_idx_provided: false support_idx_topic: "/sp"

To publish relative data

debug_mode: false

TF from IMU to Body

T_B_A: [1, 0, 0, 0.13, 0, 1, 0, 0, 0, 0 , 1, 0.12, 0, 0 ,0, 1] T_B_G: [1, 0, 0, 0.13, 0, 1, 0, 0, 0, 0 , 1, 0.12, 0, 0 ,0, 1]

TF from F/T to Left Foot sensor to leg???

T_FT_LF: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 , 1 , 0, 0, 0, 0, 1] T_FT_LH: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 , 1 , 0, 0, 0, 0, 1]

TF from F/T to Right Foot

T_FT_RF: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 , 1 , 0, 0, 0, 0, 1] T_FT_RH: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 , 1 , 0, 0, 0, 0, 1]

TF from External Pose to Body only if useLegOdom is false(is the transform between base_pose and camera_pose)

T_B_P: [0, 0, 1, 0, -1, 0, 0, 0, 0, -1 ,0 ,0, 0, 0, 0, 1] #SVO without imu

T_B_P: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0 ,1 ,0, 0, 0, 0, 1]

ROS Topic Frequencies in Hz

imu_topic_freq: 400 joint_topic_freq: 200 fsr_topic_freq: 200

mass: 58.12 #robot mass

Schmitt Trigger - Contact Classifier

LegUpThres: 650.0 #Schmitt Trigger High Threshold in Newtons (N) LegLowThres: 50.0 #Schmitt Trigger Low Threshold in Newtons (N) LosingContact: 40 StrikingContact: 550 medianWindow: 9

Compute Joint Velocities

joint_cutoff_freq: 50 joint_noise_density: 0.2 # rad

useGEM: false LFforce_sigma: 0 LHforce_sigma: 0 RFforce_sigma: 0 RHforce_sigma: 0

probabilisticContactThreshold: 0.9 ContactDetectionWithCOP: false #For Flat Feet only foot_polygon_xmin: -0.1 foot_polygon_xmax: 0.1 foot_polygon_ymin: -0.05 foot_polygon_ymax: 0.05 LFcop_sigma: 0.005 RFcop_sigma: 0.005 LHcop_sigma: 0.005 RHcop_sigma: 0.005 ContactDetectionWithKinematics: false #Kinematic contribution to contact detection VelocityThres: 0.25 LFvnorm_sigma: 0.05 RFvnorm_sigma: 0.05 LHvnorm_sigma: 0.05 RHvnorm_sigma: 0.05

Mahony Filter for attitude

useMahony: false Mahony_Kp: 2.5 Mahony_Ki: 0.0

Madgwick Filter for attitude if useMahony is false

Madgwick_gain: 0.01

Leg Odometry Coefficients

Tau0: 1.0 Tau1: 0.0 #No F/T for Centauro :(

calibrateIMUbiases: true #otherwise specify accurate initial values for bias_a, bias_g maxImuCalibrationCycles: 10000

Rigid body Extended Kalman Filter (EKF)

To estimate:

3-D Body Position/Velocity

3-D IMU-Biases

contact_random_walk: 0.05

Process Noise STDs

accelerometer_bias_random_walk: 2.4336e-04 #m/s^2/sqrt(s) gyroscope_bias_random_walk: 1.0e-05 # rad/s/sqrt(s) accelerometer_noise_density: 0.05 # m/s^2 #Continuous Time gyroscope_noise_density: 0.05 # rad/s #Continuous Time

Measuremets

Odometry Measurement Noise std:

LO

leg_odom_position_noise_density: 5.0e-04 leg_odom_orientation_noise_density: 5.0e-02

VO

odom_orientation_noise_density: 5.0e-02 #4 odom_position_noise_density_x: 3.5e-02 #2.0e-01 odom_position_noise_density_y: 3.5e-02 odom_position_noise_density_z: 1.0

velocity_noise_density_x: 0.035 #0.007 velocity_noise_density_y: 0.035 #0.009 velocity_noise_density_z: 0.035 #0.007

IMU Initial Biases in the IMU local frame - Biases are substracted and are in the Body Frame!

bias_ax: 0 bias_ay: 0 bias_az: 0 bias_gx: 0 bias_gy: 0 bias_gz: 0

Filter Params

gravity: 9.7999999 #gravity constant (m/s^2)

CoM Extended Kalman Filter (EKF)

To estimate:

CoM 3-D Position/Velocity

3-D External Forces on the CoM

estimateCoM: true #Use the CoM EKF

---------------------------------------------

Specify the rest only if estimateCoM is true!!

---------------------------------------------

Process Noise

com_position_random_walk: 1.0e-3 #in m com_velocity_random_walk: 5.0e-1 #in m/s external_force_random_walk: 5.0 #in Newtons

Measurment Noise

com_position_noise_density: 1.0e-03 #CoM position in m com_acceleration_noise_density: 1.9519 #CoM Acceleration in m/s^2

Initial errors in External Forces

bias_fx: 0.0 #in Newtons bias_fy: 0.0 bias_fz: 0.0

Gyro LPF

useGyroLPF: false

Cutt-off Frequencies

gyro_cut_off_freq: 10.0 #Only if useGyroLPF is true

Gyro Moving Average Filter

maWindow: 5 #buffer size, the larger the smoother and delayed the signal, only if useGyroLPF is false

Filter Params

Ixx : 0.263893 #torso inertia around x Iyy : 2.018032 #torso inetria around y Izz : 2.056458 #torso inertia around z

[mrsp]

Hello MUXIANGRU Thanks for sharing your results.

By a first view I can see that the estimation diverges quickly in the vertical axis. Drift with respect to the vertical axis is common in legged robot kinematic-inertial estimation due to the fact that the base/CoM is unobservable in that axis.

Nevertheless, this should happen with a much slower rate! I suspect that the bias in the vertical axis is wrongly estimated.

Right now I am in the middle of updating serow to be more operational robust in real-time and with less parameters to tune. Therefore, would you mind sharing a ROS bag with the necessary topics e.g. joint states/contact forces/base imu/ground_truth (if you have it) and I shall take a look in my free time.

[MUXIANGRU]

Thank you for your advice,I will change some parameters to try again.And The rosbag document is as followed. /gazebo/odom is ground truth serow.zip

[mrsp]

Hello Muxiangru

You also need to send me the robots URDF.

[MUXIANGRU]

Hi,the urdf is as followed.Do you need the robot model(.STL)? quadruped_model.zip

[mrsp]

I am afraid this is not a standalone urdf in the file there exist multiple macros e.g. which are not default for ROS. Check for example the nao.urdf in the share folder of serow to get an idea of a standarized urdf.

[MUXIANGRU]

Hello,I changed the document.Thank you! urdf.zip

[mrsp]

Can you record a standing no motion dataset with your quadruped?

I am starting to think that something is wrong with the measured forces. In standing mode the forces must match the weight of the robot (or at least be relative near it).

[MUXIANGRU]

Hello,the contact force is computed by the joint force.The bag is as followed:

test.zip

[mrsp]

Are the forces measured in the corresponding feet local frames?

And why are there forces in the y-axis for the back legs?

[mrsp]

Instead of measured I wanted to say computed!

[mrsp]

Actually I think I've spotted the issue.

Your joint_state msg has a rate of 15-18hz (which is very low for kinematic inertial state estimation). Your imu has a rate of 80 hz which is low for imus in general but acceptable.

and your forces are 400hz which is great.

So since you compute forces with the dynamical model at 400hz you must have the joints states also available at that rate right?

[MUXIANGRU]

Thank you for your advice.I will try it.

[mrsp]

Hi to further elaborate on this imagine that since your IMU runs 4 times faster than the joint encoders what happens is you perform the Kalman Predict step 4 times before you Kalman Update once.

The predict step increases uncertainty (and thus drift) 4 times in a row before a correction is performed with the update step.

The gait your robot performed was fast and dynamic. If you want to have reliable estimates for this kind of gait then you need faster feedback. Otherwise you can speed down your gait and perform a slow pace walking where fast feedback is not mandatory.

[mrsp]

feel free to reopen when you have new data.