Feature/owdy rl

.gitignore

@@ -24,3 +24,6 @@ serow_layout.json:Zone.Identifier
 /serow_ros2/build/
 /serow_ros2/install/
 /serow_ros2/log/
+/python/datasets/*
+/python/models/*
+/python/*_test.npz

config/a1.json

@@ -40,6 +40,8 @@
     0.0,
     -1.0
   ],
+  "imu_outlier_detection": true,
+  "use_imu_orientation": true,
   "calibrate_initial_imu_bias": true,
   "max_imu_calibration_cycles": 100,
   "bias_gyro": [

config/anymal_b.json

@@ -38,6 +38,8 @@
     0.0,
     -1.0
   ],
+  "imu_outlier_detection": true,
+  "use_imu_orientation": true,
   "calibrate_initial_imu_bias": false,
   "max_imu_calibration_cycles": 500,
   "bias_gyro": [

config/estimation.json

@@ -68,6 +68,10 @@
         0.0,
         1.0
     ],
+    // boolean: whether or not to use the IMU outlier detection during the filter step
+    "imu_outlier_detection": true,
+    // boolean: whether or not to use the IMU orientation during the ContactEKF update step
+    "use_imu_orientation": true,
     // boolean: whether or not to estimate initial values for the IMU gyro/accelerometer biases
     "calibrate_initial_imu_bias": false,
     // integer: number of IMU measurements to use for estimating the IMU gyro/accelerometer biases

config/go1.json

@@ -40,6 +40,8 @@
     0.0,
     1.0
   ],
+  "imu_outlier_detection": true,
+  "use_imu_orientation": true,
   "calibrate_initial_imu_bias": true,
   "max_imu_calibration_cycles": 100,
   "bias_gyro": [

config/go2.json

@@ -40,6 +40,8 @@
         0.0,
         -1.0
     ],
+    "imu_outlier_detection": true,
+    "use_imu_orientation": true,
     "calibrate_initial_imu_bias": true,
     "max_imu_calibration_cycles": 1000,
     "bias_gyro": [

config/go2_pytest.json

@@ -0,0 +1,250 @@
+{
+    "robot_name": "go2",
+    "base_frame": "base",
+    "point_feet": true,
+    "foot_frames": {
+        "0": "FL_foot",
+        "1": "FR_foot",
+        "2": "RL_foot",
+        "3": "RR_foot"
+    },
+    "model_path": "go2.urdf",
+    "g": 9.81,
+    "joint_rate": 500.0,
+    "estimate_joint_velocity": true,
+    "joint_cutoff_frequency": 15.0,
+    "joint_position_variance": 0.0005,
+    "angular_momentum_cutoff_frequency": 5.0,
+    "tau_0": 0.1,
+    "tau_1": 0.0,
+    "imu_rate": 500.0,
+    "R_base_to_gyro": [
+        1.0,
+        0.0,
+        0.0,
+        0.0,
+        -1.0,
+        0.0,
+        0.0,
+        0.0,
+        -1.0
+    ],
+    "R_base_to_acc": [
+        1.0,
+        0.0,
+        0.0,
+        0.0,
+        -1.0,
+        0.0,
+        0.0,
+        0.0,
+        -1.0
+    ],
+    "imu_outlier_detection": false,
+    "use_imu_orientation": false,
+    "calibrate_initial_imu_bias": false,
+    "max_imu_calibration_cycles": 1000,
+    "bias_gyro": [
+        0.0,
+        0.0,
+        0.0
+    ],
+    "bias_acc": [
+        0.0,
+        0.0,
+        0.0
+    ],
+    "gyro_cutoff_frequency": 5.0,
+    "force_torque_rate": 500.0,
+    "R_foot_to_force": {
+        "0": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0
+        ],
+        "1": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0
+        ],
+        "2": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0
+        ],
+        "3": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0
+        ]
+    },
+    "R_foot_to_torque": null,
+    "attitude_estimator_proportional_gain": 0.1,
+    "attitude_estimator_integral_gain": 0.0,
+    "estimate_contact_status": true,
+    "high_threshold": 4.0,
+    "low_threshold": 2.0,
+    "median_window": 13,
+    "outlier_detection": false,
+    "convergence_cycles": 0,
+    "imu_angular_velocity_covariance": [
+        5e-5,
+        5e-5,
+        5e-5
+    ],
+    "imu_angular_velocity_bias_covariance": [
+        1e-8,
+        1e-8,
+        1e-8
+    ],
+    "imu_linear_acceleration_covariance": [
+        5e-3,
+        5e-3,
+        5e-3
+    ],
+    "imu_linear_acceleration_bias_covariance": [
+        1e-8,
+        1e-8,
+        1e-8
+    ],
+    "base_orientation_covariance": [
+        1e-3,
+        1e-3,
+        1e-3
+    ],
+    "contact_position_covariance": [
+        5e-6,
+        5e-6,
+        5e-5
+    ],
+    "contact_orientation_covariance": null,
+    "contact_position_slip_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "contact_orientation_slip_covariance": null,
+    "com_position_process_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "com_linear_velocity_process_covariance": [
+        1e-4,
+        1e-4,
+        1e-4
+    ],
+    "external_forces_process_covariance": [
+        1e-1,
+        1e-1,
+        1e-1
+    ],
+    "com_position_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "com_linear_acceleration_covariance": [
+        1e-2,
+        1e-2,
+        1e-2
+    ],
+    "initial_base_position_covariance": [
+        1e-4,
+        1e-4,
+        1e-4
+    ],
+    "initial_base_orientation_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "initial_base_linear_velocity_covariance": [
+        1e-3,
+        1e-3,
+        1e-3
+    ],
+    "initial_contact_position_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "initial_contact_orientation_covariance": null,
+    "initial_imu_linear_acceleration_bias_covariance": [
+        1e-4,
+        1e-4,
+        1e-4
+    ],
+    "initial_imu_angular_velocity_bias_covariance": [
+        1e-4,
+        1e-4,
+        1e-4
+    ],
+    "initial_com_position_covariance": [
+        1e-6,
+        1e-6,
+        1e-6
+    ],
+    "initial_com_linear_velocity_covariance": [
+        1e-3,
+        1e-3,
+        1e-3
+    ],
+    "initial_external_forces_covariance": [
+        1.0,
+        1.0,
+        1.0
+    ],
+    "T_base_to_odom": null,
+    "enable_terrain_estimation": false,
+    "terrain_estimator": "fast",
+    "minimum_terrain_height_variance": 1e-3,
+    "maximum_contact_points": 4,
+    "maximum_recenter_distance": 0.35,
+    "minimum_contact_probability": 0.15,
+    "T_base_to_ground_truth": [
+        1.0,
+        0.0,
+        0.0,
+        -0.03270961,
+        0.0,
+        1.0,
+        0.0,
+        -0.00222456,
+        0.0,
+        0.0,
+        1.0,
+        -0.01544948,
+        0.0,
+        0.0,
+        0.0,
+        1.0
+    ],
+    "log_data": false,
+    "log_measurements": false,
+    "log_dir": "/tmp/serow"
+}