Feature/spot experiments

.gitignore

@@ -11,8 +11,10 @@ serow_layout.json:Zone.Identifier
 /evaluation/h1_test/build/
 /evaluation/fsc_test/build/
 /evaluation/mujoco_test/build/
-/evaluation/mujoco_test/mujoco_data/*/est_*.bin
-/evaluation/mujoco_test/mujoco_data/*/serow_predictions.h5
+/evaluation/spot_test/build/
+evaluation/**/est_elevation_map.bin
+evaluation/**/serow_predictions.h5
+config/*_best_config.json
 /serow_hypertuner/build/
 /python/serow/__pycache__/
 /python/build/

config/h1.json

@@ -8,14 +8,14 @@
     },
     "model_path": "h1.urdf",
     "g": 9.81,
-    "joint_rate": 150.0,
+    "joint_rate": 100.0,
     "estimate_joint_velocity": false,
     "joint_cutoff_frequency": 10.0,
     "joint_position_variance": 0.1,
     "angular_momentum_cutoff_frequency": 5.0,
     "tau_0": 1.0,
     "tau_1": 0.1,
-    "imu_rate": 150.0,
+    "imu_rate": 100.0,
     "R_base_to_gyro": [
         1.0,
         0.0,
@@ -51,7 +51,7 @@
         0.0
     ],
     "gyro_cutoff_frequency": 5.0,
-    "force_torque_rate": 150.0,
+    "force_torque_rate": 100.0,
     "R_foot_to_force": {
         "0": [
             1.0,
@@ -100,7 +100,7 @@
             1.0
         ]
     },
-    "attitude_estimator_proportional_gain": 0.02,
+    "attitude_estimator_proportional_gain": 0.1,
     "attitude_estimator_integral_gain": 0.0,
     "estimate_contact_status": true,
     "high_threshold": 175.0,

config/spot.json

@@ -0,0 +1,236 @@
+{
+  "robot_name": "spot",
+  "base_frame": "base",
+  "point_feet": true,
+  "foot_frames": {
+    "0": "fl_foot",
+    "1": "fr_foot",
+    "2": "hl_foot",
+    "3": "hr_foot"
+  },
+  "model_path": "spot.urdf",
+  "g": 9.81,
+  "joint_rate": 270.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": 270.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
+  ],
+  "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": 270.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": true,
+  "convergence_cycles": 100,
+  "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
+  ],
+  "use_contacts_in_base_estimation": true,
+  "base_linear_velocity_covariance": [
+    5e-3,
+    5e-3,
+    5e-3
+  ],
+  "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
+  ],
+  "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": null,
+  "log_data": true,
+  "log_measurements": true,
+  "log_dir": "/tmp/"
+}

evaluation/h1_test/h1_isaac_test.cpp

@@ -31,7 +31,6 @@ void saveDataToHDF5(const std::string& fileName, const std::string& datasetPath,
     if (datasetPath.empty() || datasetPath[0] != '/') {
         throw std::invalid_argument("Dataset path must be non-empty and start with '/'");
     }
-    std::cout << fileName << '\n';
 
     // Open or create the file
     H5::H5File file;
@@ -319,8 +318,8 @@ int main(int argc, char** argv) {
                                                                  base_gt_positions[i][1],
                                                                  base_gt_positions[i][2]),
                                      .orientation = Eigen::Quaterniond(
-                                         base_gt_orientations[i][0], base_gt_orientations[i][1],
-                                         base_gt_orientations[i][2], base_gt_orientations[i][3])});
+                                         base_gt_orientations[i][3], base_gt_orientations[i][0],
+                                         base_gt_orientations[i][1], base_gt_orientations[i][2])});
             } else {
                 estimator.filter(imu, joints, force_torque);
             }

evaluation/h1_test/serow_viz.py

@@ -220,7 +220,7 @@ def remove_gt_bias(positions, orientations):
     R_forces = R_forces[:(-1)]
     L_forces = L_forces[:(-1)]
 
-    gt_pos, gt_rot = remove_gt_bias(gt_pos, gt_rot)
+    # gt_pos, gt_rot = remove_gt_bias(gt_pos, gt_rot)
 
     print("Base position ATE: ", compute_ATE_pos(gt_pos, est_pos_x, est_pos_y, est_pos_z))
     print("Base rotation ATE: ", compute_ATE_rot(gt_rot, est_rot_w, est_rot_x, est_rot_y, est_rot_z))
@@ -249,22 +249,22 @@ def remove_gt_bias(positions, orientations):
     figRot_GT, axsRot_GT = plt.subplots(4, 1, figsize=(10, 10), sharex=True)
     figRot_GT.suptitle("Base Orientation")
 
-    axsRot_GT[0].plot(timestamps, gt_rot[:, 0], label="Ground Truth", color="blue")
+    axsRot_GT[0].plot(timestamps, gt_rot[:, 3], label="Ground Truth", color="blue")
     axsRot_GT[0].plot(timestamps, est_rot_w, label="Estimated", color="orange", linestyle="--")
     axsRot_GT[0].set_ylabel("Orientation W")
     axsRot_GT[0].legend()
 
-    axsRot_GT[1].plot(timestamps, gt_rot[:, 1], label="Ground Truth", color="blue")
+    axsRot_GT[1].plot(timestamps, gt_rot[:, 0], label="Ground Truth", color="blue")
     axsRot_GT[1].plot(timestamps, est_rot_x, label="Estimated", color="orange", linestyle="--")
     axsRot_GT[1].set_ylabel("Orientation X")
     axsRot_GT[1].legend()
 
-    axsRot_GT[2].plot(timestamps, gt_rot[:, 2], label="Ground Truth", color="blue")
+    axsRot_GT[2].plot(timestamps, gt_rot[:, 1], label="Ground Truth", color="blue")
     axsRot_GT[2].plot(timestamps, est_rot_y, label="Estimated", color="orange", linestyle="--")
     axsRot_GT[2].set_ylabel("Orientation Y")
     axsRot_GT[2].legend()
 
-    axsRot_GT[3].plot(timestamps, gt_rot[:, 3], label="Ground Truth", color="blue")
+    axsRot_GT[3].plot(timestamps, gt_rot[:, 2], label="Ground Truth", color="blue")
     axsRot_GT[3].plot(timestamps, est_rot_z, label="Estimated", color="orange", linestyle="--")
     axsRot_GT[3].set_ylabel("Orientation Z")
     axsRot_GT[3].set_xlabel("Timestamp")

evaluation/spot_test/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.22.1...4.0.0)
+project(serow_tests VERSION 1.0.0 LANGUAGES CXX)
+
+# Default to C++20
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+find_package(pinocchio REQUIRED)
+find_package(serow REQUIRED)
+find_package(nlohmann_json REQUIRED)
+
+# Find and link HDF5
+find_package(HDF5 REQUIRED COMPONENTS CXX)
+include_directories(${HDF5_INCLUDE_DIRS})
+
+# Add the executable for HDF5 and Serow processing
+add_executable(spot_test ../spot_test.cpp)
+target_link_libraries(spot_test PRIVATE serow pinocchio::pinocchio ${HDF5_LIBRARIES} nlohmann_json::nlohmann_json)