diff --git a/.gitignore b/.gitignore
index fae25dce1facaccabf8aa15085cf2491d5959212..7ecae7d373f3c31b6d5e51edd4c90749667984b3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,10 +1,13 @@
-.cproject
+\.cproject
.project
.settings/
README.txt
bin/
build/
+build_debug/
build_release/
+build-debug/
+build-release/
lib/
.idea/
./Wolf.user
@@ -30,6 +33,5 @@ src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
\.vscode/
-build_release/
wolf.found
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 355377d34fc5eace45662a92970c30c837fe3ad7..ff1e2eb36d460125006290be0be82e37296c54e9 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -3,6 +3,13 @@ stages:
- build_and_test
############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+ # Print variables
+ - echo $WOLF_CORE_BRANCH
+ - echo $CI_COMMIT_BRANCH
+ - echo $WOLF_BODYDYNAMICS_BRANCH
+ - echo $WOLF_IMU_BRANCH
+
.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
@@ -117,6 +124,7 @@ license_headers:
paths:
- ci_deps/wolf/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
script:
@@ -134,6 +142,7 @@ build_and_test:bionic:
paths:
- ci_deps/imu/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_wolfimu_definition
@@ -153,6 +162,7 @@ build_and_test:focal:
paths:
- ci_deps/imu/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_wolfimu_definition
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2d37d37685bef61e1d3478e941593df4a675be8..9ca4dd5c2fccc846c026b27a12f511560f160c58 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,6 @@
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
-if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
- cmake_policy(SET CMP0003 NEW)
-endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
@@ -16,7 +12,8 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
+set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
@@ -67,10 +64,6 @@ if(BUILD_TESTS)
enable_testing()
endif()
-#CMAKE modules
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
# Some wolf compilation options
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
@@ -79,12 +72,14 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
-FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
FIND_PACKAGE(wolfimu REQUIRED)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
# ============ CONFIG.H ============
-string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+# variable used to compile the config.h.in file
+string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -103,7 +98,6 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ============
-INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
@@ -121,6 +115,7 @@ include/bodydynamics/factor/factor_force_torque.h
include/bodydynamics/factor/factor_force_torque_preint.h
include/bodydynamics/factor/factor_inertial_kinematics.h
include/bodydynamics/factor/factor_point_feet_nomove.h
+include/bodydynamics/factor/factor_point_feet_altitude.h
)
SET(HDRS_FEATURE
include/bodydynamics/feature/feature_force_torque.h
@@ -184,11 +179,12 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfimu)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
# Build demos
IF(BUILD_DEMOS)
- #Build examples
MESSAGE("Building demos.")
ADD_SUBDIRECTORY(demos)
ENDIF(BUILD_DEMOS)
@@ -199,14 +195,38 @@ IF(BUILD_TESTS)
add_subdirectory(test)
ENDIF(BUILD_TESTS)
+
#install library
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib
- ARCHIVE DESTINATION lib)
+ LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+ ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+#Â Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+ ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+ INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+ PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+ FILES
+ ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+ DESTINATION
+ ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PLUGIN_NAME} INTERFACE
+ $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
@@ -221,17 +241,12 @@ INSTALL(FILES ${HDRS_PROCESSOR}
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
-
export(PACKAGE ${PLUGIN_NAME})
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
diff --git a/bodydynamics.found b/bodydynamics.found
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake b/cmake_modules/wolfbodydynamicsConfig.cmake
deleted file mode 100644
index cfb538c1d7d49ef39ea0f6994488f9bc59d27724..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfbodydynamicsConfig.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolfbodydynamics_INCLUDE_DIRS
- NAMES bodydynamics.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
-IF(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Found wolf bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
-ELSE(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Couldn't find wolf bodydynamics include dirs")
-ENDIF(wolfbodydynamics_INCLUDE_DIRS)
-
-FIND_LIBRARY(
- wolfbodydynamics_LIBRARIES
- NAMES libwolfbodydynamics.so
- PATHS /usr/local/lib)
-IF(wolfbodydynamics_LIBRARIES)
- MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
-ELSE(wolfbodydynamics_LIBRARIES)
- MESSAGE("Couldn't find wolf bodydynamics lib")
-ENDIF(wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- SET(wolfbodydynamics_FOUND TRUE)
- ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- set(wolfbodydynamics_FOUND FALSE)
-ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_FOUND)
- IF (NOT wolfbodydynamics_FIND_QUIETLY)
- MESSAGE(STATUS "Found wolf bodydynamics: ${wolfbodydynamics_LIBRARIES}")
- ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
-ELSE (wolfbodydynamics_FOUND)
- IF (wolfbodydynamics_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
- ENDIF (wolfbodydynamics_FIND_REQUIRED)
-ENDIF (wolfbodydynamics_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolfbodydynamics_FOUND FALSE)
- unset(wolfbodydynamics_INCLUDE_DIRS)
- unset(wolfbodydynamics_LIBRARIES)
-
- # Reset the CMake module path to its state when this script was called.
- set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
- # FindPackage() use the camelcase library name, not uppercase.
- if (wolfbodydynamics_FIND_QUIETLY)
- message(STATUS "Failed to find wolf bodydynamics- " ${REASON_MSG} ${ARGN})
- elseif(wolfbodydynamics_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
- # that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfbodydynamics_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
-endif(NOT wolfbodydynamics_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfbodydynamics_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Template
-
-#EXAMPLE Suppose that Wolf Template depends on laser_scan_utils
-# FIND_PACKAGE(laser_scan_utils REQUIRED)
-# list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
-# list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
-
-# Making sure that wolf is always looked for
-if(NOT wolf_FOUND)
- FIND_PACKAGE(wolfcore REQUIRED)
-
- #We reverse in order to insert at the start
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
- list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-
- list(REVERSE wolfbodydynamics_LIBRARIES)
- list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
- list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES
-SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake.in b/cmake_modules/wolfbodydynamicsConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..0722eb6dafef443a92638b2e888219c9fe0bd017
--- /dev/null
+++ b/cmake_modules/wolfbodydynamicsConfig.cmake.in
@@ -0,0 +1,13 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(wolfcore REQUIRED)
+FIND_DEPENDENCY(wolfimu REQUIRED)
+FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 4c57e21c724478a56852c13e498ee655ac4a84f8..78d2b265912a542724c6ccaa3609638dc49259e2 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -2,74 +2,63 @@
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") # necessary for files using boost
FIND_PACKAGE(pinocchio QUIET)
-FIND_PACKAGE(multicontact-api QUIET)
if (pinocchio_FOUND)
# SYSTEM disables warnings from library headers
include_directories(
SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
)
-
+
add_library(mcapi_utils mcapi_utils.cpp)
-
- # add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
- # target_link_libraries(mcapi_povcdl_estimation
- # mcapi_utils
- # ${wolfcore_LIBRARIES}
- # ${wolfimu_LIBRARIES}
- # ${PLUGIN_NAME}
- # ${multicontact-api_LIBRARIES}
- # ${pinocchio_LIBRARIES}
- # )
- # target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
-
-
- # add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
- # target_link_libraries(mcapi_pov_estimation
- # mcapi_utils
- # ${wolfcore_LIBRARIES}
- # ${wolfimu_LIBRARIES}
- # ${PLUGIN_NAME}
- # ${multicontact-api_LIBRARIES}
- # ${pinocchio_LIBRARIES}
- # )
- # target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
-
+ target_link_libraries(mcapi_utils Eigen3::Eigen)
+
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
- mcapi_utils
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
+ wolfcore
+ wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
+ mcapi_utils
)
-
- add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
- target_link_libraries(solo_real_pov_estimation
- mcapi_utils
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
+
+ add_executable(solo_imu_mocap solo_imu_mocap.cpp)
+ target_link_libraries(solo_imu_mocap
+ wolfcore
+ wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
)
-
- add_executable(solo_mocap_imu solo_mocap_imu.cpp)
- target_link_libraries(solo_mocap_imu
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
- ${PLUGIN_NAME}
- ${pinocchio_LIBRARIES}
- /usr/local/lib/libcnpy.so
- z
+
+ add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
+ target_link_libraries(solo_imu_kine_mocap
+ wolfcore
+ wolfimu
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
)
+
endif()
-# add_executable(test_cnpy test_cnpy.cpp)
-# target_link_libraries(test_cnpy
+
+
+
+# add_library(demo_logger demo_logger.cpp)
+# target_include_directories (demo_logger PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+
+# add_executable(solo_imu_mocap_refactored solo_imu_mocap_refactored.cpp)
+# target_link_libraries(solo_imu_mocap_refactored
+# demo_logger
+# ${wolfcore_LIBRARIES}
+# ${wolfimu_LIBRARIES}
+# ${PLUGIN_NAME}
+# ${pinocchio_LIBRARIES}
# /usr/local/lib/libcnpy.so
+# z
# )
-
diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
deleted file mode 100644
index 9fdc2feebbea2a407bd5268d0be86388804e5de3..0000000000000000000000000000000000000000
--- a/demos/mcapi_pov_estimation.cpp
+++ /dev/null
@@ -1,751 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <iostream>
-// #include <random>
-#include <yaml-cpp/yaml.h>
-#include <Eigen/Dense>
-#include <ctime>
-#include "eigenmvn.h"
-
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/joint-configuration.hpp"
-#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/center-of-mass.hpp"
-#include "pinocchio/algorithm/centroidal.hpp"
-#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-
-// MCAPI
-#include <curves/fwd.h>
-#include <curves/so3_linear.h>
-#include <curves/se3_curve.h>
-#include <curves/polynomial.h>
-#include <curves/bezier_curve.h>
-#include <curves/piecewise_curve.h>
-#include <curves/exact_cubic.h>
-#include <curves/cubic_hermite_spline.h>
-#include "multicontact-api/scenario/contact-sequence.hpp"
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/capture/capture_pose.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/processor/processor_odom_3d.h>
-#include <core/sensor/sensor_odom_3d.h>
-
-// IMU
-#include "imu/sensor/sensor_imu.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/capture/capture_leg_odom.h"
-#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-
-// UTILS
-#include "mcapi_utils.h"
-
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinocchio;
-using namespace multicontact_api::scenario;
-
-typedef pinocchio::SE3Tpl<double> SE3;
-typedef pinocchio::ForceTpl<double> Force;
-
-
-int main (int argc, char **argv) {
- // retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
- bool noisy_measurements = config["noisy_measurements"].as<bool>();
-
- // estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
- double std_odom3d_est = config["std_odom3d_est"].as<double>();
-
- // simulated sensor noises
- double std_acc_sim = config["std_acc_sim"].as<double>();
- double std_gyr_sim = config["std_gyr_sim"].as<double>();
- double std_pbc_sim = config["std_pbc_sim"].as<double>();
- double std_vbc_sim = config["std_vbc_sim"].as<double>();
- double std_f_sim = config["std_f_sim"].as<double>();
- double std_tau_sim = config["std_tau_sim"].as<double>();
- // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
-
- // priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
- Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
-
- double fz_thresh = config["fz_thresh"].as<double>();
- double scale_dist = config["scale_dist"].as<double>();
- bool base_dist_only = config["base_dist_only"].as<bool>();
- bool mass_dist = config["mass_dist"].as<bool>();
- bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
- bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
- bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
-
- std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
-
- ContactSequence cs;
- std::cout << "Loading file " << contact_sequence_file << std::endl;
- cs.loadFromBinary(contact_sequence_file);
-
- auto q_traj = cs.concatenateQtrajectories();
- auto dq_traj = cs.concatenateDQtrajectories();
- auto ddq_traj = cs.concatenateDDQtrajectories();
- auto c_traj = cs.concatenateCtrajectories();
- auto dc_traj = cs.concatenateDCtrajectories();
- auto L_traj = cs.concatenateLtrajectories();
- auto tau_traj = cs.concatenateTauTrajectories();
- std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
- unsigned int nbc = ee_names.size();
- std::vector<curves::piecewise_t> cforce_traj_lst;
- for (auto ee_name: ee_names){
- std::cout << ee_name << std::endl;
- auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
- cforce_traj_lst.push_back(cforce_traj);
- }
-
- if (min_t < 0){
- min_t = q_traj.min();
- }
- if (max_t < 0){
- max_t = q_traj.max();
- }
-
- // initialize some vectors and fill them with dicretized data
- std::vector<double> t_arr; // timestamps
- std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
- std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
- std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
- std::vector<VectorXd> c_traj_arr; // w_p_wc
- std::vector<VectorXd> dc_traj_arr; // w_v_wc
- std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
- std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
- int N = (max_t - min_t)/dt;
- int i_t = 0;
- for (double t=min_t; t <= max_t; t += dt){
- t_arr.push_back(t);
- q_traj_arr.push_back(q_traj(t));
- dq_traj_arr.push_back(dq_traj(t));
- ddq_traj_arr.push_back(ddq_traj(t));
- c_traj_arr.push_back(c_traj(t));
- dc_traj_arr.push_back(dc_traj(t));
- L_traj_arr.push_back(L_traj(t));
- tau_traj_arr.push_back(tau_traj(t));
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
- }
- i_t++;
- }
- std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
-
- // Creating measurements from simulated trajectory
- // Load the urdf model
- Model model;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
- std::cout << "model name: " << model.name << std::endl;
- Data data(model);
-
- // distorted model (modified inertias)
- Model model_dist;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
-
- Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
- Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
-
- // distortion
- if (base_dist_only){
- if (!mass_dist){
- int root_joint_id = model.getJointId("root_joint");
- Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
- }
- }
- else{
- for (int i=0; i < model_dist.inertias.size(); i++){
- if (mass_dist){
- double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
- model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
- std::cout << "mass model " << model.inertias[i].mass() << std::endl;
- std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
- }
- else {
- Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
- }
- }
- }
- Data data_dist(model_dist);
-
- std::vector<int> ee_ids;
- std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
- ee_ids.push_back(model.getFrameId(ee_name));
- std::cout << ee_name << std::endl;
- }
- std::vector<Vector3d> contacts = contacts_from_footrect_center();
-
- ///////////////////////////////////////////////
- // Create some vectors to aggregate the groundtruth and measurements
- // Groundtruth
- std::vector<Vector3d> p_ob_gtr_v;
- std::vector<Vector4d> q_ob_gtr_v;
- std::vector<Vector3d> v_ob_gtr_v;
- // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
- // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
- // std::vector<Vector3d> L_traj_arr; // already have it directly
-
-
- // Measurements
- std::vector<Vector3d> acc_b_meas_v;
- std::vector<Vector3d> w_b_meas_v;
- std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
- std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
- std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
- std::vector<Vector3d> p_bc_meas_v;
- std::vector<Vector3d> v_bc_meas_v;
- std::vector<Vector3d> Lq_meas_v;
- std::vector<Matrix3d> Iq_meas_v;
- // define vectors to aggregate ground truth and simulated data
- for (unsigned int i = 0; i < q_traj_arr.size(); i++)
- {
- /**
- * Groundtruth to recover (in world frame, vels/inertial frame):
- * b position --> OK
- * b orientation (quat) --> OK
- * b velocity: linear classical one --> OK
- * CoM posi --> OK
- * CoM vel --> OK
- * Angular momentum --> OK
- *
- * Measures to recover (in local frames):
- * IMU readings --> OK
- * Kinematics readings --> OK
- * Wrench readings --> OK
- * CoM relative position/vel --> OK
- * Iq, Lq --> OK
- *
- **/
-
- // retrieve traj data
- VectorXd q = q_traj_arr[i];
- VectorXd dq = dq_traj_arr[i];
- VectorXd ddq = ddq_traj_arr[i];
- // Vector3d c = c_traj_arr[i]; // gtr
- // VectorXd dc = dc_traj_arr[i]; // gtr
- // VectorXd L = L_traj_arr[i]; // gtr
- VectorXd tau = tau_traj_arr[i];
-
- // std::cout << "q " << q.transpose() << std::endl;
- // std::cout << "dq " << dq.transpose() << std::endl;
- // std::cout << "ddq " << ddq.transpose() << std::endl;
-
- //////////////////////////////////
- //////////////////////////////////
- // Set ground truth variables
- Vector3d p_wb = q.head<3>(); // gtr
- Vector4d quat_wb = q.segment<4>(3); // gtr
- SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
- // body vel is expressed in body frame
- Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
- Vector3d b_w_b = dq.segment<3>(3); // meas
- Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
-
- Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
-
- ////////////////////////////////////
- ////////////////////////////////////
- // Get measurements
-
- // IMU readings
- // We use every quantity (spatial acc, body angvel, dr=lin spatial linvel) in the same body frame -> we get acc in body frame
- Vector3d b_acc = b_asp_b + b_w_b.cross(b_v_b);
- // universe o frame in mcapi/pinocchio is supposed to have same reference point and orientation as WOLF world frame
- Vector3d b_proper_acc = b_acc - oTb.rotation().transpose() * gravity(); // meas
-
- // update and retrieve kinematics
- forwardKinematics(model, data, q);
- updateFramePlacements(model, data);
-
- // compute all linear jacobians (only for quadruped)
- MatrixXd Jlinvel_all(12, model.nv); Jlinvel_all.setZero();
- for (int i_ee=0; i_ee < nbc; i_ee++){
- MatrixXd Jee(6, model.nv); Jee.setZero();
- computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- Jlinvel_all.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
- }
- // remove free flyer (not needed for force reconstruction)
- MatrixXd Jlinvel_noff = Jlinvel_all.rightCols(model.nv-6);
-
- // estimate forces from torques
- VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- VectorXd tau_joints = tau_cf.tail(model.nv-6) - tau; // remove measured joint torque (not on the free flyer)
- VectorXd forces = Jlinvel_noff.transpose().lu().solve(tau_joints);
-
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- auto oTl = data.oMf[ee_ids[i_ee]];
- auto bTl = oTb.inverse() * oTl;
- Vector3d b_p_l = bTl.translation(); // meas
- Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
- p_bl_meas_v[i_ee].push_back(b_p_l);
- q_bl_meas_v[i_ee].push_back(b_qvec_l);
- // TODO: Only for 6D wrench or 3D force
-
- // Compute either from torques or from given force and compare
- // std::cout << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
- // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]);
- wrench_meas_v[i_ee].push_back(forces.segment(3*i_ee, 3));
- }
-
-
- //////////////////////////////////////////////
- // COM relative position and velocity measures
- VectorXd q_static = q;
- VectorXd dq_static = dq;
- q_static.head<7>() << 0,0,0, 0,0,0,1;
- dq_static.head<6>() << 0,0,0, 0,0,0;
- // compute for both perfect and distorted models to get the bias
- centerOfMass(model, data, q_static, dq_static);
- centerOfMass(model_dist, data_dist, q_static, dq_static);
-
- // c =def w_p_wc
- // Vector3d b_p_bc = oTb.inverse().act(c); // meas
- Vector3d b_p_bc = data.com[0]; // meas
- Vector3d b_p_bc_dist = data_dist.com[0]; // meas
- Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
-
- // velocity due to to gesticulation only in base frame
- // -> set world frame = base frame and set body frame spatial vel to zero
- Vector3d b_v_bc = data.vcom[0]; // meas
- Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
- Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
-
- /////////////////////////
- // Centroidal inertia and gesticulation kinetic momentum
- Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
- Matrix3d bias_Iw = b_Iw_dist - b_Iw;
-
- // gesticulation in base frame: just compute centroidal momentum with static q and vq
- Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
- Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
- Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
-
-
- // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
- // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
- // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
- // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
-
- /////////////////////////
- // Agreggate everything in vectors for easier WOLF consumption
- p_ob_gtr_v.push_back(p_wb);
- q_ob_gtr_v.push_back(quat_wb);
- v_ob_gtr_v.push_back(w_v_wb);
- bp_traj_arr.push_back(bias_pbc);
- // Lc_gtr_v;
-
- // Measurements
- acc_b_meas_v.push_back(b_proper_acc);
- w_b_meas_v.push_back(b_w_b);
- p_bc_meas_v.push_back(b_p_bc_dist);
- if (vbc_is_dist){
- v_bc_meas_v.push_back(b_v_bc_dist);
- }
- else {
- v_bc_meas_v.push_back(b_v_bc);
- }
- if (Lgest_is_dist){
- Lq_meas_v.push_back(b_Lc_gesti_dist);
- }
- else{
- Lq_meas_v.push_back(b_Lc_gesti);
- }
- if (Iw_is_dist){
- Iq_meas_v.push_back(b_Iw_dist);
- }
- else {
- Iq_meas_v.push_back(b_Iw);
- }
- }
-
- /////////////////////////
- // WOLF enters the scene
- // SETUP PROBLEM/SENSORS/PROCESSORS
- std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- ProblemPtr problem = Problem::create("POV", 3);
-
- // CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
- ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
-
- // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
- Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
- SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
- SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
- ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
- // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
- ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
- ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
- //////////////////////////////
-
- TimeStamp t0(t_arr[0]);
- VectorComposite x_origin("POV", {p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0]});
- VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
-
- proc_imu->setOrigin(KF1);
- proc_legodom->setOrigin(KF1);
-
-
- ///////////////////////////////////////////
- // process measurements in sequential order
- double ts_since_last_kf = 0;
-
- // Add gaussian noises
- std::time_t epoch = std::time(nullptr);
- int64_t seed = (int64_t)epoch;
- Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
-
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
- //////////////////////////////////////////
-
- unsigned int nb_kf = problem->getTrajectory()->getFrameMap().size();
- for (unsigned int i=0; i < t_arr.size(); i++){
- // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
- TimeStamp ts(t_arr[i]); // works best with pyb trajectories
-
- // IMU
- Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
-
- if (noisy_measurements){
- acc_gyr_meas.head<3>() += noise_acc.samples(1);
- acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
- }
-
- Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
-
- ////////////////////
- /////////////////
-
- CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
- CImu->process();
-
-
- if (i > 0){
- // Leg odometry
- // 1) detect feet in contact
- std::vector<int> feet_int_contact;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
- // basic contact detection based on normal foot vel, things break if no foot is in contact
- // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
- if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
- feet_int_contact.push_back(i_ee);
- }
- }
- // 2) fill the leg odometry data matrix, depending on the contact dimension
- Eigen::MatrixXd data_legodom;
- if (dimc == 6){
- // cols organized as:
- // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
- data_legodom.resize(7,feet_int_contact.size()*2);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_int_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
- j++;
- }
- }
- else if (dimc == 3){
- // cols organized as:
- // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
- data_legodom.resize(3,feet_int_contact.size()*2 + 1);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_int_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
- j++;
- }
- data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
- }
- // std::cout << "data_legodom\n" << data_legodom << std::endl;
- // 3) process the data and its cov
- Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
- CLO->process();
- }
-
- // if new KF add
- // unsigned int new_kf_nb = problem->getTrajectory()->getFrameMap().size();
- // if (new_kf_nb > nb_kf){
- // auto last_kf = problem->getTrajectory()->getFrameMap().rbegin()->second;
- // nb_kf = new_kf_nb;
- // // ADD ABSOLUTE FACTOR (GPS LIKE)
- // }
-
-
- ts_since_last_kf += dt;
- if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- // problem->print(4,true,true,true);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
- ts_since_last_kf = 0;
- }
-
- // Store current state estimation
- VectorComposite curr_state = problem->getState(ts);
- p_ob_fbk_v.push_back(curr_state['P']);
- q_ob_fbk_v.push_back(curr_state['O']);
- v_ob_fbk_v.push_back(curr_state['V']);
-
- }
-
- ////////////////////////////////////////////
- // BIAS FACTORS
- // Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
- FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
-
-
- ///////////////////////////////////////////
- //////////////// SOLVING //////////////////
- ///////////////////////////////////////////
- problem->print(4,true,true,true);
- if (solve_end){
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
- std::cout << report << std::endl;
- }
-
- //////////////////////////////////////
- //////////////////////////////////////
- // STORE DATA //
- //////////////////////////////////////
- //////////////////////////////////////
- std::fstream file_gtr;
- std::fstream file_est;
- std::fstream file_fbk;
- std::fstream file_kfs;
- std::fstream file_cov;
- file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
- file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
- file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
- file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
- file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
- std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
- file_est << header_est;
- file_fbk << header_est;
- std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz\n";
- file_kfs << header_kfs;
- file_gtr << header_kfs;
- std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz\n";
- file_cov << header_cov;
-
- VectorComposite state_est;
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_gtr_v[i](0) << ","
- << p_ob_gtr_v[i](1) << ","
- << p_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](0) << ","
- << q_ob_gtr_v[i](1) << ","
- << q_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](3) << ","
- << v_ob_gtr_v[i](0) << ","
- << v_ob_gtr_v[i](1) << ","
- << v_ob_gtr_v[i](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- state_est = problem->getState(t_arr[i]);
- file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << state_est['P'](0) << ","
- << state_est['P'](1) << ","
- << state_est['P'](2) << ","
- << state_est['O'](0) << ","
- << state_est['O'](1) << ","
- << state_est['O'](2) << ","
- << state_est['O'](3) << ","
- << state_est['V'](0) << ","
- << state_est['V'](1) << ","
- << state_est['V'](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_fbk_v[i](0) << ","
- << p_ob_fbk_v[i](1) << ","
- << p_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](0) << ","
- << q_ob_fbk_v[i](1) << ","
- << q_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](3) << ","
- << v_ob_fbk_v[i](0) << ","
- << v_ob_fbk_v[i](1) << ","
- << v_ob_fbk_v[i](2)
- << "\n";
- }
-
- VectorComposite kf_state;
- CaptureBasePtr cap_imu;
- VectorComposite bi_bias_est;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
- auto kf = elt.second;
- kf_state = kf->getState();
- cap_imu = kf->getCaptureOf(sen_imu);
- bi_bias_est = cap_imu->getState();
-
- file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << kf_state['P'](0) << ","
- << kf_state['P'](1) << ","
- << kf_state['P'](2) << ","
- << kf_state['O'](0) << ","
- << kf_state['O'](1) << ","
- << kf_state['O'](2) << ","
- << kf_state['O'](3) << ","
- << kf_state['V'](0) << ","
- << kf_state['V'](1) << ","
- << kf_state['V'](2) << ","
- << bi_bias_est['I'](0) << ","
- << bi_bias_est['I'](1) << ","
- << bi_bias_est['I'](2) << ","
- << bi_bias_est['I'](3) << ","
- << bi_bias_est['I'](4) << ","
- << bi_bias_est['I'](5)
- << "\n";
-
- // compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
- problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
- problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
- problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
-
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
- file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << Qp(0,0) << ","
- << Qp(1,1) << ","
- << Qp(2,2) << ","
- << Qo(0,0) << ","
- << Qo(1,1) << ","
- << Qo(2,2) << ","
- << Qv(0,0) << ","
- << Qv(1,1) << ","
- << Qv(2,2) << ","
- << Qbi(0,0) << ","
- << Qbi(1,1) << ","
- << Qbi(2,2) << ","
- << Qbi(3,3) << ","
- << Qbi(4,4) << ","
- << Qbi(5,5)
- << "\n";
- }
-
- file_gtr.close();
- file_est.close();
- file_fbk.close();
- file_kfs.close();
- file_cov.close();
-
-
- // // Print factor energy
- // for (auto kf: problem->getTrajectory()->getFrameMap()){
- // for (auto cap: kf->getCaptureList()){
- // for (auto feat: cap->getFeatureList()){
- // for (auto fac: feat->getFactorList()){
- // if (fac->getType() == "FactorForceTorquePreint"){
- // std::cout << "Type: FactorForceTorquePreint" << std::endl;
- // auto fac_ftp = std::static_pointer_cast<FactorForceTorquePreint>(fac);
- // std::cout << "try the cost" << std::endl;
- // std::cout << fac_ftp->cost() << std::endl;
- // }
- // }
- // }
- // }
- // }
-
-
-}
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
deleted file mode 100644
index 82bf7d64e2c9779a17a590fc7b9a6cf2b58cded1..0000000000000000000000000000000000000000
--- a/demos/mcapi_povcdl_estimation.cpp
+++ /dev/null
@@ -1,969 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <iostream>
-// #include <random>
-#include <yaml-cpp/yaml.h>
-#include <Eigen/Dense>
-#include <ctime>
-#include "eigenmvn.h"
-
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/joint-configuration.hpp"
-#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/center-of-mass.hpp"
-#include "pinocchio/algorithm/centroidal.hpp"
-#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-
-// MCAPI
-#include <curves/fwd.h>
-#include <curves/so3_linear.h>
-#include <curves/se3_curve.h>
-#include <curves/polynomial.h>
-#include <curves/bezier_curve.h>
-#include <curves/piecewise_curve.h>
-#include <curves/exact_cubic.h>
-#include <curves/cubic_hermite_spline.h>
-#include "multicontact-api/scenario/contact-sequence.hpp"
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/capture/capture_pose.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/processor/processor_odom_3d.h>
-#include <core/sensor/sensor_odom_3d.h>
-
-// IMU
-#include "imu/sensor/sensor_imu.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/capture/capture_leg_odom.h"
-#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-
-// UTILS
-#include "mcapi_utils.h"
-
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinocchio;
-using namespace multicontact_api::scenario;
-
-typedef pinocchio::SE3Tpl<double> SE3;
-typedef pinocchio::ForceTpl<double> Force;
-
-
-int main (int argc, char **argv) {
- // retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
- bool noisy_measurements = config["noisy_measurements"].as<bool>();
-
- // estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
- double std_odom3d_est = config["std_odom3d_est"].as<double>();
-
- // simulated sensor noises
- double std_acc_sim = config["std_acc_sim"].as<double>();
- double std_gyr_sim = config["std_gyr_sim"].as<double>();
- double std_pbc_sim = config["std_pbc_sim"].as<double>();
- double std_vbc_sim = config["std_vbc_sim"].as<double>();
- double std_f_sim = config["std_f_sim"].as<double>();
- double std_tau_sim = config["std_tau_sim"].as<double>();
- // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
-
- // priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
- Eigen::Map<Vector3d> bias_pbc_prior(bias_pbc_prior_vec.data());
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
- Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
-
- double fz_thresh = config["fz_thresh"].as<double>();
- double scale_dist = config["scale_dist"].as<double>();
- bool base_dist_only = config["base_dist_only"].as<bool>();
- bool mass_dist = config["mass_dist"].as<bool>();
- bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
- bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
- bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
-
- std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
-
- ContactSequence cs;
- std::cout << "Loading file " << contact_sequence_file << std::endl;
- cs.loadFromBinary(contact_sequence_file);
-
- auto q_traj = cs.concatenateQtrajectories();
- auto dq_traj = cs.concatenateDQtrajectories();
- auto ddq_traj = cs.concatenateDDQtrajectories();
- auto c_traj = cs.concatenateCtrajectories();
- auto dc_traj = cs.concatenateDCtrajectories();
- auto L_traj = cs.concatenateLtrajectories();
- auto tau_traj = cs.concatenateTauTrajectories();
- std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
- unsigned int nbc = ee_names.size();
- std::vector<curves::piecewise_t> cforce_traj_lst;
- for (auto ee_name: ee_names){
- std::cout << ee_name << std::endl;
- auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
- cforce_traj_lst.push_back(cforce_traj);
- }
-
- if (min_t < 0){
- min_t = q_traj.min();
- }
- if (max_t < 0){
- max_t = q_traj.max();
- }
-
- // initialize some vectors and fill them with dicretized data
- std::vector<double> t_arr; // timestamps
- std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
- std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
- std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
- std::vector<VectorXd> c_traj_arr; // w_p_wc
- std::vector<VectorXd> dc_traj_arr; // w_v_wc
- std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
- std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
- int N = (max_t - min_t)/dt;
- int i_t = 0;
- for (double t=min_t; t <= max_t; t += dt){
- t_arr.push_back(t);
- q_traj_arr.push_back(q_traj(t));
- dq_traj_arr.push_back(dq_traj(t));
- ddq_traj_arr.push_back(ddq_traj(t));
- c_traj_arr.push_back(c_traj(t));
- dc_traj_arr.push_back(dc_traj(t));
- L_traj_arr.push_back(L_traj(t));
- tau_traj_arr.push_back(tau_traj(t));
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
- }
- i_t++;
- }
- std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
-
- // Creating measurements from simulated trajectory
- // Load the urdf model
- Model model;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
- std::cout << "model name: " << model.name << std::endl;
- Data data(model);
-
- // distorted model (modified inertias)
- Model model_dist;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
- Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
- Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
- if (base_dist_only){
- if (!mass_dist){
- int root_joint_id = model.getJointId("root_joint");
- Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
- }
- }
- else{
- for (int i=0; i < model_dist.inertias.size(); i++){
- if (mass_dist){
- double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
- model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
- std::cout << "mass model " << model.inertias[i].mass() << std::endl;
- std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
- }
- else {
- Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
- }
- }
- }
- Data data_dist(model_dist);
-
- // Recover end effector frame ids
- std::vector<int> ee_ids;
- std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
- ee_ids.push_back(model.getFrameId(ee_name));
- std::cout << ee_name << std::endl;
- }
- // std::vector<Vector3d> contacts = contacts_from_footrect_center(); // for Humanoid only TODO
-
-
- ///////////////////////////////////////////////
- // Create some vectors to aggregate the groundtruth and measurements
- // Groundtruth
- std::vector<Vector3d> p_ob_gtr_v;
- std::vector<Vector4d> q_ob_gtr_v;
- std::vector<Vector3d> v_ob_gtr_v;
- // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
- // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
- // std::vector<Vector3d> L_traj_arr; // already have it directly
-
- // Measurements
- std::vector<Vector3d> acc_b_meas_v;
- std::vector<Vector3d> w_b_meas_v;
- std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
- std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
- std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
- std::vector<Vector3d> p_bc_meas_v;
- std::vector<Vector3d> v_bc_meas_v;
- std::vector<Vector3d> Lq_meas_v;
- std::vector<Matrix3d> Iq_meas_v;
- // define vectors to aggregate ground truth and simulated data
- for (unsigned int i = 0; i < q_traj_arr.size(); i++)
- {
- /**
- * Groundtruth to recover (in world frame, vels/inertial frame):
- * b position --> OK
- * b orientation (quat) --> OK
- * b velocity: linear classical one --> OK
- * CoM posi --> OK
- * CoM vel --> OK
- * Angular momentum --> OK
- *
- * Measures to recover (in local frames):
- * IMU readings --> OK
- * Kinematics readings --> OK
- * Wrench readings --> OK
- * CoM relative position/vel --> OK
- * Iq, Lq --> OK
- *
- **/
-
- // retrieve traj data
- VectorXd q = q_traj_arr[i];
- VectorXd dq = dq_traj_arr[i];
- VectorXd ddq = ddq_traj_arr[i];
- // Vector3d c = c_traj_arr[i]; // gtr
- // VectorXd dc = dc_traj_arr[i]; // gtr
- // VectorXd L = L_traj_arr[i]; // gtr
- VectorXd tau = tau_traj_arr[i];
-
- // std::cout << "q " << q.transpose() << std::endl;
- // std::cout << "dq " << dq.transpose() << std::endl;
- // std::cout << "ddq " << ddq.transpose() << std::endl;
-
- //////////////////////////////////
- //////////////////////////////////
- // Set ground truth variables
- Vector3d p_wb = q.head<3>(); // gtr
- Vector4d quat_wb = q.segment<4>(3); // gtr
- SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
- // body vel is expressed in body frame
- Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
- Vector3d b_w_b = dq.segment<3>(3); // meas
- Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
-
- Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
-
- ////////////////////////////////////
- ////////////////////////////////////
- // Get measurements
-
- // IMU readings
- // We use every quantity (spatial acc, body angvel, dr=lin spatial linvel) in the same body frame -> we get acc in body frame
- Vector3d b_acc = b_asp_b + b_w_b.cross(b_v_b);
- // universe o frame in mcapi/pinocchio is supposed to have same reference point and orientation as WOLF world frame
- Vector3d b_proper_acc = b_acc - oTb.rotation().transpose() * gravity(); // meas
-
- // update and retrieve kinematics
- forwardKinematics(model, data, q);
- updateFramePlacements(model, data);
-
- // compute all linear jacobians (only for quadruped)
- MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
- for (int i_ee=0; i_ee < nbc; i_ee++){
- MatrixXd Jee(6, model.nv); Jee.setZero();
- computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
- }
-
- // estimate forces from torques
- VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
-
- // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
- // Solve in Least square sense (3 ways):
- // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
- VectorXd forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
- // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
-
- Vector3d o_f_tot = Vector3d::Zero();
- std::cout << "" << std::endl;
- std::cout << "t: " << t_arr[i] << std::endl;
- // std::cout << "ddq: " << ddq.transpose() << std::endl;
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- auto oTl = data.oMf[ee_ids[i_ee]];
- auto bTl = oTb.inverse() * oTl;
- Vector3d b_p_l = bTl.translation(); // meas
- Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
- p_bl_meas_v[i_ee].push_back(b_p_l);
- q_bl_meas_v[i_ee].push_back(b_qvec_l);
- // TODO: Only for 6D wrench or 3D force
-
- // Compute either from torques or from given force and compare
- std::cout << "f_force_from_tau - f_force_from_cs_file: " << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
-
- Vector3d l_f_l = forces.segment(3*i_ee, 3);
- // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]); // EITHER
- wrench_meas_v[i_ee].push_back(l_f_l);
-
- Vector3d o_f_l = oTl.rotation() * l_f_l;
- // std::cout << "o_p_" << ee_names[i_ee] << " " << oTl.translation().transpose() << std::endl;
- // std::cout << "o_f_" << ee_names[i_ee] << " " << o_f_l.transpose() << std::endl;
- o_f_tot += o_f_l;
- }
- // std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl;
- std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
- std::cout << "pin.gravity - wolf gravity: " << model.gravity.linear() - wolf::gravity() << std::endl;
-
-
- //////////////////////////////////////////////
- // COM relative position and velocity measures
- VectorXd q_static = q;
- VectorXd dq_static = dq;
- q_static.head<7>() << 0,0,0, 0,0,0,1;
- dq_static.head<6>() << 0,0,0, 0,0,0;
- // compute for both perfect and distorted models to get the bias
- centerOfMass(model, data, q_static, dq_static);
- centerOfMass(model_dist, data_dist, q_static, dq_static);
-
- // c =def w_p_wc
- // Vector3d b_p_bc = oTb.inverse().act(c); // meas
- Vector3d b_p_bc = data.com[0]; // meas
- Vector3d b_p_bc_dist = data_dist.com[0]; // meas
- Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
-
- // velocity due to to gesticulation only in base frame
- // -> set world frame = base frame and set body frame spatial vel to zero
- Vector3d b_v_bc = data.vcom[0]; // meas
- Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
- Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
-
- /////////////////////////
- // Centroidal inertia and gesticulation kinetic momentum
- Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
- Matrix3d bias_Iw = b_Iw_dist - b_Iw;
-
- // gesticulation in base frame: just compute centroidal momentum with static q and vq
- Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
- Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
- Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
-
-
- // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
- // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
- // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
- // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
-
- /////////////////////////
- // Agreggate everything in vectors for easier WOLF consumption
- p_ob_gtr_v.push_back(p_wb);
- q_ob_gtr_v.push_back(quat_wb);
- v_ob_gtr_v.push_back(w_v_wb);
- bp_traj_arr.push_back(bias_pbc);
- // Lc_gtr_v;
-
- // Measurements
- acc_b_meas_v.push_back(b_proper_acc);
- w_b_meas_v.push_back(b_w_b);
- p_bc_meas_v.push_back(b_p_bc_dist);
- if (vbc_is_dist){
- v_bc_meas_v.push_back(b_v_bc_dist);
- }
- else {
- v_bc_meas_v.push_back(b_v_bc);
- }
- if (Lgest_is_dist){
- Lq_meas_v.push_back(b_Lc_gesti_dist);
- }
- else{
- Lq_meas_v.push_back(b_Lc_gesti);
- }
- if (Iw_is_dist){
- Iq_meas_v.push_back(b_Iw_dist);
- }
- else {
- Iq_meas_v.push_back(b_Iw);
- }
- }
-
- /////////////////////////
- // WOLF enters the scene
- // SETUP PROBLEM/SENSORS/PROCESSORS
- std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- ProblemPtr problem = Problem::create("POVCDL", 3);
-
- // CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- //===================================================== INITIALIZATION
- // SENSOR + PROCESSOR INERTIAL KINEMATICS
- ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = std_pbc_est;
- intr_ik->std_vbc = std_vbc_est;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
- SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
- // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
- SensorInertialKinematicsPtr sen_ikin = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
-
- ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
- params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = std_bp_drift;
- ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
- ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
- ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
-
- // SENSOR + PROCESSOR FT PREINT
- ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->std_f = std_f_est;
- intr_ft->std_tau = std_tau_est;
- intr_ft->mass = data.mass[0];
- std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
- SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
- // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
- SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
- ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
- params_sen_ft->sensor_ikin_name = "SenIK";
- params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->nbc = nbc;
- params_sen_ft->dimc = dimc;
- params_sen_ft->time_tolerance = 0.0005;
- params_sen_ft->unmeasured_perturbation_std = 0.000001;
- params_sen_ft->max_time_span = 1000;
- params_sen_ft->max_buff_length = 500;
- params_sen_ft->dist_traveled = 20000.0;
- params_sen_ft->angle_turned = 1000;
- params_sen_ft->voting_active = false;
- ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
- ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
-
- // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
- Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
- SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
- SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
- ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
- // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
- ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
- ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
- //////////////////////////////
-
-
- // SETPRIOR RETRO-ENGINEERING
- // We are not using setPrior because of processors initial captures problems so we have to
- // - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
- TimeStamp t0(t_arr[0]);
- VectorXd x_origin; x_origin.resize(19);
- x_origin << p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0];
- MatrixXd P_origin(9,9); P_origin.setIdentity();
- P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
- FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
- // Prior pose factor
- CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
- pose_prior_capture->emplaceFeatureAndFactor();
- // Prior velocity factor
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
-
- // Special trick to make things work in the IMU + IKin + FTPreint case
- // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
- // 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
- // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
- // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
- proc_inkin->keyFrameCallback(KF1, 0.0005);
- proc_imu->setOrigin(KF1);
- proc_legodom->setOrigin(KF1);
-
- // INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
- // momentum parameters
- Matrix<double, 9, 1> meas_ikin; meas_ikin << p_bc_meas_v[0], v_bc_meas_v[0], w_b_meas_v[0];
- auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, Iq_meas_v[0], Lq_meas_v[0]);
- CIKin0->process();
- proc_ftpreint->setOrigin(KF1);
-
-
- ////////////////////////////////////////////
- // INITIAL BIAS FACTORS
- // Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
- FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
-
- // Add loose absolute factor on initial bp bias since dynamical trajectories
- Matrix3d Q_bp = pow(std_abs_bias_pbc, 2)* Matrix3d::Identity();
- CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
- FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
-
-
- ///////////////////////////////////////////
- // process measurements in sequential order
- // SE3 oTb_prev(Quaterniond(q_ob_gtr_v[0]).toRotationMatrix(), p_ob_gtr_v[0]);
- // FrameBasePtr KF_prev = KF1;
- double ts_since_last_kf = 0;
-
- // Add gaussian noises
- std::time_t epoch = std::time(nullptr);
- int64_t seed = (int64_t)epoch;
- Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau_sim, 2)*Matrix3d::Identity(), false, seed);
-
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
- std::vector<Vector3d> c_ob_fbk_v;
- std::vector<Vector3d> cd_ob_fbk_v;
- std::vector<Vector3d> Lc_ob_fbk_v;
- //////////////////////////////////////////
-
- for (unsigned int i=0; i < t_arr.size(); i++){
- // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
- TimeStamp ts(t_arr[i]); // works best with pyb trajectories
-
- // IMU
- Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
-
- if (noisy_measurements){
- acc_gyr_meas.head<3>() += noise_acc.samples(1);
- acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
- }
-
- Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
-
- // Inertial kinematics
- meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
- // meas_ikin << p_bc_meas_v[ii], v_bc_meas_v[ii], w_b_meas_v[ii];
- // meas_ikin << p_bc_meas_v[ii+1], v_bc_meas_v[ii+1], w_b_meas_v[ii+1];
-
- if (noisy_measurements){
- meas_ikin.segment<3>(0) += noise_pbc.samples(1);
- meas_ikin.segment<3>(3) += noise_vbc.samples(1);
- }
-
- ////////////////////
- // Force Torque
- ////////////////////
-
-
- VectorXd f_meas(nbc*3);
- VectorXd tau_meas(nbc*3);
- VectorXd kin_meas(nbc*7);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- if (dimc == 3){
- f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i];
- }
- else if (dimc == 6){
- f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].head<3>();
- tau_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].tail<3>();
- }
- kin_meas.segment<3>(i_ee*3) = p_bl_meas_v[i_ee][i];
- kin_meas.segment<4>(nbc*3 + i_ee*4) = q_bl_meas_v[i_ee][i];
- }
-
- if (noisy_measurements){
- f_meas.segment<3>(0) += noise_f.samples(1);
- f_meas.segment<3>(3) += noise_f.samples(1);
- tau_meas.segment<3>(0) += noise_tau.samples(1);
- tau_meas.segment<3>(3) += noise_tau.samples(1);
- }
-
- VectorXd cap_ftp_data(dimc*nbc+3+3+nbc*7);
- MatrixXd Qftp(dimc*nbc+3+3,dimc*nbc+3+3); // kin not in covariance mat
- if (dimc == 3){
- cap_ftp_data << f_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
- Qftp.setIdentity();
- Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
- Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
- }
- if (dimc == 6){
- cap_ftp_data << f_meas, tau_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
- Qftp.setIdentity();
- Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
- Qftp.block(nbc*3,nbc*3, nbc*3,nbc*3) *= pow(std_tau_est, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
- Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
- }
- ////////////////////
- /////////////////
-
- CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
- CImu->process();
- auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, Iq_meas_v[i], Lq_meas_v[i]);
- CIKin->process();
- auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
- CFTpreint->process();
-
-
-
- if (i > 0){
- // Leg odometry
- // 1) detect feet in contact
- std::vector<int> feet_in_contact;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
- // basic contact detection based on normal foot vel, things break if no foot is in contact
- if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
- feet_in_contact.push_back(i_ee);
- }
- }
- // 2) fill the leg odometry data matrix, depending on the contact dimension
- Eigen::MatrixXd data_legodom;
- if (dimc == 6){
- // cols organized as:
- // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
- data_legodom.resize(7,feet_in_contact.size()*2);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_in_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
- j++;
- }
- }
- else if (dimc == 3){
- // cols organized as:
- // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
- data_legodom.resize(3,feet_in_contact.size()*2 + 1);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_in_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
- j++;
- }
- data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
- }
- // std::cout << "data_legodom\n" << data_legodom << std::endl;
- // 3) process the data and its cov
- Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
- CLO->process();
- }
-
- ts_since_last_kf += dt;
- if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- // problem->print(4,true,true,true);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
- ts_since_last_kf = 0;
- }
-
-
- // Store current state estimation
- VectorComposite curr_state = problem->getState(ts);
- p_ob_fbk_v.push_back(curr_state['P']);
- q_ob_fbk_v.push_back(curr_state['O']);
- v_ob_fbk_v.push_back(curr_state['V']);
- c_ob_fbk_v.push_back(curr_state['C']);
- cd_ob_fbk_v.push_back(curr_state['D']);
- Lc_ob_fbk_v.push_back(curr_state['L']);
- }
-
-
-
- ///////////////////////////////////////////
- //////////////// SOLVING //////////////////
- ///////////////////////////////////////////
- problem->print(4,true,true,true);
- if (solve_end){
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
- std::cout << report << std::endl;
- }
-
- //////////////////////////////////////
- //////////////////////////////////////
- // STORE DATA //
- //////////////////////////////////////
- //////////////////////////////////////
- std::fstream file_gtr;
- std::fstream file_est;
- std::fstream file_fbk;
- std::fstream file_kfs;
- std::fstream file_cov;
- std::fstream file_wre;
- file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
- file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
- file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
- file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
- file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
- file_wre.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/wre.csv", std::fstream::out);
- std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz\n";
- file_est << header_est;
- file_fbk << header_est;
- std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz,bpx,bpy,bpz\n";
- file_kfs << header_kfs;
- file_gtr << header_kfs; // at the same frequency as "est" but ground truth biases added for comparison with kfs
- std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLcx,QLcy,QLcz,Qbpx,Qbpy,Qbpz\n";
- file_cov << header_cov;
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- // TODO: change if simulate a non zero imu bias
- file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_gtr_v[i](0) << ","
- << p_ob_gtr_v[i](1) << ","
- << p_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](0) << ","
- << q_ob_gtr_v[i](1) << ","
- << q_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](3) << ","
- << v_ob_gtr_v[i](0) << ","
- << v_ob_gtr_v[i](1) << ","
- << v_ob_gtr_v[i](2) << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << c_traj_arr[i](0) << ","
- << c_traj_arr[i](1) << ","
- << c_traj_arr[i](2) << ","
- << dc_traj_arr[i](0) << ","
- << dc_traj_arr[i](1) << ","
- << dc_traj_arr[i](2) << ","
- << L_traj_arr[i](0) << ","
- << L_traj_arr[i](1) << ","
- << L_traj_arr[i](2) << ","
- << bp_traj_arr[i](0) << ","
- << bp_traj_arr[i](1) << ","
- << bp_traj_arr[i](2)
- << std::endl;
- }
-
- VectorComposite state_est;
- for (unsigned int i=0; i < t_arr.size(); i++) {
- state_est = problem->getState(t_arr[i]);
- file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << state_est['P'](0) << ","
- << state_est['P'](1) << ","
- << state_est['P'](2) << ","
- << state_est['O'](0) << ","
- << state_est['O'](1) << ","
- << state_est['O'](2) << ","
- << state_est['O'](3) << ","
- << state_est['V'](0) << ","
- << state_est['V'](1) << ","
- << state_est['V'](2) << ","
- << state_est['C'](0) << ","
- << state_est['C'](1) << ","
- << state_est['C'](2) << ","
- << state_est['D'](0) << ","
- << state_est['D'](1) << ","
- << state_est['D'](2) << ","
- << state_est['L'](0) << ","
- << state_est['L'](1) << ","
- << state_est['L'](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_fbk_v[i](0) << ","
- << p_ob_fbk_v[i](1) << ","
- << p_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](0) << ","
- << q_ob_fbk_v[i](1) << ","
- << q_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](3) << ","
- << v_ob_fbk_v[i](0) << ","
- << v_ob_fbk_v[i](1) << ","
- << v_ob_fbk_v[i](2) << ","
- << c_ob_fbk_v[i](0) << ","
- << c_ob_fbk_v[i](1) << ","
- << c_ob_fbk_v[i](2) << ","
- << cd_ob_fbk_v[i](0) << ","
- << cd_ob_fbk_v[i](1) << ","
- << cd_ob_fbk_v[i](2) << ","
- << Lc_ob_fbk_v[i](0) << ","
- << Lc_ob_fbk_v[i](1) << ","
- << Lc_ob_fbk_v[i](2)
- << "\n";
- }
-
- VectorComposite kf_state;
- CaptureBasePtr cap_ikin;
- VectorComposite bp_bias_est;
- CaptureBasePtr cap_imu;
- VectorComposite bi_bias_est;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
- auto kf = elt.second;
- kf_state = kf->getState();
- cap_ikin = kf->getCaptureOf(sen_ikin);
- bp_bias_est = cap_ikin->getState();
- cap_imu = kf->getCaptureOf(sen_imu);
- bi_bias_est = cap_imu->getState();
-
- file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << kf_state['P'](0) << ","
- << kf_state['P'](1) << ","
- << kf_state['P'](2) << ","
- << kf_state['O'](0) << ","
- << kf_state['O'](1) << ","
- << kf_state['O'](2) << ","
- << kf_state['O'](3) << ","
- << kf_state['V'](0) << ","
- << kf_state['V'](1) << ","
- << kf_state['V'](2) << ","
- << bi_bias_est['I'](0) << ","
- << bi_bias_est['I'](1) << ","
- << bi_bias_est['I'](2) << ","
- << bi_bias_est['I'](3) << ","
- << bi_bias_est['I'](4) << ","
- << bi_bias_est['I'](5) << ","
- << kf_state['C'](0) << ","
- << kf_state['C'](1) << ","
- << kf_state['C'](2) << ","
- << kf_state['D'](0) << ","
- << kf_state['D'](1) << ","
- << kf_state['D'](2) << ","
- << kf_state['L'](0) << ","
- << kf_state['L'](1) << ","
- << kf_state['L'](2) << ","
- << bp_bias_est['I'](0) << ","
- << bp_bias_est['I'](1) << ","
- << bp_bias_est['I'](2)
- << "\n";
-
- // compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
- // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
- problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
- problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
- problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
- problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
- problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
- problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
-
- solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
- // std::cout << "Qbp\n" << Qbp << std::endl;
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
- file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << Qp(0,0) << ","
- << Qp(1,1) << ","
- << Qp(2,2) << ","
- << Qo(0,0) << ","
- << Qo(1,1) << ","
- << Qo(2,2) << ","
- << Qv(0,0) << ","
- << Qv(1,1) << ","
- << Qv(2,2) << ","
- << Qbi(0,0) << ","
- << Qbi(1,1) << ","
- << Qbi(2,2) << ","
- << Qbi(3,3) << ","
- << Qbi(4,4) << ","
- << Qbi(5,5) << ","
- << Qc(0,0) << ","
- << Qc(1,1) << ","
- << Qc(2,2) << ","
- << Qd(0,0) << ","
- << Qd(1,1) << ","
- << Qd(2,2) << ","
- << QL(0,0) << ","
- << QL(1,1) << ","
- << QL(2,2) << ","
- << Qbp(0,0) << ","
- << Qbp(1,1) << ","
- << Qbp(2,2)
- << "\n";
- }
-
- file_gtr.close();
- file_est.close();
- file_kfs.close();
- file_cov.close();
- file_wre.close();
-
-
-}
diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml
deleted file mode 100644
index 28b57f008d929631d65099e27a746a82c049c3da..0000000000000000000000000000000000000000
--- a/demos/mcapi_povcdl_estimation.yaml
+++ /dev/null
@@ -1,76 +0,0 @@
-# trajectory handling
-dt: 0.001
-min_t: -1.0 # -1 means from the beginning of the trajectory
-max_t: -1 # -1 means until the end of the trajectory
-solve_every_sec: -1 # < 0 strict --> no solve
-solve_end: False
-# solve_every_sec: 0.3 # < 0 strict --> no solve
-# solve_end: True
-
-# estimator sensor noises
-std_acc_est: 0.001
-std_gyr_est: 0.001
-std_pbc_est: 0.001
-std_vbc_est: 0.001 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
-std_f_est: 0.001
-std_tau_est: 0.001
-# std_odom3d_est: 0.000001
-std_odom3d_est: 100000
-
-# simulated sensor noises
-std_acc_sim: 0.0001
-std_gyr_sim: 0.0001
-std_pbc_sim: 0.0001
-std_vbc_sim: 0.0001
-std_f_sim: 0.0001
-std_tau_sim: 0.0001
-std_odom3d_sim: 100000000
-
-# some controls
-fz_thresh: 1
-noisy_measurements: False
-
-# priors
-std_prior_p: 0.01
-std_prior_o: 0.1
-std_prior_v: 0.1
-
-# std_abs_biasimu: 100000
-std_abs_biasimu: 0.0000001 # almost fixed
-std_abs_bias_pbc: 10000 # noprior
-# std_abs_bias_pbc: 0.00001 # almost fixed
-std_bp_drift: 10000 # All the drift you want
-# std_bp_drift: 0.1 # no drift
-
-bias_pbc_prior: [0,0,0]
-
-
-# model disturbance
-scale_dist: 0.00 # disturbance of link inertia
-mass_dist: False
-base_dist_only: False
-
-# which measurement/parameter is disturbed?
-vbc_is_dist: False
-Iw_is_dist: False
-Lgest_is_dist: False
-
-
-# Robot model
-# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
-# dimc: 6
-robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
-dimc: 3
-
-
-#Â Trajectory files
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
-contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
-
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
-
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml
index f92b74c06167c96ef4294e00600c7270bd0d20e2..14964b20685f70720ed8d21c19ba8cdf66406aac 100644
--- a/demos/processor_imu_solo12.yaml
+++ b/demos/processor_imu_solo12.yaml
@@ -2,7 +2,7 @@ keyframe_vote:
angle_turned: 1000
dist_traveled: 20000.0
max_buff_length: 100000000000
- max_time_span: 0.19999
+ max_time_span: 0.05
voting_active: true
name: Main imu
time_tolerance: 0.0005
diff --git a/demos/sensor_imu_solo12.yaml b/demos/sensor_imu_solo12.yaml
index 68ad34e3b0d14ec0711760551d8875ab005622cc..baf999bfa6179df1967dceb5f95a17da8134d3f3 100644
--- a/demos/sensor_imu_solo12.yaml
+++ b/demos/sensor_imu_solo12.yaml
@@ -1,9 +1,9 @@
-type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "Main Imu Sensor" # This is ignored. The name provided to the SensorFactory prevails
-motion_variances:
- a_noise: 0.05 # standard deviation of Acceleration noise (same for all the axis) in m/s2
- w_noise: 0.005 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
- ab_initial_stdev: 0.00 # m/s2 - initial bias
- wb_initial_stdev: 0.0 # rad/sec - initial bias
- ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
- wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
\ No newline at end of file
+motion_variances:
+ a_noise: 0.02
+ ab_initial_stdev: 0.0
+ ab_rate_stdev: 1.0e-06
+ w_noise: 0.03
+ wb_initial_stdev: 0.0
+ wb_rate_stdev: 1.0e-06
+name: Main Imu Sensor
+type: SensorImu
diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9f52035066348852f9a296504e414520669656e
--- /dev/null
+++ b/demos/solo_imu_kine.cpp
@@ -0,0 +1,742 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <iostream>
+#include <fstream>
+
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include <cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_pose_3d_with_extrinsics.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_leg_odom.h"
+#include "bodydynamics/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include "bodydynamics/factor/factor_force_torque_preint.h"
+
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+#include "bodydynamics/factor/factor_point_feet_nomove.h"
+
+
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+void printFactorCosts(FrameBasePtr kf_last){
+ int nb_max_fact_ptf = 4;
+ int nb = 0;
+ for (auto f: kf_last->getFactorList()){
+ auto fimu = std::dynamic_pointer_cast<FactorImu>(f);
+ if (fimu){
+ std::cout << std::setprecision(12) << " C_IMU " << sqrt(fimu->cost()) << std::endl;
+ }
+ else{
+ auto fptn = std::dynamic_pointer_cast<FactorPointFeetNomove>(f);
+ if (fptn and (nb < nb_max_fact_ptf)){
+ Vector3d error = fptn->error();
+ double cost = fptn->cost();
+ std::cout << std::setprecision(12) << " E_PF" << nb << " " << error.transpose() << std::endl;
+ std::cout << std::setprecision(12) << " C_PF" << nb << " " << sqrt(cost) << std::endl;
+ nb++;
+ }
+ }
+ }
+}
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ double dt = config["dt"].as<double>();
+ // double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+
+ // Ceres setup
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+ double func_tol = config["func_tol"].as<double>();
+ bool compute_cov = config["compute_cov"].as<bool>();
+
+ // robot
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
+
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
+
+
+ // kinematics
+ double std_foot_nomove = config["std_foot_nomove"].as<double>();
+ double std_altitude = config["std_altitude"].as<double>();
+ double foot_radius = config["foot_radius"].as<double>();
+ std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
+ std::vector<double> alpha_qa_vec = config["alpha_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double,12,1>> alpha_qa(alpha_qa_vec.data());
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
+ double fz_thresh = config["fz_thresh"].as<double>();
+
+
+ // MOCAP
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+ // MOCAP to IMU transform
+ Quaterniond i_q_m(i_qvec_m);
+ i_q_m.normalize();
+ assert(i_q_m.normalized().isApprox(i_q_m));
+ SE3 i_T_m(i_q_m, i_p_im);
+ Matrix3d i_R_m = i_T_m.rotation();
+
+ // IMU to MOCAP transform
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+
+ // MOCAP to BASE transform
+ Quaterniond m_q_b(m_qvec_b);
+ m_q_b.normalize();
+ assert(m_q_b.normalized().isApprox(m_q_b));
+ SE3 m_T_b(m_q_b, m_p_mb);
+ Matrix3d m_R_b = m_T_b.rotation();
+
+ // IMU to BASE transform (composition)
+ SE3 i_T_b = i_T_m*m_T_b;
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+
+
+ // initialize some vectors and fill them with dicretized data
+ cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+ //load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+ // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+ // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+ cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+ cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+ // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray contact_npyarr = arr_map["contacts"];
+ // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
+ // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+ double* t_arr = t_npyarr.data<double>();
+ double* imu_acc_arr = imu_acc_npyarr.data<double>();
+ // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+ double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
+ double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+ double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+ double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+ double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
+ double* contact_arr = contact_npyarr.data<double>();
+ // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
+ // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+ unsigned int N = t_npyarr.shape[0];
+ if (max_t > 0){
+ N = N < int(max_t/dt) ? N : int(max_t/dt);
+ }
+
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("POV", 3);
+
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ problem->setTreeManager(tree_manager);
+
+ //////////////////////
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+ solver->getSolverOptions().function_tolerance = func_tol; // 1e-6
+ solver->getSolverOptions().gradient_tolerance = 1e-4*solver->getSolverOptions().function_tolerance;
+
+ // solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION;
+ // solver->getSolverOptions().trust_region_strategy_type = ceres::DOGLEG;
+ // solver->getSolverOptions().trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
+
+ // solver->getSolverOptions().minimizer_type = ceres::LINE_SEARCH;
+ // solver->getSolverOptions().line_search_direction_type = ceres::LBFGS;
+
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ intr_pfn->std_altitude_ = std_altitude;
+ intr_pfn->foot_radius_ = foot_radius;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ params_pfnm->voting_active = false;
+ params_pfnm->time_tolerance = 0.0;
+ params_pfnm->max_time_vote = 0.0;
+ ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+
+ // SETPRIOR RETRO-ENGINEERING
+ // We are not using setPrior because of processors initial captures problems so we have to
+ // - Manually create the first KF and its pose and velocity priors
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
+ w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
+ VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
+
+ proc_imu->setOrigin(KF1);
+
+ //////////////////////////////////////////
+ // INITIAL BIAS FACTORS
+ // Add absolute factor on Imu biases to simulate a fix()
+ Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
+ sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+
+
+ // Allocate on the heap to prevent stack overflow for large datasets
+ double* o_p_ob_fbk_carr = new double[3*N];
+ double* o_q_b_fbk_carr = new double[4*N];
+ double* o_v_ob_fbk_carr = new double[3*N];
+ double* o_p_oi_fbk_carr = new double[3*N];
+ double* o_q_i_fbk_carr = new double[4*N];
+ double* o_v_oi_fbk_carr = new double[3*N];
+ double* imu_bias_fbk_carr = new double[6*N];
+ double* i_pose_im_fbk_carr = new double[7*N];
+
+ // covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+
+
+ std::vector<Vector3d> i_omg_oi_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = 1;
+ for (unsigned int i=0; i < N; i++){
+ TimeStamp ts(t_arr[i]);
+
+ ////////////////////////////////////
+ // Retrieve current state
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ Vector3d o_v_oi_curr = curr_state['V'];
+
+
+ //////////////
+ // PROCESS IMU
+ //////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i);
+ // store i_omg_oi for later computation of o_v_ob from o_v_oi
+ i_omg_oi_v.push_back(i_omg_oi);
+
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+ Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, sen_imu->getNoiseCov());
+ CImu->process();
+ //////////////
+ //////////////
+
+ //////////////////////////////////
+ // Kinematics + forces
+ // retrieve traj data
+ Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+ Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+ // std::cout << contact_gtr.transpose() << std::endl;
+ // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
+
+ // Kinematics model
+ qa = qa + delta_qa + alpha_qa.cwiseProduct(tau);
+
+ Matrix3d o_R_i = Quaterniond(o_q_i_curr).toRotationMatrix(); // IMU internal filter
+
+ // Or else, retrieve from preprocessed dataset
+ Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+ Vector3d o_f_tot = Vector3d::Zero();
+ std::vector<Vector3d> l_f_l_vec;
+ std::vector<Vector3d> o_f_l_vec;
+ std::vector<Vector3d> i_p_il_vec;
+ std::vector<Vector4d> i_qvec_l_vec;
+ // needing relative kinematics measurements
+ VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
+ VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ forwardKinematics(model, data, q_static, dq_static);
+ updateFramePlacements(model, data);
+
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation();
+ Matrix3d i_R_l = i_T_l.rotation();
+ Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();
+ i_p_il = i_p_il + i_R_l*l_p_lg;
+
+ Vector3d l_f_l = l_forces.segment(3*i_ee, 3);
+ Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+
+ l_f_l_vec.push_back(l_f_l);
+ o_f_l_vec.push_back(o_f_l);
+ i_p_il_vec.push_back(i_p_il);
+ i_qvec_l_vec.push_back(i_qvec_l);
+ }
+
+ // Detect feet in contact
+ int nb_feet_in_contact = 0;
+ std::unordered_map<int, Vector7d> kin_incontact;
+ // std::cout << "" << std::endl;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+ // if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
+ if (contact_gtr[i_ee] > 0.5){
+ nb_feet_in_contact++;
+ Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ kin_incontact.insert({i_ee, i_pose_il});
+ }
+ }
+
+ CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ CPF->process();
+
+ // solve every new KF
+ if (problem->getTrajectory()->size() > nb_kf ){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ // ////////////////////////////////
+ // // FOR PLOTS
+ // if (solver->iterations() == 1){
+ // std::cout << "ONLY ONE!" << std::endl;
+ // printFactorCosts(kf_last);
+ // for (int k=0; k < 5; k++){
+ // std::cout << "P E R T U B!" << std::endl;
+ // // problem->perturb(0.000001);
+ // kf_last->perturb();
+ // // std::cout << "kf_last P " << kf_last->getP()->getState().transpose() << std::endl;
+ // report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ // printFactorCosts(kf_last);
+ // }
+
+ // }
+ // ///////////////////////
+
+
+ ////////////////////////////////
+ // FOR REAL
+ if (solver->iterations() == 1){
+ // kf_last->perturb(0.000001);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ }
+ ///////////////////////
+
+ std::cout << ts << " ";
+ std::cout << report << std::endl;
+ //////////////////////////////
+
+ // printFactorCosts(kf_last);
+
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
+
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
+
+ nb_kf = problem->getTrajectory()->getFrameMap().size();
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ Vector3d o_v_oi = state_fbk['V'];
+
+ // IMU to base frame
+ o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ // Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
+
+ imu_bias = sen_imu->getIntrinsic()->getState();
+ Vector7d i_pose_im_fbk; i_pose_im_fbk << i_p_im, i_qvec_m;
+
+ mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
+ mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ // if (solve_end){
+ // std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ // problem->print(3,true,false,false);
+ // std::cout << report << std::endl;
+ // }
+
+ double* o_p_ob_carr = new double[3*N];
+ double* o_q_b_carr = new double[4*N];
+ double* o_v_ob_carr = new double[3*N];
+ double* o_p_oi_carr = new double[3*N];
+ double* o_q_i_carr = new double[4*N];
+ double* o_v_oi_carr = new double[3*N];
+ double* imu_bias_carr = new double[6*N];
+
+ for (unsigned int i=0; i < N; i++) {
+ VectorComposite state_est = problem->getState(t_arr[i]);
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
+
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
+ Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ // Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
+
+ mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
+ mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ }
+
+
+ // Compute Covariances
+ unsigned int Nkf = problem->getTrajectory()->size();
+ double* tkf_carr = new double[1*Nkf];
+ double* Qp_carr = new double[3*Nkf];
+ double* Qo_carr = new double[3*Nkf];
+ double* Qv_carr = new double[3*Nkf];
+ double* Qbi_carr = new double[6*Nkf];
+ double* Qm_carr = new double[6*Nkf];
+ // feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
+ double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_carr = new double[6*Nkf];
+
+ // factor errors
+ double* fac_imu_err_carr = new double[9*Nkf];
+ double* fac_pose_err_carr = new double[6*Nkf];
+ int i = 0;
+ for (auto elt: problem->getTrajectory()->getFrameMap())
+ {
+ std::cout << "Traj " << i << "/" << problem->getTrajectory()->size() << std::endl;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
+ VectorComposite kf_state = kf->getState();
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+ // diagonal components
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
+ memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
+ memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
+ memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
+ memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+
+ // Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
+
+
+ // Factor errors
+ // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
+ FactorBasePtrList fac_lst;
+ kf->getFactorList(fac_lst);
+ Vector9d imu_err = Vector9d::Zero();
+ Vector6d pose_err = Vector6d::Zero();
+
+ i++;
+ }
+
+
+ // Save trajectories in npz file
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+
+ // Online estimates
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
+
+ // offline states
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
+
+ // and biases/extrinsics
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
+
+ // covariances
+ cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
+
+}
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_imu_kine_mocap.cpp
similarity index 61%
rename from demos/solo_real_pov_estimation.cpp
rename to demos/solo_imu_kine_mocap.cpp
index fbae4d51a0f97f30aa05c5329ca754986ce72cb2..82d2df1c42da66a66e6894fe49e037b4a04259cd 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -81,9 +81,6 @@
-// UTILS
-#include "mcapi_utils.h"
-
using namespace Eigen;
using namespace wolf;
@@ -96,42 +93,53 @@ typedef pinocchio::ForceTpl<double> Force;
int main (int argc, char **argv) {
// retrieve parameters from yaml file
YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- int nb_feet = config["nb_feet"].as<int>();
double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
+ // double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
+ // double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>();
+ // robot
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ // int dimc = config["dimc"].as<int>();
+ // int nb_feet = config["nb_feet"].as<int>();
+
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
+ bool compute_cov = config["compute_cov"].as<bool>();
// estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
- double std_odom3d_est = config["std_odom3d_est"].as<double>();
+ // double std_pbc_est = config["std_pbc_est"].as<double>();
+ // double std_vbc_est = config["std_vbc_est"].as<double>();
+ // double std_f_est = config["std_f_est"].as<double>();
+ // double std_tau_est = config["std_tau_est"].as<double>();
+ double std_foot_nomove = config["std_foot_nomove"].as<double>();
// priors
double std_prior_p = config["std_prior_p"].as<double>();
double std_prior_o = config["std_prior_o"].as<double>();
double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ // double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
+ // double std_bp_drift = config["std_bp_drift"].as<double>();
std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
- std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
+
+ // contacts
std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
- Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
- Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
- double fz_thresh = config["fz_thresh"].as<double>();
+ // double fz_thresh = config["fz_thresh"].as<double>();
+ std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
// MOCAP
double std_pose_p = config["std_pose_p"].as<double>();
@@ -146,17 +154,31 @@ int main (int argc, char **argv) {
std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
- std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
- // Base to IMU transform
- Quaterniond b_q_i(b_qvec_i);
- assert(b_q_i.normalized().isApprox(b_q_i));
- Quaterniond i_q_b = b_q_i.conjugate();
- SE3 b_T_i(b_q_i, b_p_bi);
- SE3 i_T_b = b_T_i.inverse();
- Matrix3d b_R_i = b_T_i.rotation();
+ // MOCAP to IMU transform
+ Quaterniond i_q_m(i_qvec_m);
+ i_q_m.normalize();
+ assert(i_q_m.normalized().isApprox(i_q_m));
+ SE3 i_T_m(i_q_m, i_p_im);
+ // Matrix3d i_R_m = i_T_m.rotation();
+
+ // IMU to MOCAP transform
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+
+ // MOCAP to BASE transform
+ Quaterniond m_q_b(m_qvec_b);
+ m_q_b.normalize();
+ assert(m_q_b.normalized().isApprox(m_q_b));
+ SE3 m_T_b(m_q_b, m_p_mb);
+ // Matrix3d m_R_b = m_T_b.rotation();
+
+ // IMU to BASE transform (composition)
+ SE3 i_T_b = i_T_m*m_T_b;
Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
@@ -177,7 +199,7 @@ int main (int argc, char **argv) {
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
- // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+ cnpy::NpyArray contact_npyarr = arr_map["contacts"];
// cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
@@ -197,7 +219,7 @@ int main (int argc, char **argv) {
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
double* w_q_m_arr = w_q_m_npyarr.data<double>();
- // double* contact_arr = contact_npyarr.data<double>();
+ double* contact_arr = contact_npyarr.data<double>();
// double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
@@ -206,12 +228,13 @@ int main (int argc, char **argv) {
N = N < int(max_t/dt) ? N : int(max_t/dt);
}
- // Creating measurements from simulated trajectory
// Load the urdf model
Model model;
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
std::cout << "model name: " << model.name << std::endl;
Data data(model);
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
// Recover end effector frame ids
std::vector<int> ee_ids;
@@ -235,22 +258,6 @@ int main (int argc, char **argv) {
problem->setTreeManager(tree_manager);
//////////////////////
- // COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
- Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
- w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
-
- // initial state
- VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
- VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
-
- //////////////////////
-
// CERES WRAPPER
SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
@@ -262,21 +269,25 @@ int main (int argc, char **argv) {
ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
- // // SENSOR + PROCESSOR POINT FEET NOMOVE
- // ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- // intr_pfn->std_foot_nomove_ = std_odom3d_est;
- // VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
- // SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- // SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
- // ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- // ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ params_pfnm->voting_active = false;
+ params_pfnm->time_tolerance = 0.0;
+ params_pfnm->max_time_vote = 0.0;
+ ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = std_pose_p;
intr_sp->std_o = toRad(std_pose_o_deg);
- Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+ Vector7d extr_pose; extr_pose << i_p_im, i_q_m.coeffs();
auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = time_tolerance_mocap;
@@ -293,16 +304,23 @@ int main (int argc, char **argv) {
sensor_pose->fixExtrinsics();
}
-
-
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+ // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
- VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
+ w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0); // needed to anchor the problem
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
@@ -317,29 +335,25 @@ int main (int argc, char **argv) {
FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
- // sen_imu->fixIntrinsics();
-
-
- ///////////////////////////////////////////
- // process measurements in sequential order
- double ts_since_last_kf = 0;
- std::vector<Vector3d> i_p_il_vec_prev;
- std::vector<Vector4d> i_qvec_l_vec_prev;
- std::vector<bool> feet_in_contact_prev;
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
// Allocate on the heap to prevent stack overflow for large datasets
double* o_p_ob_fbk_carr = new double[3*N];
double* o_q_b_fbk_carr = new double[4*N];
double* o_v_ob_fbk_carr = new double[3*N];
+ double* o_p_oi_fbk_carr = new double[3*N];
+ double* o_q_i_fbk_carr = new double[4*N];
+ double* o_v_oi_fbk_carr = new double[3*N];
double* imu_bias_fbk_carr = new double[6*N];
- double* extr_mocap_fbk_carr = new double[7*N];
+ double* i_pose_im_fbk_carr = new double[7*N];
+
+ // covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+
std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
@@ -351,154 +365,154 @@ int main (int argc, char **argv) {
////////////////////////////////////
// Retrieve current state
VectorComposite curr_state = problem->getState();
- Vector4d o_qvec_i_curr = curr_state['O'];
- Quaterniond o_q_i_curr(o_qvec_i_curr);
- Vector3d o_v_oi_curr = curr_state['V'];
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ // Vector3d o_v_oi_curr = curr_state['V'];
- ////////////////////////////////////
- ////////////////////////////////////
+
+ //////////////
+ // PROCESS IMU
+ //////////////
// Get measurements
// retrieve traj data
Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
- Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
- // Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
- // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
- w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
-
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i);
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
-
- ////////////////////////////////////
- // IMU
Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
- Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+ CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, sen_imu->getNoiseCov());
+ CImu->process();
+ //////////////
+ //////////////
- ////////////////////////////////////
- // Kinematics + forces
- // SE3 o_T_i(o_q_i_curr, Vector3d::Zero());
- // Matrix3d o_R_i = o_T_i.rotation();
- // Matrix3d i_R_o = o_R_i.transpose();
- // Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
- // Vector3d i_acc = imu_acc + i_R_o * gravity();
- // Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr);
-
- // VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
- // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa;
- // VectorXd ddq(model.nv); ddq.setZero();
- // ddq.head<3>() = i_asp_i;
- // // TODO: add other terms to compute forces (ddqa...)
-
- // // update and retrieve kinematics
- // forwardKinematics(model, data, q);
- // updateFramePlacements(model, data);
-
- // // compute all linear jacobians in feet frames (only for quadruped)
- // MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
- // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- // MatrixXd Jee(6, model.nv); Jee.setZero();
- // computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- // Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
- // }
-
- // // estimate forces from torques
- // VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- // tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
-
- // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
- // Solve in Least square sense (3 ways):
- // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
- // VectorXd l_forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
- // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
- // Or else, retrieve from preprocessed dataset
- // Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
-
- // Vector3d o_f_tot = Vector3d::Zero();
- // std::vector<Vector3d> l_f_l_vec;
- // std::vector<Vector3d> o_f_l_vec;
- // std::vector<Vector3d> i_p_il_vec;
- // std::vector<Vector4d> i_qvec_l_vec;
- // // needing relative kinematics measurements
- // VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
- // VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
- // forwardKinematics(model, data, q_static, dq_static);
- // updateFramePlacements(model, data);
- // // std::cout << "" << std::endl;
- // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- // // std::cout << i_ee << std::endl;
- // auto b_T_l = data.oMf[ee_ids[i_ee]];
-
- // // overides with value from mocap
- // // std::cout << b_T_l.translation().transpose() << std::endl;
- // b_T_l.translation(b_p_bl_mocap.segment<3>(3*i_ee));
- // // std::cout << b_T_l.translation().transpose() << std::endl;
-
- // auto i_T_l = i_T_b * b_T_l;
- // Vector3d i_p_il = i_T_l.translation(); // meas
- // Matrix3d i_R_l = i_T_l.rotation();
- // Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
- // i_p_il = i_p_il + i_R_l*l_p_lg;
-
- // Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
- // // Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
-
- // l_f_l_vec.push_back(l_f_l);
- // // o_f_l_vec.push_back(o_f_l);
- // i_p_il_vec.push_back(i_p_il);
- // i_qvec_l_vec.push_back(i_qvec_l);
- // }
-
- CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
- CImu->process();
+ ////////////////
+ // PROCESS MOCAP
+ ////////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
- // 1) detect feet in contact
- // int nb_feet_in_contact = 0;
- // std::unordered_map<int, Vector3d> kin_incontact;
- // for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
- // // basic contact detection based on normal foot vel, things break if no foot is in contact
- // // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
- // // nb_feet: just for debugging/testing kinematic factor
-
- // // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
- // if (contact_gtr(i_ee)){
- // nb_feet_in_contact++;
- // kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
- // }
- // }
-
- // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
- // CPF->process();
-
- // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
+ ////////////////
+ ////////////////
+
+ //////////////////////////////////
+ // Kinematics + forces
+ // retrieve traj data
+ Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+ Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+ // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
+
+ qa = qa + delta_qa;
- // ts_since_last_kf += dt;
- // if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- // // problem->print(4,true,true,true);
- // auto kf = problem->emplaceFrame(ts);
- // problem->keyFrameCallback(kf, nullptr, 0.0005);
- // std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- // std::cout << report << std::endl;
- // ts_since_last_kf = 0;
- // // std::cout << "Extr SPose: " << sensor_pose->getP()->getState().transpose() << " " << sensor_pose->getO()->getState().transpose() << std::endl;
- // }
+ Matrix3d o_R_i = Quaterniond(o_q_i_curr).toRotationMatrix(); // IMU internal filter
+
+ // Or else, retrieve from preprocessed dataset
+ Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+ std::vector<Vector3d> l_f_l_vec;
+ std::vector<Vector3d> o_f_l_vec;
+ std::vector<Vector3d> i_p_il_vec;
+ std::vector<Vector4d> i_qvec_l_vec;
+ // needing relative kinematics measurements
+ VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
+ VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ forwardKinematics(model, data, q_static, dq_static);
+ updateFramePlacements(model, data);
+
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation();
+ Matrix3d i_R_l = i_T_l.rotation();
+ Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();
+ i_p_il = i_p_il + i_R_l*l_p_lg;
+
+ Vector3d l_f_l = l_forces.segment(3*i_ee, 3);
+ Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+
+ l_f_l_vec.push_back(l_f_l);
+ o_f_l_vec.push_back(o_f_l);
+ i_p_il_vec.push_back(i_p_il);
+ i_qvec_l_vec.push_back(i_qvec_l);
+ }
+
+ // Detect feet in contact
+ int nb_feet_in_contact = 0;
+ std::unordered_map<int, Vector7d> kin_incontact;
+ // std::cout << "" << std::endl;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+ // if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
+ if (contact_gtr[i_ee] > 0.5){
+ nb_feet_in_contact++;
+ Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ kin_incontact.insert({i_ee, i_pose_il});
+ }
+ }
+
+ CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ CPF->process();
// solve every new KF
if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
+
+ auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
+
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ if (compute_cov)
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
+
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
+
nb_kf = problem->getTrajectory()->getFrameMap().size();
}
@@ -509,27 +523,23 @@ int main (int argc, char **argv) {
Vector3d o_v_oi = state_fbk['V'];
// IMU to base frame
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi);
- // Vector3d o_p_ob = o_p_oi;
- // Vector3d o_v_ob = o_v_oi;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
imu_bias = sen_imu->getIntrinsic()->getState();
- Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+ Vector7d i_pose_im_fbk; i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
-
- p_ob_fbk_v.push_back(o_p_ob);
- q_ob_fbk_v.push_back(o_qvec_b);
- v_ob_fbk_v.push_back(o_v_ob);
+ mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
}
@@ -545,8 +555,12 @@ int main (int argc, char **argv) {
double* o_p_ob_carr = new double[3*N];
double* o_q_b_carr = new double[4*N];
+ double* o_v_ob_carr = new double[3*N];
+ double* o_p_oi_carr = new double[3*N];
+ double* o_q_i_carr = new double[4*N];
+ double* o_v_oi_carr = new double[3*N];
double* imu_bias_carr = new double[6*N];
- double* o_v_ob_carr = new double[6*N];
+
for (unsigned int i=0; i < N; i++) {
VectorComposite state_est = problem->getState(t_arr[i]);
Vector3d i_omg_oi = i_omg_oi_v[i];
@@ -556,20 +570,20 @@ int main (int argc, char **argv) {
auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
Vector6d imu_bias = sb->getState();
- // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
- // Vector3d o_p_ob = o_p_oi;
- // Vector3d o_v_ob = o_v_oi;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
}
@@ -582,6 +596,13 @@ int main (int argc, char **argv) {
double* Qv_carr = new double[3*Nkf];
double* Qbi_carr = new double[6*Nkf];
double* Qm_carr = new double[6*Nkf];
+ // feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
+ double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_carr = new double[6*Nkf];
+
// factor errors
double* fac_imu_err_carr = new double[9*Nkf];
double* fac_pose_err_carr = new double[6*Nkf];
@@ -600,19 +621,15 @@ int main (int argc, char **argv) {
Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
-
- std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
- problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
- // std::cout << "Qbp\n" << Qbp << std::endl;
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// diagonal components
@@ -628,43 +645,23 @@ int main (int argc, char **argv) {
memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+ // Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
+
+
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
FactorBasePtrList fac_lst;
kf->getFactorList(fac_lst);
- Vector9d imu_err = Vector9d::Zero();
- Vector6d pose_err = Vector6d::Zero();
- for (auto fac: fac_lst){
- if (fac->getProcessor() == proc_imu){
- auto f = std::dynamic_pointer_cast<FactorImu>(fac);
- // Matrix6d R_bias_drift = f->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
- // Matrix6d R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
- // std::cout << R_bias_drift << std::endl;
- // MatrixXd R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper();
- // MatrixXd Cov_bias_drift = f->getFeature()->getMeasurementCovariance();
- // std::cout << "R_bias_drift" << std::endl;
- // std::cout << R_bias_drift << std::endl;
- // std::cout << "Cov_bias_drift" << std::endl;
- // std::cout << Cov_bias_drift << std::endl;
-
- if (f){
- imu_err = f->error();
- }
- }
- if (fac->getProcessor() == proc_pose){
- auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
- if (f){
- pose_err = f->error();
- }
- }
- }
- memcpy(fac_imu_err_carr + 9*i, imu_err.data(), 9*sizeof(double));
- memcpy(fac_pose_err_carr + 6*i, pose_err.data(), 6*sizeof(double));
-
i++;
}
+
// Save trajectories in npz file
// save ground truth
cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
@@ -672,22 +669,35 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
- // save estimates
+ // Online estimates
cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
+ // offline states
cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
+
+ // imu states
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
// and biases/extrinsics
cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
@@ -696,6 +706,11 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_imu_mocap.cpp
similarity index 69%
rename from demos/solo_mocap_imu.cpp
rename to demos/solo_imu_mocap.cpp
index 19f8dfaa700dbcb7876dfd2f3587bb996d68ab1b..1ffabd4c847bb9f3f0954f5eb70862cc20eedf00 100644
--- a/demos/solo_mocap_imu.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -67,9 +67,6 @@
#include "bodydynamics/internal/config.h"
-// UTILS
-#include "mcapi_utils.h"
-
using namespace Eigen;
using namespace wolf;
@@ -88,6 +85,7 @@ int main (int argc, char **argv) {
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
+ bool compute_cov = config["compute_cov"].as<bool>();
// priors
double std_prior_p = config["std_prior_p"].as<double>();
@@ -97,10 +95,14 @@ int main (int argc, char **argv) {
double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
- std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
- Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
- Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
// MOCAP
double std_pose_p = config["std_pose_p"].as<double>();
@@ -115,21 +117,38 @@ int main (int argc, char **argv) {
std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
- // Base to IMU transform
- Quaterniond b_q_i(b_qvec_i);
- assert(b_q_i.normalized().isApprox(b_q_i));
- Quaterniond i_q_b = b_q_i.conjugate();
- SE3 b_T_i(b_q_i, b_p_bi);
- SE3 i_T_b = b_T_i.inverse();
- Matrix3d b_R_i = b_T_i.rotation();
+ // MOCAP to IMU transform
+ Quaterniond i_q_m(i_qvec_m);
+ i_q_m.normalize();
+ assert(i_q_m.normalized().isApprox(i_q_m));
+ SE3 i_T_m(i_q_m, i_p_im);
+ Matrix3d i_R_m = i_T_m.rotation();
+
+ // IMU to MOCAP transform
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+
+ // MOCAP to BASE transform
+ Quaterniond m_q_b(m_qvec_b);
+ m_q_b.normalize();
+ assert(m_q_b.normalized().isApprox(m_q_b));
+ SE3 m_T_b(m_q_b, m_p_mb);
+ // Matrix3d m_R_b = m_T_b.rotation();
+
+ // IMU to BASE transform (composition)
+ SE3 i_T_b = i_T_m*m_T_b;
Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
//load it into a new array
cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
@@ -142,20 +161,39 @@ int main (int argc, char **argv) {
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
- cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
- // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+
+ // cnpy::NpyArray contact_npyarr = arr_map["contacts"];
// cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+ // std::cout << data_file_path << std::endl;
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+ // std::cout << w_p_wm_npyarr.shape[0] << ", " << w_p_wm_npyarr.shape[1] << std::endl;
+ // std::cout << w_p_wm_npyarr.word_size << std::endl;
+ // std::cout << w_p_wm_npyarr.fortran_order << std::endl;
+ // std::cout << w_p_wm_npyarr.num_vals << std::endl;
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+ // std::cout << w_q_m_npyarr.shape[0] << ", " << w_q_m_npyarr.shape[1] << std::endl;
+ // std::cout << w_q_m_npyarr.word_size << std::endl;
+ // std::cout << w_q_m_npyarr.fortran_order << std::endl;
+ // std::cout << w_q_m_npyarr.num_vals << std::endl;
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+ // std::cout << i_omg_oi_npyarr.shape[0] << ", " << i_omg_oi_npyarr.shape[1] << std::endl;
+ // std::cout << i_omg_oi_npyarr.word_size << std::endl;
+ // std::cout << i_omg_oi_npyarr.fortran_order << std::endl;
+ // std::cout << i_omg_oi_npyarr.num_vals << std::endl;
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+ // std::cout << "\n\n\n\n\n\n" << std::endl;
+
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
- double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* dqa_arr = dqa_npyarr.data<double>();
+ // double* tau_arr = tau_npyarr.data<double>();
+ // double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
@@ -170,7 +208,8 @@ int main (int argc, char **argv) {
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
if (max_t > 0){
- N = N < int(max_t/dt) ? N : int(max_t/dt);
+ unsigned int N_max = (max_t/dt);
+ N = N < N_max ? N : N_max;
}
/////////////////////////
@@ -189,11 +228,9 @@ int main (int argc, char **argv) {
//////////////////////
// COMPUTE INITIAL STATE
TimeStamp t0(t_arr[0]);
- Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
- w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+ Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
+ w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
//////////////////////
@@ -213,7 +250,7 @@ int main (int argc, char **argv) {
auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = std_pose_p;
intr_sp->std_o = toRad(std_pose_o_deg);
- Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+ Vector7d extr_pose; extr_pose << i_p_im, i_q_m.coeffs();
auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = time_tolerance_mocap;
@@ -230,12 +267,13 @@ int main (int argc, char **argv) {
sensor_pose->fixExtrinsics();
}
- Matrix3d w_R_m_init = q2R(w_qvec_wm);
- Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
- VectorComposite x_origin("POV", {w_p_wi_init, w_qvec_wm, Vector3d::Zero()});
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
@@ -250,24 +288,25 @@ int main (int argc, char **argv) {
FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
- // sen_imu->fixIntrinsics();
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
// Allocate on the heap to prevent stack overflow for large datasets
double* o_p_ob_fbk_carr = new double[3*N];
double* o_q_b_fbk_carr = new double[4*N];
double* o_v_ob_fbk_carr = new double[3*N];
+ double* o_p_oi_fbk_carr = new double[3*N];
+ double* o_q_i_fbk_carr = new double[4*N];
+ double* o_v_oi_fbk_carr = new double[3*N];
double* imu_bias_fbk_carr = new double[6*N];
- double* extr_mocap_fbk_carr = new double[7*N];
+ double* i_pose_im_fbk_carr = new double[7*N];
+
// covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
//
std::vector<Vector3d> i_omg_oi_v;
@@ -277,15 +316,14 @@ int main (int argc, char **argv) {
for (unsigned int i=0; i < N; i++){
TimeStamp ts(t_arr[i]);
- ////////////////////////////////////
- ////////////////////////////////////
+
+ //////////////
+ // PROCESS IMU
+ //////////////
// Get measurements
// retrieve traj data
Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
- w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
@@ -293,44 +331,80 @@ int main (int argc, char **argv) {
Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
-
- ////////////////////////////////////
- // IMU
Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
CImu->process();
-
- // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ //////////////
+ //////////////
+
+
+ ////////////////
+ // PROCESS MOCAP
+ ////////////////
+ // retrieve traj data
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
+ ////////////////
+ ////////////////
+
+
// solve every new KF
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ if (problem->getTrajectory()->size() > nb_kf )
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
// recover covariances at this point
- auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+ auto kf_last = problem->getTrajectory()->getLastFrame();
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
-
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
- Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ if (compute_cov)
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
- // std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
- // std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
- nb_kf++;
+ nb_kf = problem->getTrajectory()->getFrameMap().size();
}
// Store current state estimation
@@ -346,23 +420,21 @@ int main (int argc, char **argv) {
Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*i_R_b*b_p_bi);
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
// Vector3d o_p_ob = o_p_oi;
// Vector3d o_v_ob = o_v_oi;
imu_bias = sen_imu->getIntrinsic()->getState();
- // imu_bias = sen_imu->getIntrinsic(ts)->getState();
- Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+ Vector7d i_pose_im_fbk; i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
-
- p_ob_fbk_v.push_back(o_p_ob);
- q_ob_fbk_v.push_back(o_qvec_b);
- v_ob_fbk_v.push_back(o_v_ob);
+ mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
}
@@ -378,8 +450,12 @@ int main (int argc, char **argv) {
double* o_p_ob_carr = new double[3*N];
double* o_q_b_carr = new double[4*N];
+ double* o_v_ob_carr = new double[3*N];
+ double* o_p_oi_carr = new double[3*N];
+ double* o_q_i_carr = new double[4*N];
+ double* o_v_oi_carr = new double[3*N];
double* imu_bias_carr = new double[6*N];
- double* o_v_ob_carr = new double[6*N];
+
for (unsigned int i=0; i < N; i++) {
VectorComposite state_est = problem->getState(t_arr[i]);
Vector3d i_omg_oi = i_omg_oi_v[i];
@@ -389,26 +465,26 @@ int main (int argc, char **argv) {
auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
Vector6d imu_bias = sb->getState();
- // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_b = o_R_i * i_R_m;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
- // Vector3d o_p_ob = o_p_oi;
- // Vector3d o_v_ob = o_v_oi;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_im;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
+ mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
+ mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
}
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ unsigned int Nkf = problem->getTrajectory()->size();
double* tkf_carr = new double[1*Nkf];
double* Qp_carr = new double[3*Nkf];
double* Qo_carr = new double[3*Nkf];
@@ -416,14 +492,19 @@ int main (int argc, char **argv) {
double* Qbi_carr = new double[6*Nkf];
double* Qm_carr = new double[6*Nkf];
// feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_carr = new double[6*Nkf];
// factor errors
double* fac_imu_err_carr = new double[9*Nkf];
double* fac_pose_err_carr = new double[6*Nkf];
int i = 0;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
- std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
+ for (auto elt: problem->getTrajectory()->getFrameMap())
+ {
+ std::cout << "Traj " << i << "/" << problem->getTrajectory()->size() << std::endl;
tkf_carr[i] = elt.first.get();
auto kf = elt.second;
VectorComposite kf_state = kf->getState();
@@ -436,7 +517,8 @@ int main (int argc, char **argv) {
Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ if (compute_cov)
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
@@ -444,11 +526,12 @@ int main (int argc, char **argv) {
CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ if (compute_cov)
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
- // std::cout << "Qbp\n" << Qbp << std::endl;
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ if (compute_cov)
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// diagonal components
@@ -465,7 +548,11 @@ int main (int argc, char **argv) {
memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
// Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
// Factor errors
@@ -477,16 +564,6 @@ int main (int argc, char **argv) {
for (auto fac: fac_lst){
if (fac->getProcessor() == proc_imu){
auto f = std::dynamic_pointer_cast<FactorImu>(fac);
- // Matrix6d R_bias_drift = f->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
- // Matrix6d R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
- // std::cout << R_bias_drift << std::endl;
- // MatrixXd R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper();
- // MatrixXd Cov_bias_drift = f->getFeature()->getMeasurementCovariance();
- // std::cout << "R_bias_drift" << std::endl;
- // std::cout << R_bias_drift << std::endl;
- // std::cout << "Cov_bias_drift" << std::endl;
- // std::cout << Cov_bias_drift << std::endl;
-
if (f){
imu_err = f->error();
}
@@ -514,20 +591,28 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
- // save estimates
+ // Online estimates
cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
+ // offline states
cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
// and biases/extrinsics
cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
@@ -535,8 +620,12 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
diff --git a/demos/solo_kine_mocap.cpp b/demos/solo_kine_mocap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f3927500e2298866b1b31788a1d2c6d1e1a3608
--- /dev/null
+++ b/demos/solo_kine_mocap.cpp
@@ -0,0 +1,640 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <iostream>
+#include <fstream>
+
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include <cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_pose_3d_with_extrinsics.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+#include "bodydynamics/capture/capture_point_feet_nomove.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+ // retrieve parameters from yaml file
+ YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ double solve_every_sec = config["solve_every_sec"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
+
+ // robot
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
+
+ // Ceres setup
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+
+ // estimator sensor noises
+ // double std_pbc_est = config["std_pbc_est"].as<double>();
+ // double std_vbc_est = config["std_vbc_est"].as<double>();
+ // double std_f_est = config["std_f_est"].as<double>();
+ // double std_tau_est = config["std_tau_est"].as<double>();
+ double std_foot_nomove = config["std_foot_nomove"].as<double>();
+
+ // priors
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+
+ // IMU
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+
+ // base extrinsics
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+ Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+
+ // contacts
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
+ double fz_thresh = config["fz_thresh"].as<double>();
+
+
+ // MOCAP
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+ // Base to IMU transform
+ Quaterniond b_q_i(b_qvec_i);
+ assert(b_q_i.normalized().isApprox(b_q_i));
+ Quaterniond i_q_b = b_q_i.conjugate();
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
+
+ // initialize some vectors and fill them with dicretized data
+ cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+ //load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+ // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+ // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+ cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+ cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+ // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray contact_npyarr = arr_map["contacts"];
+ // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
+ // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+ double* t_arr = t_npyarr.data<double>();
+ double* imu_acc_arr = imu_acc_npyarr.data<double>();
+ // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+ double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
+ double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+ double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+ double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+ double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
+ double* contact_arr = contact_npyarr.data<double>();
+ // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
+ // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+ unsigned int N = t_npyarr.shape[0];
+ if (max_t > 0){
+ unsigned int N_max = (unsigned int)max_t/dt;
+ N = N < N_max ? N : N_max;
+ }
+ std::cout << "N: " << N << std::endl;
+
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("PO", 3);
+ // ProblemPtr problem = Problem::create("POV", 3);
+
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ problem->setTreeManager(tree_manager);
+
+ //////////////////////
+ // COMPUTE INITIAL STATE
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // initial configuration
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
+ VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+
+ //////////////////////
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ params_pfnm->voting_active = true;
+ params_pfnm->time_tolerance = 0.001;
+ params_pfnm->max_time_vote = 0.19999999;
+ ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+
+ // SENSOR + PROCESSOR POSE (for mocap)
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = std_pose_p;
+ intr_sp->std_o = toRad(std_pose_o_deg);
+ Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = time_tolerance_mocap;
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
+ sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
+ if (unfix_extr_sensor_pose){
+ sensor_pose->unfixExtrinsics();
+ }
+ else{
+ sensor_pose->fixExtrinsics();
+ }
+
+ Matrix3d w_R_m_init = q2R(w_qvec_wm);
+ Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
+
+ VectorComposite x_origin("PO", {w_p_wi_init, w_qvec_wm});
+ VectorComposite std_origin("PO", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones()});
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, time_tolerance_mocap); // if mocap used
+
+
+ //////////////////////////////////////////
+ // Vectors to store estimates at the time of data processing
+ // fbk -> feedback: to check if the estimation is stable enough for feedback control
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ // Allocate on the heap to prevent stack overflow for large datasets
+ double* o_p_ob_fbk_carr = new double[3*N];
+ double* o_q_b_fbk_carr = new double[4*N];
+ double* o_v_ob_fbk_carr = new double[3*N];
+ double* imu_bias_fbk_carr = new double[6*N];
+ double* i_pose_ib_fbk_carr = new double[7*N];
+
+ // covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+
+ //
+ std::vector<Vector3d> i_omg_oi_v;
+ //////////////////////////////////////////
+
+ unsigned int nb_kf = 1;
+ for (unsigned int i=0; i < N; i++){
+ TimeStamp ts(t_arr[i]);
+
+ ////////////////
+ // PROCESS MOCAP
+ ////////////////
+ // Get measurements
+ // retrieve traj data
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
+
+ // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ CP->process();
+ ////////////////
+ ////////////////
+
+ /////////////////////
+ // PROCESS KINEMATICS
+ /////////////////////
+ Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+ Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+
+
+ // Or else, retrieve from preprocessed dataset
+ Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+ Vector3d o_f_tot = Vector3d::Zero();
+ std::vector<Vector3d> l_f_l_vec;
+ // std::vector<Vector3d> o_f_l_vec;
+ std::vector<Vector3d> i_p_il_vec;
+ std::vector<Vector4d> i_qvec_l_vec;
+ // needing relative kinematics measurements
+ VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
+ VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ forwardKinematics(model, data, q_static, dq_static);
+ updateFramePlacements(model, data);
+
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation(); // meas
+ Matrix3d i_R_l = i_T_l.rotation();
+ Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
+ i_p_il = i_p_il + i_R_l*l_p_lg;
+
+ Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
+ // Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+
+ l_f_l_vec.push_back(l_f_l);
+ // o_f_l_vec.push_back(o_f_l);
+ i_p_il_vec.push_back(i_p_il);
+ i_qvec_l_vec.push_back(i_qvec_l);
+ }
+
+ // Detect feet in contact
+ int nb_feet_in_contact = 0;
+ std::unordered_map<int, Vector7d> kin_incontact;
+ // std::cout << "" << std::endl;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+ // if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
+ if (contact_gtr[i_ee] > 0.5){
+ nb_feet_in_contact++;
+ Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ kin_incontact.insert({i_ee, i_pose_il});
+ }
+ }
+
+
+ CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ CPF->process();
+
+
+ // solve every new KF
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << ts << " ";
+ std::cout << report << std::endl;
+
+ // recover covariances at this point
+ auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ // problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
+
+
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
+
+
+ nb_kf++;
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState();
+ // VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ // Vector3d o_v_oi = state_fbk['V'];
+ Vector3d o_v_oi = Vector3d::Zero();
+
+ // IMU to base frame
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ // Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
+ // Vector3d o_p_ob = o_p_oi;
+ Vector3d o_v_ob = o_v_oi;
+
+ Vector3d imu_bias = Vector3d::Zero();
+ // imu_bias = sen_imu->getIntrinsic()->getState();
+ // imu_bias = sen_imu->getIntrinsic(ts)->getState();
+ Vector7d i_pose_ib_est; i_pose_ib_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+ mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ mempcpy(i_pose_ib_fbk_carr+7*i, i_pose_ib_est.data(), 7*sizeof(double));
+
+ p_ob_fbk_v.push_back(o_p_ob);
+ q_ob_fbk_v.push_back(o_qvec_b);
+ v_ob_fbk_v.push_back(o_v_ob);
+ }
+
+
+
+ ///////////////////////////////////////////
+ //////////////// SOLVING //////////////////
+ ///////////////////////////////////////////
+ if (solve_end){
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem->print(4,true,true,true);
+ std::cout << report << std::endl;
+ }
+
+ double* o_p_ob_carr = new double[3*N];
+ double* o_q_b_carr = new double[4*N];
+ double* o_v_ob_carr = new double[6*N];
+ double* imu_bias_carr = new double[6*N];
+ for (unsigned int i=0; i < N; i++) {
+ VectorComposite state_est = problem->getState(t_arr[i]);
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = Vector3d::Zero();
+
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
+ Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ // Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ Vector3d o_v_ob = o_v_oi;
+
+ mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
+ mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+ mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ // mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ }
+
+
+ // Compute Covariances
+ unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ double* tkf_carr = new double[1*Nkf];
+ double* Qp_carr = new double[3*Nkf];
+ double* Qo_carr = new double[3*Nkf];
+ double* Qv_carr = new double[3*Nkf];
+ double* Qbi_carr = new double[6*Nkf];
+ double* Qm_carr = new double[6*Nkf];
+ // feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
+ double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_carr = new double[6*Nkf];
+
+ // factor errors
+ double* fac_imu_err_carr = new double[9*Nkf];
+ double* fac_pose_err_carr = new double[6*Nkf];
+ int i = 0;
+ for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
+ VectorComposite kf_state = kf->getState();
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+ problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+ // problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+ std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+ solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
+
+ // diagonal components
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
+ memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
+ memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
+ memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
+ memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+
+ // Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
+
+ i++;
+ }
+
+ // problem->check(1);
+
+ // Save trajectories in npz file
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+
+ // Online estimates
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_b_fbk_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
+
+ // offline states
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_b_carr, {N,4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_ob_carr, {N,3}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
+ // cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
+
+ // and biases/extrinsics
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_ib_fbk", i_pose_ib_fbk_carr, {N,7}, "a");
+
+ // covariances
+ cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
+
+}
diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml
index 1867ec7a227dd3a3d623fc6db430a9cbb36d823b..7106f2fce6adf0893afce1f7434b83f41e162c1f 100644
--- a/demos/solo_real_estimation.yaml
+++ b/demos/solo_real_estimation.yaml
@@ -10,14 +10,17 @@ solve_end: True
# Ceres setup
max_num_iterations: 1000
+func_tol: 1e-8
+compute_cov: false
# estimator sensor noises
std_pbc_est: 0.01
std_vbc_est: 0.01
std_f_est: 1
std_tau_est: 1000
-std_odom3d_est: 0.01
-# std_odom3d_est: 10000000
+std_foot_nomove: 0.01
+std_altitude: 0.01
+foot_radius: 0.0
# some controls
fz_thresh: 1 # slow walk
@@ -63,102 +66,25 @@ nb_feet: 4
l_p_lg: [0.0, 0, 0]
# l_p_lg: [0, 0, -0.016]
# l_p_lg: [0, 0, -0.031] # for sin traj, point to which position estimation on z starts to be less good
+foot_radius: 0.0
-# base to IMU transformation
-# b_p_bi: [0.0, 0.0, 0.0]
-# b_p_bi: [-0.2, 0.0, 0.0]
-b_p_bi: [0.1163, 0.0, 0.02]
-b_q_i: [0.0, 0.0, 0.0, 1.0]
-# b_q_i: [0.00000592745, -0.03255761280, -0.00025745595, 0.706732091]
-artificial_bias_p: [0.0, 0.0, 0.0]
-# artificial_bias_p: [0.03, 0.06, 0.04]
-# artificial_bias_p: [3, 6, 4]
-
-
-
-
-
-# Data files
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_10_04_StandingStill_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_10_04_StandingStill_shortened_format.npz"
-
-# 18:58 Fixe 4 stance phase (20s)
-# 19:00 Rotation 4 stance phase (30s)
-# 19:02 Mouvement avant arrière, rotation, rotation, mvt bas haut, roll (30s)
-# 19:03 Replay sin wave
-# 19:05 Replay stamping
-# 19:06 Marche 0.32 (30s)
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_18_58_format.npz" # OK
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_18_58_format_shortened.npz" # OK
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_00_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_02_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_03_format.npz" # OK
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_05_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_06_format.npz"
-
-
-
-
-# data_2020_10_15_14_34 standing still
-# data_2020_10_15_14_36 sin traj
-# data_2020_10_15_14_38 solo stamping
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened.npz"
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_Ogyr_Gacc_0dqa.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_Ogyr_Gacc_Filtereddqa.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTmean_Filtereddqa.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTacc_0gyr_Filtereddqa.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr_Gacc.npz"
+delta_qa: [0,0,0,0,0,0,0,0,0,0,0,0]
+alpha_qa: [0,0,0,0,0,0,0,0,0,0,0,0]
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTqa.npz"
+# IMU MOCAP extr
+m_p_mb: [-0.11872364, -0.0027602, -0.01272801]
+m_q_b: [0.00698701, 0.00747303, 0.00597298, 0.99992983]
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_VERY_shortened.npz"
+i_p_im: [-0.0111, -0.0073, -0.0024]
+i_q_m: [ 0.0035, 0.0042, 0.0051, 0.9999]
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format.npz"
-
-
-# data_2020_10_15_18_21: ...
-# data_2020_10_15_18_23: walk
-# data_2020_10_15_18_24: walk
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_21.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23.npz"
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_24.npz"
-
-
-# same but with precomputed forces
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_forces.npz" # standing still
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format_forces.npz" # sinusoid
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format_forces.npz" # stamping
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23_format_forces.npz" # fast walk
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_31_20_12_format_forces.npz" # 0.48s walk
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_18_format_forces.npz" # standing still+walk 0.48 FAIL
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_20_format_forces.npz" # standing still+walk 0.48
-
-
-# POINT FEET
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Point_feet_27_11_20/data_2020_11_27_14_46_format.npz" # standing still+walk 0.48
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_17_format.npz" # stamping
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_18_format.npz" # sin
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_22_format.npz" # walking
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_54_format.npz" # sin
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_56_format.npz" # stamping
-
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_25_format.npz" # point feet walk with corrected kinematics
-# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_29_format.npz" # point feet walk with corrected kinematics
+artificial_bias_p: [0.0, 0.0, 0.0]
+# artificial_bias_p: [0.03, 0.06, 0.04]
+# artificial_bias_p: [3, 6, 4]
-data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Mesures_Contact/data_2021_01_19_17_11_format.npz" # Contact experiment with contact status and mocap leg kin
+data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/LAAS_21_11_09/solo_trot_round_feet_format.npz"
out_npz_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments_results/out.npz"
\ No newline at end of file
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 0e4e6a4a6ce958563c3b4e84dfe3f31feb42f464..9953db41444d0d1b1da732f5460c3bd398cd3a63 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -69,7 +69,6 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
-// UTILS
#include "mcapi_utils.h"
@@ -88,7 +87,7 @@ int main (int argc, char **argv) {
int dimc = config["dimc"].as<int>();
double mass_offset = config["mass_offset"].as<double>();
double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
+ // double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>();
@@ -101,7 +100,7 @@ int main (int argc, char **argv) {
double std_vbc_est = config["std_vbc_est"].as<double>();
double std_f_est = config["std_f_est"].as<double>();
double std_tau_est = config["std_tau_est"].as<double>();
- double std_odom3d_est = config["std_odom3d_est"].as<double>();
+ double std_foot_nomove = config["std_foot_nomove"].as<double>();
// priors
double std_prior_p = config["std_prior_p"].as<double>();
@@ -303,7 +302,7 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = std_odom3d_est;
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
@@ -334,7 +333,7 @@ int main (int argc, char **argv) {
// 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
// 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
// 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
- proc_inkin->keyFrameCallback(KF1, 0.0005);
+ proc_inkin->keyFrameCallback(KF1);
proc_imu->setOrigin(KF1);
// proc_legodom->setOrigin(KF1);
@@ -577,19 +576,20 @@ int main (int argc, char **argv) {
}
}
- CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
- CPF->process();
+ // TODO: ???? kin_incontact is a map to Vector3d in this implementation, should be Vector7d [p, q]
+ // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ // CPF->process();
// add zero vel artificial factor
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
- auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
- std::cout << "New KF " << kf_pair->first << std::endl;
- auto kf = kf_pair->second;
+ if (problem->getTrajectory()->size() > nb_kf)
+ {
+ auto kf = problem->getTrajectory()->getLastFrame();
+ std::cout << "New KF " << kf->getTimeStamp() << std::endl;
- // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
+ // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf->getTimeStamp());
// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
// FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
@@ -728,7 +728,8 @@ int main (int argc, char **argv) {
VectorComposite bp_bias_est;
CaptureBasePtr cap_imu;
VectorComposite bi_bias_est;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ for (auto elt: problem->getTrajectory()->getFrameMap())
+ {
auto kf = elt.second;
kf_state = kf->getState();
cap_ikin = kf->getCaptureOf(sen_ikin);
diff --git a/demos/tree_manager.yaml b/demos/tree_manager.yaml
index ceb0dd77261e7361210a690b2ba563aaa58e7779..eb2b370c5838b91cffb42dcc06fc2dde1aec49f5 100644
--- a/demos/tree_manager.yaml
+++ b/demos/tree_manager.yaml
@@ -2,6 +2,6 @@ config:
problem:
tree_manager:
n_fix_first_frames: 3
- n_frames: 100000000000
+ n_frames: 50000
type: TreeManagerSlidingWindow
viral_remove_empty_parent: true
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index 12f4eb58c9e2a8b8d89e7923bea393e93bae48a5..81db7bf05f90fc9048a907253b477b73f8c748a5 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -62,7 +62,9 @@ class CaptureForceTorquePreint : public CaptureMotion
~CaptureForceTorquePreint() override;
- CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_; }
+ CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;}
+ CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;}
+ CaptureBaseConstPtr getGyroCaptureOther() const { return cap_gyro_other_;}
CaptureBasePtr getGyroCaptureOther(){ return cap_gyro_other_;}
private:
diff --git a/include/bodydynamics/capture/capture_point_feet_nomove.h b/include/bodydynamics/capture/capture_point_feet_nomove.h
index 1e57c0cdd71d3cc2cd85f196a51e16df57dce844..ffad3a2d2549f2cf55b0f785c47fe5fd4561f601 100644
--- a/include/bodydynamics/capture/capture_point_feet_nomove.h
+++ b/include/bodydynamics/capture/capture_point_feet_nomove.h
@@ -42,13 +42,13 @@ class CapturePointFeetNomove : public CaptureBase
CapturePointFeetNomove(
const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const std::unordered_map<int, Eigen::Vector3d>& kin_incontact
+ const std::unordered_map<int, Eigen::Vector7d>& kin_incontact
);
~CapturePointFeetNomove() override;
// <foot in contact number (0,1,2,3), b_p_bf>
- std::unordered_map<int, Eigen::Vector3d> kin_incontact_;
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_;
};
} // namespace wolf
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 85d74de654cfd2976eae06ab640086fec74870aa..958398a325c0e6c77fd5b70a2cabb4497fec746b 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -79,7 +79,7 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3
const T* const _qkm,
T* _res) const;
- void computeJac(const FeatureForceTorquePtr& _feat,
+ void computeJac(const FeatureForceTorqueConstPtr& _feat,
const double& _mass,
const double& _dt,
const Eigen::VectorXd& _bp,
@@ -149,25 +149,24 @@ void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePt
}
-void FactorForceTorque::computeJac(const FeatureForceTorquePtr& _feat,
+void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
const double& _mass,
const double& _dt,
const Eigen::VectorXd& _bp,
Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
{
_J_ny_nz.setZero();
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
// Measurements retrieval
- Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
- Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
+ Eigen::Map<const Eigen::Vector3d> f1 (_feat->getForcesMeas().data());
+ Eigen::Map<const Eigen::Vector3d> f2 (_feat->getForcesMeas().data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> tau1(_feat->getTorquesMeas().data());
+ Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> pbl1(_feat->getKinMeas().data());
+ Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3 );
+ Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
+ Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
+ Eigen::Map<const Eigen::Vector3d> pbc (_feat->getPbcMeas().data());
Eigen::Matrix3d bRl1 = q2R(bql1);
Eigen::Matrix3d bRl2 = q2R(bql2);
@@ -226,7 +225,7 @@ bool FactorForceTorque::operator () (
const T* const _qkm,
T* _res) const
{
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(getFeature());
+ auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
// State variables instanciation
Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index c0b36d22bdd65b63b575e6d80863ea2eea978405..ff824e20b8673da4d6de79afa06e80da6cd2607e 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -158,7 +158,7 @@ bool FactorInertialKinematics::operator () (
Map<Matrix<T,9,1> > res(_res);
- FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
+ auto feat = std::static_pointer_cast<const FeatureInertialKinematics>(getFeature());
// Measurements retrieval
Map<const Vector3d> pBC_m(getMeasurement().data() ); // B_p_BC
diff --git a/include/bodydynamics/factor/factor_point_feet_altitude.h b/include/bodydynamics/factor/factor_point_feet_altitude.h
new file mode 100644
index 0000000000000000000000000000000000000000..235244a272ded4a67a5a4d8eae8c986476420140
--- /dev/null
+++ b/include/bodydynamics/factor/factor_point_feet_altitude.h
@@ -0,0 +1,145 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file factor_point_feet_altitude.h
+ *
+ * Created on: Nov 5, 2020
+ * \author: mfourmy
+ */
+
+#ifndef FACTOR_POINT_FEET_ALTITUDE_H_
+#define FACTOR_POINT_FEET_ALTITUDE_H_
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorPointFeetAltitude);
+
+class FactorPointFeetAltitude : public FactorAutodiff<FactorPointFeetAltitude,
+ 1,
+ 3,
+ 4
+ >
+{
+ public:
+ FactorPointFeetAltitude(const FeatureBasePtr& _ftr_current_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetAltitude() override { /* nothing */ }
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ */
+ template<typename T>
+ bool operator () (
+ const T* const _pb,
+ const T* const _qb,
+ T* _res) const;
+
+ Vector1d error() const;
+ Vector1d res() const;
+ double cost() const;
+
+};
+
+} /* namespace wolf */
+
+
+namespace wolf {
+
+FactorPointFeetAltitude::FactorPointFeetAltitude(
+ const FeatureBasePtr& _ftr_current_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff("FactorPointFeetAltitude",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ nullptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(),
+ _ftr_current_ptr->getFrame()->getO()
+ )
+{
+}
+
+Vector1d FactorPointFeetAltitude::error() const{
+
+ Vector3d pb = getFrame()->getP()->getState();
+ Vector4d qb_vec = getFrame()->getO()->getState();
+ Quaterniond qb(qb_vec);
+
+ // Measurements retrieval
+ Eigen::Matrix<double,4,1> meas = getMeasurement();
+ Vector3d b_p_bl = meas.topRows(3);
+ Vector1d z = meas.bottomRows(1);
+
+ return (pb + qb*b_p_bl).block(2,0,1,1) - z;
+}
+
+Vector1d FactorPointFeetAltitude::res() const{
+ return getMeasurementSquareRootInformationUpper() * error();
+}
+
+
+double FactorPointFeetAltitude::cost() const{
+ return res().squaredNorm();
+}
+
+template<typename T>
+bool FactorPointFeetAltitude::operator () (
+ const T* const _pb,
+ const T* const _qb,
+ T* _res) const
+{
+ Map<Matrix<T,1,1> > res(_res);
+
+ // State variables instanciation
+ Map<const Matrix<T,3,1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
+
+ // Measurements retrieval
+ Eigen::Matrix<T,4,1> meas = getMeasurement().cast<T>();
+ Matrix<T,3,1> b_p_bl = meas.topRows(3); // Bpast_p_BpastL
+ Matrix<T,1,1> z = meas.bottomRows(1); // altitude
+
+ Matrix<T,1,1> err = (pb + qb*b_p_bl).block(2,0,1,1) - z;
+
+ res = getMeasurementSquareRootInformationUpper() * err;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif /* FACTOR__POINT_FEET_ALTITUDE_H_ */
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index 58e7804f43b3c3e1ffe0de8e85711c3f3d595e06..9e17e16b75f6cc703ea6f5f7c58b35df27b39aad 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -29,18 +29,11 @@
#ifndef FACTOR_POINT_FEET_NOMOVE_H_
#define FACTOR_POINT_FEET_NOMOVE_H_
+#include "core/math/rotations.h"
#include "core/factor/factor_autodiff.h"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
-// namespace
-// {
-
-// constexpr std::size_t RESIDUAL_SIZE = 3;
-// constexpr std::size_t POSITION_SIZE = 3;
-// constexpr std::size_t ORIENTATION_SIZE = 4;
-
-// }
-
namespace wolf
{
@@ -67,19 +60,20 @@ class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
Factor state blocks:
_pb: W_p_WB -> base position in world frame
_qb: W_q_B -> base orientation in world frame
- _pbm: W_p_WB -> base position in world frame, previous KF
- _qbm: W_q_B -> base orientation in world frame, previous KF
+ _pb_past: W_p_WB -> base position in world frame, pastious KF
+ _qb_past: W_q_B -> base orientation in world frame, pastious KF
*/
template<typename T>
bool operator () (
const T* const _pb,
const T* const _qb,
- const T* const _pbm,
- const T* const _qbm,
+ const T* const _pb_past,
+ const T* const _qb_past,
T* _res) const;
- // Vector9d residual() const;
- // double cost() const;
+ Vector3d error() const;
+ Vector3d res() const;
+ double cost() const;
};
@@ -112,33 +106,39 @@ FactorPointFeetNomove::FactorPointFeetNomove(
{
}
-// Vector9d FactorPointFeetNomove::residual() const{
-// Vector9d res;
-// double * pb, * qb, * vb, * c, * cd, * Lc, * bp, * baw;
-// pb = getFrame()->getP()->getState().data();
-// qb = getFrame()->getO()->getState().data();
-// vb = getFrame()->getV()->getState().data();
-// c = getFrame()->getStateBlock('C')->getState().data();
-// cd = getFrame()->getStateBlock('D')->getState().data();
-// Lc = getFrame()->getStateBlock('L')->getState().data();
-// bp = getCapture()->getStateBlock('I')->getState().data();
-// baw = sb_bias_imu_->getState().data();
-
-// operator() (pb, qb, vb, c, cd, Lc, bp, baw, res.data());
-// // std::cout << "res: " << res.transpose() << std::endl;
-// return res;
-// }
-
-// double FactorPointFeetNomove::cost() const{
-// return residual().squaredNorm();
-// }
+Vector3d FactorPointFeetNomove::error() const{
+
+ Vector3d pb = getFrame()->getP()->getState();
+ Vector4d qb_vec = getFrame()->getO()->getState();
+ Quaterniond qb(qb_vec);
+ Vector3d pbm = getFrameOther()->getP()->getState();
+ Vector4d qb_past_vec = getFrameOther()->getO()->getState();
+ Quaterniond qbm(qb_past_vec);
+
+ // Measurements retrieval
+ Matrix<double,6,1> meas = getMeasurement();
+ Vector3d bm_p_bml = meas.topRows(3);
+ Vector3d b_p_bl = meas.bottomRows(3);
+
+ return (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
+
+}
+
+Vector3d FactorPointFeetNomove::res() const{
+ return getMeasurementSquareRootInformationUpper() * error();
+}
+
+
+double FactorPointFeetNomove::cost() const{
+ return res().squaredNorm();
+}
template<typename T>
bool FactorPointFeetNomove::operator () (
const T* const _pb,
const T* const _qb,
- const T* const _pbm,
- const T* const _qbm,
+ const T* const _pb_past,
+ const T* const _qb_past,
T* _res) const
{
Map<Matrix<T,3,1> > res(_res);
@@ -146,15 +146,48 @@ bool FactorPointFeetNomove::operator () (
// State variables instanciation
Map<const Matrix<T,3,1> > pb(_pb);
Map<const Quaternion<T> > qb(_qb);
- Map<const Matrix<T,3,1> > pbm(_pbm);
- Map<const Quaternion<T> > qbm(_qbm);
+ Map<const Matrix<T,3,1> > pbm(_pb_past);
+ Map<const Quaternion<T> > qbm(_qb_past);
// Measurements retrieval
- Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
- Matrix<T,3,1> bm_p_bml = meas.topRows(3); // Bprev_p_BprevL
- Matrix<T,3,1> b_p_bl = meas.bottomRows(3); // B_p_BL
+ auto& meas = getMeasurement();
+ Matrix<T,3,1> bm_p_bml = meas.segment(0, 3).cast<T>(); // Bminus_p_BminusL
+ Matrix<T,4,1> bm_qvec_l = meas.segment(3, 4).cast<T>(); // Bminus_q_BminusL
+ Matrix<T,3,1> b_p_bl = meas.segment(7, 3).cast<T>(); // B_p_BL
+ Matrix<T,4,1> b_qvec_l = meas.segment(10, 4).cast<T>(); // B_q_BL
+ Eigen::Quaternion<T> bm_q_l(bm_qvec_l);
+ Eigen::Quaternion<T> b_q_l(b_qvec_l);
+
+ // double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius(); // DOES NOT COMPILE
+ double radius = 0.0; // deactivate, was not doing so great anyway
+
+ // If the radius of the foot is not negligeable, try to account for it with a more complex model
+ Matrix<T,3,1> err;
+ if (std::fabs(radius) < wolf::Constants::EPS){
+ err = (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
+ }
+ else{
+ /*
+ n: frame with (xy) plane == world but orientation aligned in yaw with the robot base frame
+ sometimes called "Navigation frame" in some papers.
+ Simple roll without slipping model: we assume that most of actual movement
+ of the frame attached to the center of the foot is due to rolling along y axis of the foot (for solo)
+ */
+ Quaternion<T> l_q_lm = (qbm*bm_q_l).inverse() * qb*b_q_l;
+ Matrix<T,3,1> l_aa_lm = log_q(l_q_lm);
+ Matrix<T,3,1> n_p_lml = Matrix<T,3,1>::Zero();
+ n_p_lml(0) = -l_aa_lm(1)*radius;
+
+ // Factor: w_p_wl = w_p_wlm + w_R_n * n_p_lml
+ // By def, w_R_n is w_R_b with roll and pitch set to zero
+ // Matrix<T,3,1> aa_bm = q2e(qbm); // DOES NOT WORK -> equivalent angle axis in this case
+ Matrix<T,3,1> aa_bm = log_q(qbm);
+ aa_bm.segment(0,2) = Matrix<T,2,1>::Zero();
+ Quaternion<T> w_q_nm = exp_q(aa_bm);
+
+ err = (pbm + qbm*bm_p_bml) + w_q_nm*n_p_lml - (pb + qb*b_p_bl);
+ }
- Matrix<T,3,1> err = (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
res = getMeasurementSquareRootInformationUpper() * err;
diff --git a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
new file mode 100644
index 0000000000000000000000000000000000000000..1907583d9d198d90c9f265a970e69dfc8dcfe507
--- /dev/null
+++ b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
@@ -0,0 +1,134 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file factor_point_feet_nomove.h
+ *
+ * Created on: Feb 22, 2020
+ * \author: mfourmy
+ */
+
+#ifndef FACTOR_POINT_FEET_ZERO_VELOCITY_H_
+#define FACTOR_POINT_FEET_ZERO_VELOCITY_H_
+
+#include "core/factor/factor_autodiff.h"
+
+
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorPointFeetNomove);
+
+class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
+ 3,
+ 3,
+ 4,
+ 3
+ >
+{
+ public:
+ FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const StateBlockPtr& _bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetNomove() override { /* nothing */ }
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ _pbm: W_p_WB -> base position in world frame, previous KF
+ _qbm: W_q_B -> base orientation in world frame, previous KF
+ */
+ template<typename T>
+ bool operator () (
+ const T* const _vb,
+ const T* const _qb,
+ const T* const _bi,
+ T* _res) const;
+
+ // Vector9d residual() const;
+ // double cost() const;
+
+};
+
+} /* namespace wolf */
+
+
+namespace wolf {
+
+FactorPointFeetNomove::FactorPointFeetNomove(
+ const FeatureBasePtr& _ftr_current_ptr,
+ const StateBlockPtr& _bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff("FactorPointFeetNomove",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ nullptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getV(),
+ _ftr_current_ptr->getFrame()->getO(),
+ _bias_imu
+ )
+{
+}
+
+template<typename T>
+bool FactorPointFeetNomove::operator () (
+ const T* const _vb,
+ const T* const _qb,
+ const T* const _bi,
+ T* _res) const
+{
+ Map<Matrix<T,3,1> > res(_res);
+
+ // State variables instanciation
+ Map<const Matrix<T,3,1> > vb(_vb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T,3,1> > bw(_pbm+3); // gyro bias
+ Map<const Quaternion<T> > qbm(_qbm);
+
+ // Measurements retrieval
+ Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
+ Eigen::Matrix<T,3,1> b_v_bl = meas.head<3>();
+ Eigen::Matrix<T,3,1> i_omg_i = meas.segment<3>(3);
+ Eigen::Matrix<T,3,1> b_p_bl = meas.tail<3>();
+
+ Matrix<T,3,1> err = qb.conjugate()*vb + (i_omg_i - bw).cross(b_p_bl) + b_v_bl;
+
+ res = getMeasurementSquareRootInformationUpper() * err;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif /* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index bbe2fda19d7fcdbde4e88aa564efc3547a6293ef..14594821f3171f7628199e4b3b5f76026f4f7f63 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -50,19 +50,19 @@ class FeatureForceTorque : public FeatureBase
~FeatureForceTorque() override;
- const double & getDt(){return dt_;}
- const double & getMass(){return mass_;}
+ const double & getDt() const {return dt_;}
+ const double & getMass() const {return mass_;}
void setDt(const double & _dt){dt_ = _dt;}
void setMass(const double & _mass){mass_ = _mass;}
- const Eigen::Vector6d& getForcesMeas(){return forces_meas_;}
- const Eigen::Vector6d& getTorquesMeas(){return torques_meas_;}
- const Eigen::Vector3d& getPbcMeas(){return pbc_meas_;}
- const Eigen::Matrix<double,14,1>& getKinMeas(){return kin_meas_;}
- const Eigen::Matrix6d& getCovForcesMeas(){return cov_forces_meas_;}
- const Eigen::Matrix6d& getCovTorquesMeas(){return cov_torques_meas_;}
- const Eigen::Matrix3d& getCovPbcMeas(){return cov_pbc_meas_;}
- const Eigen::Matrix<double,14,14>& getCovKinMeas(){return cov_kin_meas_;}
+ const Eigen::Vector6d& getForcesMeas() const {return forces_meas_;}
+ const Eigen::Vector6d& getTorquesMeas() const {return torques_meas_;}
+ const Eigen::Vector3d& getPbcMeas() const {return pbc_meas_;}
+ const Eigen::Matrix<double,14,1>& getKinMeas() const {return kin_meas_;}
+ const Eigen::Matrix6d& getCovForcesMeas() const {return cov_forces_meas_;}
+ const Eigen::Matrix6d& getCovTorquesMeas() const {return cov_torques_meas_;}
+ const Eigen::Matrix3d& getCovPbcMeas() const {return cov_pbc_meas_;}
+ const Eigen::Matrix<double,14,14>& getCovKinMeas() const {return cov_kin_meas_;}
void setForcesMeas(const Eigen::Vector6d& _forces_meas){forces_meas_ = _forces_meas;}
void setTorquesMeas(const Eigen::Vector6d& _torques_meas){torques_meas_ = _torques_meas;}
diff --git a/include/bodydynamics/feature/feature_inertial_kinematics.h b/include/bodydynamics/feature/feature_inertial_kinematics.h
index bb9588ffbc24e78162bedddc5df7a914f7d55c63..98f03fc3e948e1c5feeaf399e3bdf10297a2b1a6 100644
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
@@ -43,8 +43,8 @@ class FeatureInertialKinematics : public FeatureBase
~FeatureInertialKinematics() override = default;
- const Eigen::Matrix3d & getBIq(){return BIq_;}
- const Eigen::Vector3d & getBLcm(){return BLcm_;}
+ const Eigen::Matrix3d & getBIq() const {return BIq_;}
+ const Eigen::Vector3d & getBLcm() const {return BLcm_;}
void setBIq(const Eigen::Matrix3d & _BIq){BIq_ = _BIq;}
void setBLcm(const Eigen::Vector3d & _BLcm){BLcm_ = _BLcm;}
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index c834f71b8e1fc42b2d9d8e7dfd1ee43157627d43..6cd089107cfebd55fad2a51ff1b6cf5eddb0b036 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -92,7 +92,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
bool voteForKeyFrame() const override;
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
index 49c40be86b331397416bc8bf806aac4a4b6f9419..5c3f727fc26936f518795d146aad3ba02ae4dec6 100644
--- a/include/bodydynamics/processor/processor_point_feet_nomove.h
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -29,21 +29,26 @@
#include "bodydynamics/capture/capture_point_feet_nomove.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/factor/factor_point_feet_nomove.h"
+#include "bodydynamics/factor/factor_point_feet_altitude.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPointFeetNomove);
struct ParamsProcessorPointFeetNomove : public ParamsProcessorBase
{
+ double max_time_vote = -1;
+
ParamsProcessorPointFeetNomove() = default;
ParamsProcessorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server)
{
+ max_time_vote = _server.getParam<double>(_unique_name + "/max_time_vote");
}
~ParamsProcessorPointFeetNomove() override = default;
std::string print() const override
{
- return "\n" + ParamsProcessorBase::print() + "\n";
+ return "\n" + ParamsProcessorBase::print() + "\n"
+ + "max_time_vote: " + std::to_string(max_time_vote) + "\n";
}
};
@@ -68,11 +73,18 @@ class ProcessorPointFeetNomove : public ProcessorBase{
bool storeCapture(CaptureBasePtr) override;
bool voteForKeyFrame() const override;
- void createFactorIfNecessary();
+ void createFactorConsecutive();
+
+ void processCaptureCreateFactorFar(CaptureBasePtr _capture);
+ void processKeyFrameCreateFactorFar();
+
protected:
ParamsProcessorPointFeetNomovePtr params_pfnomove_;
+ CaptureBasePtr origin_;
+ CaptureBasePtr incoming_;
+ std::map<int, CaptureBasePtr> tracks_;
};
@@ -97,10 +109,6 @@ inline bool ProcessorPointFeetNomove::storeCapture(CaptureBasePtr)
return true;
}
-inline bool ProcessorPointFeetNomove::voteForKeyFrame() const
-{
- return false;
-}
} /* namespace wolf */
diff --git a/include/bodydynamics/sensor/sensor_point_feet_nomove.h b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
index 7f58ad65e96d1ca4c249cdd23a4f8e88b03f8060..077765e0ae0fc3d4df3bba4fb627a54cd507f473 100644
--- a/include/bodydynamics/sensor/sensor_point_feet_nomove.h
+++ b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
@@ -34,20 +34,29 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPointFeetNomove);
struct ParamsSensorPointFeetNomove : public ParamsSensorBase
{
- //noise std dev
- double std_foot_nomove_; // standard deviation on the assumption that the feet are not moving when in contact
+ // standard deviation on the assumption that the feet are not moving when in contact
+ // the noise represents a random walk on the foot position, hence the unit
+ double std_foot_nomove_; // m/(s^2 sqrt(dt))
+ // certainty on current ground altitude
+ double std_altitude_ = 1000; // m, deactivated by default
+ double foot_radius_ = 0; // m
ParamsSensorPointFeetNomove() = default;
ParamsSensorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- std_foot_nomove_ = _server.getParam<double>(_unique_name + "/std_foot_nomove");
+ std_foot_nomove_ = _server.getParam<double>(_unique_name + "/std_foot_nomove");
+ std_altitude_ = _server.getParam<double>(_unique_name + "/std_altitude");
+ foot_radius_ = _server.getParam<double>(_unique_name + "/foot_radius");
}
~ParamsSensorPointFeetNomove() override = default;
std::string print() const override
{
- return "\n" + ParamsSensorBase::print() + "\n"
- + "std_foot_nomove: " + std::to_string(std_foot_nomove_) + "\n";
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "std_altitude: " + std::to_string(std_altitude_) + "\n"
+ + "std_foot_nomove: " + std::to_string(std_foot_nomove_) + "\n";
+ + "foot_radius_: " + std::to_string(foot_radius_) + "\n";
+
}
};
@@ -57,7 +66,9 @@ class SensorPointFeetNomove : public SensorBase
{
protected:
- Matrix3d cov_foot_nomove_;
+ Matrix3d cov_foot_nomove_; // random walk covariance in (m/s^2/sqrt(Hz))^2
+ Matrix1d cov_altitude_; // m^2
+ double foot_radius_;
public:
@@ -67,6 +78,8 @@ class SensorPointFeetNomove : public SensorBase
WOLF_SENSOR_CREATE(SensorPointFeetNomove, ParamsSensorPointFeetNomove, 0);
Matrix3d getCovFootNomove() const;
+ Matrix1d getCovAltitude() const;
+ double getFootRadius() const;
};
inline Matrix3d SensorPointFeetNomove::getCovFootNomove() const
@@ -74,6 +87,16 @@ inline Matrix3d SensorPointFeetNomove::getCovFootNomove() const
return cov_foot_nomove_;
}
+inline Matrix1d SensorPointFeetNomove::getCovAltitude() const
+{
+ return cov_altitude_;
+}
+
+inline double SensorPointFeetNomove::getFootRadius() const
+{
+ return foot_radius_;
+}
+
} // namespace wolf
diff --git a/internal/config.h.in b/internal/config.h.in
index 7014ca320e5c5fea1f59a6fe5d9d1ea0987bf95e..35f3468331656ba09a63ad7f26e5c0fb78c8d5bc 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,13 +24,13 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
-#ifndef WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
-#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
+#ifndef WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
+#define WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
#cmakedefine _WOLF_DEBUG
#cmakedefine _WOLF_TRACE
-#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_${PROJECT_NAME_UPPER}_ROOT_DIR "${_WOLF_ROOT_DIR}"
#endif /* WOLF_INTERNAL_CONFIG_H_ */
diff --git a/src/capture/capture_point_feet_nomove.cpp b/src/capture/capture_point_feet_nomove.cpp
index 3d4dca45ced860903bfdfabc3f4a86e088b63859..3851ea477acd49d8c403f5e27dd3840b49304008 100644
--- a/src/capture/capture_point_feet_nomove.cpp
+++ b/src/capture/capture_point_feet_nomove.cpp
@@ -38,7 +38,7 @@ namespace wolf {
CapturePointFeetNomove::CapturePointFeetNomove(
const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const std::unordered_map<int, Eigen::Vector3d>& kin_incontact
+ const std::unordered_map<int, Eigen::Vector7d>& kin_incontact
) :
CaptureBase("CapturePointFeetNomove",
_init_ts,
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index 994b91a49c911dc5835b1d642ead04d2f5ca0223..1b84f786077a0f65ee3cc944bf8ebe4f54511499 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -136,14 +136,14 @@ Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd&
return bodydynamics::plus(delta_preint, delta_step);
}
-VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const
{
VectorXd bias_vec(6);
if (_capture) // access from capture is quicker
{
- CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+ auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture));
// get calib part from Ikin capture
bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
@@ -211,7 +211,7 @@ void ProcessorForceTorquePreint::computeCurrentDelta(
bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
MatrixXd jac_delta_body(12,data_size_-nbc_);
- bodydynamics::body2delta(body, _dt, std::static_pointer_cast<SensorForceTorque>(getSensor())->getMass(),
+ bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
nbc_, dimc_,
_delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 46eb8f944900d0eff9596ad239351d19ec74f1a3..360ef7d654952c17a18dce633f53f9177e79e61d 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -68,7 +68,6 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
// done AFTER processCapture)
// 1. get corresponding KF
- FrameBasePtr kf;
auto buffer_frame_it = buffer_frame_.getContainer().begin();
auto buffer_capture_it = buffer_capture_.getContainer().begin();
diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index 358ad06be77d987125dcd0341bbd10e62c6fdd78..161c16dd18e4bdd512868d6c9bc76ed2d4f87442 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -34,8 +34,14 @@ namespace wolf{
inline ProcessorPointFeetNomove::ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove) :
ProcessorBase("ProcessorPointFeetNomove", 3, _params_pfnomove),
- params_pfnomove_(std::make_shared<ParamsProcessorPointFeetNomove>(*_params_pfnomove))
+ params_pfnomove_(std::make_shared<ParamsProcessorPointFeetNomove>(*_params_pfnomove)),
+ origin_(nullptr)
{
+ // Init tracks to nothing
+
+ for (int i=0; i < 4; i++){
+ tracks_[0] = nullptr;
+ }
}
@@ -43,7 +49,119 @@ void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor)
{
}
-void ProcessorPointFeetNomove::createFactorIfNecessary(){
+
+
+inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+ incoming_ = _capture;
+
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ createFactorConsecutive();
+ // processCaptureCreateFactorFar(_capture);
+
+}
+
+inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
+{
+ if (!_keyframe_ptr)
+ {
+ WOLF_ERROR("Received KF is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+
+ createFactorConsecutive();
+ // processKeyFrameCreateFactorFar();
+
+}
+
+void ProcessorPointFeetNomove::processCaptureCreateFactorFar(CaptureBasePtr _capture){
+ // Far away factors
+ auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(_capture);
+ auto kin_incontact_current = cap_curr->kin_incontact_;
+ for (int i=0; i<4; i++){
+ if (!kin_incontact_current.count(i)){
+ tracks_[i] = nullptr;
+ }
+ }
+}
+
+void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
+
+ auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
+
+ for (auto kf_it: buffer_frame_.getContainer()){
+ TimeStamp ts = kf_it.first;
+ auto kf = kf_it.second;
+ auto cap_it = buffer_capture_.selectIterator(ts, params_->time_tolerance);
+
+ // We have a match between the KF and a capture from the capture buffer
+ if (cap_it != buffer_capture_.getContainer().end()){
+ auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
+
+ for (auto kin: cap_curr->kin_incontact_){
+ // check if each leg in contact has a current active track
+ int leg_id = kin.first;
+ auto cap_origin = tracks_[leg_id];
+ if (!cap_origin){
+ // if not, define current KF as the leg track origin and link the capture to this KF
+ // Error occurs if we try to link several time the same Cap->kf due to the loop on legs -> do only once
+ if (!cap_curr->getFrame()){
+ cap_curr->link(kf);
+ }
+ tracks_[leg_id] = cap_curr;
+ }
+ else {
+ if (!cap_curr->getFrame()){
+ cap_curr->link(kf);
+ }
+ // if it has, create a factor
+ auto cap_origin = std::static_pointer_cast<CapturePointFeetNomove>(tracks_[leg_id]);
+ Matrix<double, 14, 1> meas; meas << cap_origin->kin_incontact_[leg_id], kin.second;
+ double dt_kf = cap_curr->getTimeStamp() - cap_origin->getTimeStamp();
+ Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
+ // Matrix3d cov_int = sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap_curr, "PointFeet", meas, cov_int);
+ FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, cap_origin->getFrame(), nullptr, true);
+
+ // Zero Altitude factor
+ Vector4d meas_altitude; meas_altitude << kin.second.head<3>(), 0;
+ Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
+ CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf, "Alt0", cap_curr->getTimeStamp());
+ FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
+ FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
+ }
+ }
+ buffer_frame_.removeUpTo(cap_curr->getTimeStamp());
+ buffer_capture_.removeUpTo(cap_curr->getTimeStamp());
+ }
+ }
+}
+
+void ProcessorPointFeetNomove::createFactorConsecutive(){
+
auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
while (buffer_frame_.size() >= 2)
@@ -66,23 +184,22 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
// store the initial contact/kinematic meas map
auto cap1 = std::static_pointer_cast<CapturePointFeetNomove>(cap1_it->second);
auto kin_incontact_from_t1 = cap1->kin_incontact_;
- std::cout << kin_incontact_from_t1.size() << " ";
-
+ // then loop over the captures in between cap1 and cap2
auto cap_it = std::next(cap1_it);
- auto it_after_capt_t2 = std::next(cap2_it);
+ auto it_after_capt_t2 = std::next(cap2_it); // used to detect the end of the loop
// loop throught the captures between t1 and t2
while (cap_it != it_after_capt_t2){
// for each new capture, filter the initial contact/kin meas map to track which feet stay in contact throughout
- auto cap_pfeet = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
- if (!cap_pfeet){
-
+ auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
+ if (!cap_curr){
+ WOLF_ERROR("Wrong capture type in buffer")
}
- auto kin_incontact_current = cap_pfeet->kin_incontact_;
- // std::cout << kin_incontact_current.size() << " ";
-
+ // check which feet are currently in contact, if one is missing wrt the previous
+ // timestamp, remove it from kin_incontact_from_t1
+ auto kin_incontact_current = cap_curr->kin_incontact_;
for (auto elt_it = kin_incontact_from_t1.cbegin(); elt_it != kin_incontact_from_t1.cend(); /* no increment */){
if (kin_incontact_current.find(elt_it->first) == kin_incontact_current.end()){
// The foot has lost contact with the ground since t1
@@ -94,7 +211,6 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
}
cap_it++;
}
- // std::cout << std::endl;
if (!kin_incontact_from_t1.empty())
{
@@ -107,9 +223,18 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
for (auto kin_pair1: kin_incontact_from_t1){
auto kin_pair2_it = kin_incontact_t2.find(kin_pair1.first);
- Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second;
- FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove());
+ Matrix<double, 14, 1> meas; meas << kin_pair1.second, kin_pair2_it->second;
+ double dt_kf = cap2->getTimeStamp() - cap1->getTimeStamp();
+ Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, cov_int);
FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
+
+ // Zero Altitude factor
+ Vector4d meas_altitude; meas_altitude << kin_pair2_it->second.head<3>(), 0;
+ Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
+ CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second, "Alt0", cap2->getTimeStamp());
+ FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
+ FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
}
}
@@ -122,51 +247,15 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
}
-inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
-{
- if (!_capture)
- {
- WOLF_ERROR("Received capture is nullptr.");
- return;
- }
- // nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
- return;
- }
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
- return;
- }
-
- createFactorIfNecessary();
-}
-inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
+inline bool ProcessorPointFeetNomove::voteForKeyFrame() const
{
- if (!_keyframe_ptr)
- {
- WOLF_ERROR("Received KF is nullptr.");
- return;
- }
- // nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
- return;
- }
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
- return;
- }
-
- createFactorIfNecessary();
+ return false;
}
-
-
} /* namespace wolf */
// Register in the FactoryProcessor
diff --git a/src/sensor/sensor_point_feet_nomove.cpp b/src/sensor/sensor_point_feet_nomove.cpp
index 6e5a48b7c058aa152ad4179404a061e7fe74dedd..45d1c1ade0ac9cd8fe92ce44f01033b30933602b 100644
--- a/src/sensor/sensor_point_feet_nomove.cpp
+++ b/src/sensor/sensor_point_feet_nomove.cpp
@@ -36,6 +36,8 @@ SensorPointFeetNomove::SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics_
assert(_extrinsics_pq.size() == 0 && "Bad extrinsics vector size! Should be 0 since not used.");
cov_foot_nomove_ = pow(_params->std_foot_nomove_, 2)*Matrix3d::Identity();
+ cov_altitude_ = pow(_params->std_altitude_, 2)*Matrix1d::Identity();
+ foot_radius_ = _params->foot_radius_;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 60f23c1fe3f823d3c1c848365735928550d24095..5850e2db6763d74e6385f16417ed48102091ba5b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,66 +1,33 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-# INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
-################# ADD YOUR TESTS BELOW ####################
+############# USE THIS TEST AS AN EXAMPLE #################
# #
-# ==== IN ALPHABETICAL ORDER! ==== #
+# Create a specific test executable for gtest_example #
+# wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
+###########################################################
+
wolf_add_gtest(gtest_capture_inertial_kinematics gtest_capture_inertial_kinematics.cpp)
-target_link_libraries(gtest_capture_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_capture_leg_odom gtest_capture_leg_odom.cpp)
-target_link_libraries(gtest_capture_leg_odom ${PLUGIN_NAME})
wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinematics.cpp)
-target_link_libraries(gtest_feature_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-target_link_libraries(gtest_factor_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-target_link_libraries(gtest_factor_force_torque ${PLUGIN_NAME})
wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
-target_link_libraries(gtest_force_torque_delta_tools ${PLUGIN_NAME})
+# TODO: revive those
# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
-# target_link_libraries(gtest_feature_force_torque_preint ${PLUGIN_NAME})
-
# wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
-# target_link_libraries(gtest_processor_inertial_kinematics ${PLUGIN_NAME})
-
# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
-# target_link_libraries(gtest_processor_force_torque_preint ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
-target_link_libraries(gtest_processor_point_feet_nomove ${PLUGIN_NAME})
wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
-target_link_libraries(gtest_sensor_force_torque ${PLUGIN_NAME})
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
-target_link_libraries(gtest_sensor_inertial_kinematics ${PLUGIN_NAME})
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..2294c819aec95c1f2c6bed5c0e7b1d6cc857c614 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,73 @@
-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ message("CMake version less than 3.11.0")
-# We need thread support
-#find_package(Threads REQUIRED)
+ # Enable ExternalProject CMake module
+ include(ExternalProject)
-# Enable ExternalProject CMake module
-include(ExternalProject)
+ set(GTEST_FORCE_SHARED_CRT ON)
+ set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+ # Download GoogleTest
+ ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+ )
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
- set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+ # Get GTest source and binary directories from CMake project
-# Download GoogleTest
-ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+ # Specify include dir
+ ExternalProject_Get_Property(googletest source_dir)
+ set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-# Get GTest source and binary directories from CMake project
+ # Specify MainTest's link libraries
+ ExternalProject_Get_Property(googletest binary_dir)
+ set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+ # Create a libgtest target to be used as a dependency by test programs
+ add_library(libgtest IMPORTED STATIC GLOBAL)
+ add_dependencies(libgtest googletest)
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+ # Set libgtest properties
+ set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+ )
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+ message("CMake version equal or greater than 3.11.0")
+ include(FetchContent)
+
+ FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
+
+ SET(INSTALL_GTEST OFF) # Disable installation of googletest
+ FetchContent_MakeAvailable(googletest)
+
+endif()
+
function(wolf_add_gtest target)
add_executable(${target} ${ARGN})
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} libgtest)
-
- #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
+ target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+ else()
+ target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
+ endif()
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
index 2a246f3a1292c1b96a74ffeecb7c1ec65bc66471..ac09e66fcc1747aee3e28ecec1e47867f4d3a3c4 100644
--- a/test/gtest_processor_point_feet_nomove.cpp
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -111,11 +111,11 @@ class PointFeetCaptures : public testing::Test
KF0_ = problem_->getTrajectory()->getLastFrame();
// Simulate captures with 4 point feet
- std::unordered_map<int, Vector3d> kin_incontact_A({
- {1, Vector3d::Zero()},
- {2, Vector3d::Zero()},
- {3, Vector3d::Zero()},
- {4, Vector3d::Zero()}
+ std::unordered_map<int, Vector7d> kin_incontact_A({
+ {1, Vector7d::Zero()},
+ {2, Vector7d::Zero()},
+ {3, Vector7d::Zero()},
+ {4, Vector7d::Zero()}
});
CPF0_ = std::make_shared<CapturePointFeetNomove>(0.0, sen_pfnm_, kin_incontact_A);
@@ -124,18 +124,18 @@ class PointFeetCaptures : public testing::Test
CPF2_ = std::make_shared<CapturePointFeetNomove>(2.0, sen_pfnm_, kin_incontact_A);
// contact of the 2nd foot lost
- std::unordered_map<int, Eigen::Vector3d> kin_incontact_B({
- {1, Vector3d::Ones()},
- {3, Vector3d::Ones()},
- {4, Vector3d::Ones()}
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_B({
+ {1, Vector7d::Ones()},
+ {3, Vector7d::Ones()},
+ {4, Vector7d::Ones()}
});
CPF3_ = std::make_shared<CapturePointFeetNomove>(3.0, sen_pfnm_, kin_incontact_B);
CPF4_ = std::make_shared<CapturePointFeetNomove>(4.0, sen_pfnm_, kin_incontact_B);
// contact of the 3rd foot lost
- std::unordered_map<int, Eigen::Vector3d> kin_incontact_C({
- {1, 2*Vector3d::Ones()},
- {4, 2*Vector3d::Ones()}
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_C({
+ {1, 2*Vector7d::Ones()},
+ {4, 2*Vector7d::Ones()}
});
CPF5_ = std::make_shared<CapturePointFeetNomove>(5.0, sen_pfnm_, kin_incontact_C);
CPF6_ = std::make_shared<CapturePointFeetNomove>(6.0, sen_pfnm_, kin_incontact_C);