diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42d62a42e5e77ccfebc1dab94a4a2082ea66f5a6..d6e7e3183f583d1dd265b8132b994e399e6164ba 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,6 +33,7 @@ CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") # necessary for files using boost
elseif(COMPILER_SUPPORTS_CXX0X)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
@@ -47,8 +48,8 @@ if(UNIX)
endif(UNIX)
-#OPTION(BUILD_DOC "Build Documentation" OFF)
OPTION(BUILD_TESTS "Build Unit tests" ON)
+OPTION(BUILD_DEMOS "Build demos in demos folder, requires multicontact-api and pinocchio packages" OFF)
#############
## Testing ##
#############
@@ -112,8 +113,6 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
include_directories("include")
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b0e944a0b40f6145f769b297419dd5da2e2d784d
--- /dev/null
+++ b/demos/CMakeLists.txt
@@ -0,0 +1,49 @@
+
+find_package(PkgConfig REQUIRED)
+
+pkg_check_modules(MCAPI REQUIRED multicontact-api)
+pkg_check_modules(PINOCCHIO REQUIRED pinocchio)
+
+# Variables to be used
+# message("\n\nMCAPI")
+# message("\n${MCAPI_INCLUDE_DIRS}")
+# message("\n${MCAPI_LIBRARY_DIRS}")
+# message("\n${MCAPI_LIBRARIES}")
+# message("\n\nPINOCCHIO")
+message("\n${PINOCCHIO_INCLUDE_DIRS}")
+# message("\n${PINOCCHIO_LIBRARY_DIRS}")
+# message("\n${PINOCCHIO_LIBRARIES}")
+message("\n${PINOCCHIO_CFLAGS_OTHER}")
+
+# message("${wolf_LIBRARIES}")
+# message("${wolfimu_LIBRARIES}")
+# message("${PLUGIN_NAME}")
+
+# message("\n\n${CMAKE_CXX_FLAGS}")
+
+# SYSTEM disables warnings from library headers
+include_directories(
+ SYSTEM ${MCAPI_INCLUDE_DIRS}
+ SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
+)
+
+link_directories(
+ ${MCAPI_LIBRARY_DIRS}
+ ${PINOCCHIO_LIBRARY_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)
+target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER}) # ??
+
+
+
+target_link_libraries(mcapi_povcdl_estimation
+ ${wolf_LIBRARIES}
+ ${wolfimu_LIBRARIES}
+ ${PLUGIN_NAME}
+ ${MCAPI_LIBRARIES}
+ ${PINOCCHIO_LIBRARIES}
+)
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..799eac9c059c46ff94ed86d17a202d92735c31ae
--- /dev/null
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -0,0 +1,456 @@
+// debug
+#include <iostream>
+
+// #define BOOST_TEST_MODULE StatsTests
+// #include <boost/utility/binary.hpp>
+
+#include "Eigen/Dense"
+
+#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"
+
+// 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/ceres_manager.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/factor/factor_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/processor/processor_inertial_kinematics.h"
+#include "bodydynamics/processor/processor_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;
+
+const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
+const Eigen::Vector3d ONES3 = Eigen::Vector3d::Ones();
+
+const Eigen::Vector3d BIAS_PBC_SMAL = {0.01, 0.02, 0.03};
+
+
+int main (int argc, char **argv) {
+ std::cout << "HELLO MULTICONTACT" << std::endl;
+ ContactSequence cs;
+ cs.loadFromBinary("/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/multicontact-api-master-examples/examples/com_motion_above_feet_WB.cs");
+ std::cout << "cs.size(): " << cs.size() << std::endl;
+
+
+ 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 cw_traj = cs.concatenateWrenchTrajectories();
+ auto L_traj = cs.concatenateLtrajectories();
+ std::vector<std::string> ee_in_contact = cs.getAllEffectorsInContact();
+ std::string left_leg_name = ee_in_contact[0];
+ std::string right_leg_name = ee_in_contact[1];
+ auto ll_contactf_traj = cs.concatenateContactForceTrajectories(left_leg_name);
+ auto rl_contactf_traj = cs.concatenateContactForceTrajectories(right_leg_name);
+
+ std::cout << "q_traj.dim(): " << q_traj.dim() << std::endl;
+ double dt = 1e-2; // chosen data sampling
+ double min_t = q_traj.min();
+ double mac_t = q_traj.max();
+
+ // initialize some vectors and fill them with dicretized data
+ std::vector<double> t_arr;
+ std::vector<VectorXd> q_traj_arr;
+ std::vector<VectorXd> dq_traj_arr;
+ std::vector<VectorXd> ddq_traj_arr;
+ std::vector<VectorXd> c_traj_arr;
+ std::vector<VectorXd> dc_traj_arr;
+ std::vector<VectorXd> cw_traj_arr;
+ std::vector<VectorXd> L_traj_arr;
+ std::vector<VectorXd> ll_contactf_traj_arr;
+ std::vector<VectorXd> rl_contactf_traj_arr;
+ for (double t=min_t; t <= mac_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));
+ cw_traj_arr.push_back(cw_traj(t));
+ L_traj_arr.push_back(L_traj(t));
+ ll_contactf_traj_arr.push_back(ll_contactf_traj(t));
+ rl_contactf_traj_arr.push_back(rl_contactf_traj(t));
+ }
+
+ std::cout << "AFTER FILLING VECTORS with traj" << std::endl;
+
+ // Creating measurements from simulated trajectory
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel("/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf", JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ // Create data required by the algorithms
+ Data data(model);
+ // Sample a random configuration
+ // Eigen::VectorXd q = randomConfiguration(model);
+ // std::cout << "q: " << q.transpose() << std::endl;
+ // // Perform the forward kinematics over the kinematic tree
+ // forwardKinematics(model,data,q);
+ int ll_sole_idx = model.getFrameId("leg_left_sole_fix_joint");
+ int rl_sole_idx = model.getFrameId("leg_right_sole_fix_joint");
+ std::vector<Vector3d> contacts = contacts_from_footrect_center();
+
+
+ ///////////////////////////////////////////////
+ // Create some vectors to aggregate the groundtruth and measurements
+ // Groundtruth
+ std::vector<Vector3d> p_wb_gtruth_v;
+ std::vector<Vector4d> q_wb_gtruth_v;
+ // std::vector<Vector3d> v_wb_gtruth_v;
+ // std::vector<VectorXd>& c_gtruth_v = c_traj_arr; // already have it directly
+ // std::vector<VectorXd>& cd_gtruth_v = dc_traj_arr; // already have it directly
+ // std::vector<Vector3d> Lc_gtruth_v;
+
+ // Measurements
+ std::vector<Vector3d> acc_b_meas_v;
+ std::vector<Vector3d> w_b_meas_v;
+ std::vector<Vector3d> p_bll_meas_v;
+ std::vector<Vector3d> p_brl_meas_v;
+ std::vector<Vector4d> q_bll_meas_v;
+ std::vector<Vector4d> q_brl_meas_v;
+ std::vector<Vector3d> p_bc_meas_v;
+ std::vector<Vector3d> v_bc_meas_v;
+ std::vector<Vector6d> wrench_ll_meas_v;
+ std::vector<Vector6d> wrench_rl_meas_v;
+ std::vector<Vector3d> Lq_meas_v;
+ std::vector<Matrix3d> Iq_meas_v;
+ // define vectors to aggregate ground truth and simulated data
+ for (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 -> careful
+ * CoM posi --> OK
+ * CoM vel
+ * Angular momentum
+ *
+ * Measures to recover (in local frames):
+ * IMU readings --> ALMOST OK
+ * Kinematics readings --> OK
+ * Wrench readings --> OK
+ * CoM relative position/vel --> OK
+ * Iq, Lq
+ *
+ **/
+
+ // 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]; // gtruth
+ VectorXd dc = dc_traj_arr[i]; // gtruth (??)
+ VectorXd cw = cw_traj_arr[i];
+ VectorXd L = L_traj_arr[i];
+ VectorXd cf_ll = ll_contactf_traj_arr[i];
+ VectorXd cf_rl = rl_contactf_traj_arr[i];
+
+ //////////////////////////////////
+ //////////////////////////////////
+ // Set ground truth variables
+ Vector3d p_wb = q.head<3>(); // gtruth
+ Vector4d quat_wb = q.segment<4>(3); // gtruth
+ // vb ??
+ // vc ??
+ // Lc ??
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+
+
+ // global frame pose
+ SE3 oMb(Quaterniond(quat_wb).toRotationMatrix(), p_wb);
+
+ ////////////////////////////////////
+ ////////////////////////////////////
+ // Get measurements
+
+ // IMU readings
+ // Vector3d b_v_b = dq.head<3>();
+ Vector3d b_w_b = dq.segment<3>(3); // meas
+ Vector3d w_acc = ddq.head<3>(); // TODO, almost same as classical acceleration
+ // Vector3d w_acc = acc_from_spatial(ddq.head<3>(), b_w_b, b_v_b); // NOT THAT -> dr?
+ Vector3d b_proper_acc = oMb.inverse().rotation() * (w_acc - gravity()); // meas
+
+ // update and retrieve kinematics
+ forwardKinematics(model,data,q);
+ auto oMll = data.oMf[ll_sole_idx];
+ auto oMrl = data.oMf[rl_sole_idx];
+ auto bMll = oMb.inverse() * oMll;
+ auto bMrl = oMb.inverse() * oMrl;
+ Vector3d b_p_ll = bMll.translation(); // meas
+ Vector4d b_qvec_ll = Quaterniond(bMll.rotation()).coeffs(); // meas
+ Vector3d b_p_rl = bMrl.translation(); // meas
+ Vector4d b_qvec_rl = Quaterniond(bMrl.rotation()).coeffs(); // meas
+
+ // get local feet wrenchs
+ Vector6d l_wrench_ll = contact_force_to_wrench(contacts, cf_ll, oMll.rotation()); // meas
+ Vector6d l_wrench_rl = contact_force_to_wrench(contacts, cf_rl, oMrl.rotation()); // meas
+
+ //////////////////////////////////////////////
+ // COM relative position and velocity measures
+ Vector3d b_p_bc = oMb.inverse().act(c); // meas
+
+ 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;
+ centerOfMass(model, data, q_static, dq_static);
+ Vector3d b_v_bc = data.vcom[0]; // meas (???)
+
+ // Iq, Lq
+ computeCentroidalMap(model,data,q);
+
+ /////////////////////////
+ // Agreggate everything in vectors for easier WOLF consumption
+
+ p_wb_gtruth_v.push_back(p_wb);
+ q_wb_gtruth_v.push_back(quat_wb);
+ // v_wb_gtruth_v;
+ // Lc_gtruth_v;
+
+ // Measurements
+ acc_b_meas_v.push_back(b_proper_acc);
+ w_b_meas_v.push_back(b_w_b);
+ p_bll_meas_v.push_back(b_p_ll);
+ p_brl_meas_v.push_back(b_p_rl);
+ q_bll_meas_v.push_back(b_qvec_ll);
+ q_brl_meas_v.push_back(b_qvec_rl);
+ p_bc_meas_v.push_back(b_p_bc);
+ v_bc_meas_v.push_back(b_v_bc);
+ wrench_ll_meas_v.push_back(l_wrench_ll);
+ wrench_rl_meas_v.push_back(l_wrench_rl);
+ Lq_meas_v.push_back(ZERO3); // TODO
+ Iq_meas_v.push_back(Eigen::Matrix3d::Identity()); // TODO
+ }
+
+ /////////////////////////
+ // 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
+ ceres::Solver::Options ceres_options;
+ CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR INERTIAL KINEMATICS
+ ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
+ intr_ik->pbc_noise_std = 0.1;
+ intr_ik->vbc_noise_std = 0.1;
+ 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);
+
+ ProcessorParamsInertialKinematicsPtr params_ik = std::make_shared<ProcessorParamsInertialKinematics>();
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->pb_rate_stdev = 1e-2;
+ 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->f_noise_std = 0.001;
+ intr_ft->tau_noise_std = 0.001;
+ intr_ft->mass = data.mass[0];
+ 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);
+ ProcessorParamsForceTorquePreintPtr params_sen_ft = std::make_shared<ProcessorParamsForceTorquePreint>();
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->time_tolerance = 0.25;
+ params_sen_ft->unmeasured_perturbation_std = 1e-4;
+ 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 = true;
+ params_sen_ft->voting_aux_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);
+
+
+ // 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 isMotion 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_wb_gtruth_v[0], q_wb_gtruth_v[0], ZERO3, ZERO3, ZERO3, ZERO3; // TODO: origin vel shoulde not be zero but v_wb_gtruth_v[0]
+ MatrixXd P_origin = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+ FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
+ // 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, KF1->getV());
+
+ // 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.05);
+ proc_imu->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);
+ problem->setPriorIsSet(true); // We handle the whole initialization manually here
+
+ std::cout << "BEFORE WOLF CAPTURES CREATION" << std::endl;
+
+ ///////////////////////////////////////////
+ // process measurements in sequential order
+ int traj_size = problem->getTrajectory()->getFrameList().size();
+ SE3 oMb_prev(Quaterniond(q_wb_gtruth_v[0]).toRotationMatrix(), p_wb_gtruth_v[0]);
+ FrameBasePtr KF_prev = KF1;
+ for (int i=1; i < t_arr.size(); i++){
+ TimeStamp ts(t_arr[i]);
+
+ // IMU
+ Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
+ Matrix6d acc_gyr_cov = 1e-3 * Matrix6d::Identity();
+
+ // Inertial kinematics
+ meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
+
+ // Force Torque
+ Vector6d f_meas; f_meas << wrench_ll_meas_v[i].head<3>(), wrench_rl_meas_v[i].head<3>();
+ Vector6d tau_meas; tau_meas << wrench_ll_meas_v[i].head<3>(), wrench_rl_meas_v[i].head<3>();
+ VectorXd kin_meas(14);
+ kin_meas << p_bll_meas_v[i], p_brl_meas_v[i], q_bll_meas_v[i], q_brl_meas_v[i];
+
+ VectorXd cap_ftp_data(32);
+ cap_ftp_data << f_meas, tau_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
+
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
+ Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
+
+ MatrixXd Qftp = Matrix<double, 15, 15>::Identity();
+ Qftp.block<6, 6>(0, 0) = cov_f;
+ Qftp.block<6, 6>(6, 6) = cov_tau;
+ Qftp.block<3, 3>(12, 12) = cov_pbc;
+
+
+ CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CImu2->process();
+ auto CIKin2 = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, Iq_meas_v[i], Lq_meas_v[i]);
+ CIKin2->process();
+ auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin2, CImu2, cap_ftp_data, Qftp);
+ CFTpreint2->process();
+
+ /////////////////////////////////////////////
+ // Manually create an Odom3d factor when a new KF is detected (will be put in a processorLegOdom3d)
+ if (problem->getTrajectory()->getFrameList().size() > traj_size){
+ FrameBasePtr KF_now = problem->getTrajectory()->getLastKeyFrame();
+ SE3 oMb_now(Quaterniond(q_wb_gtruth_v[i]).toRotationMatrix(), p_wb_gtruth_v[i]);
+ SE3 bprev_M_bnow = oMb_prev.inverse() * oMb_now;
+ Vector7d prev_pose_now; prev_pose_now << bprev_M_bnow.translation(), Quaterniond(bprev_M_bnow.rotation()).coeffs();
+ Eigen::Matrix6d rel_pose_cov = 1e-6 * Eigen::Matrix6d::Identity();
+
+ CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF_now, KF_now->getTimeStamp(), nullptr, prev_pose_now, rel_pose_cov);
+ FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_now, rel_pose_cov);
+ FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF_prev, nullptr, false);
+
+ oMb_prev = oMb_now;
+ KF_prev = KF_now;
+ traj_size = problem->getTrajectory()->getFrameList().size();
+ }
+
+ }
+
+ ////////////////////////////////////////////
+ // BIAS FACTORS
+ Vector6d bias_imu = ZERO6;
+ Vector3d bias_pbc = ZERO3;
+
+ // Add absolute factor on Imu biases to simulate a fix()
+ Eigen::Matrix6d Q_bi = 1e-8 * Eigen::Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic());
+
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
+ Eigen::Matrix3d Q_bp = 1e-1 * Eigen::Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
+ FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc, Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic());
+
+
+ ///////////////////////////////////////////
+ /////////// SOLVING AND PRINTING //////////
+ ///////////////////////////////////////////
+ problem->print(4,true,true,true);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+
+
+}
\ No newline at end of file
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5368b1c8ea879b1936bde516e4d79749f3290890
--- /dev/null
+++ b/demos/mcapi_utils.cpp
@@ -0,0 +1,38 @@
+
+
+#include "mcapi_utils.h"
+
+Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+ return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+ double lx = 0.1;
+ double ly = 0.65;
+ std::vector<Vector3d> contacts; contacts.resize(4);
+ contacts[0] = {-lx, -ly, 0};
+ contacts[1] = {-lx, ly, 0};
+ contacts[2] = { lx, -ly, 0};
+ contacts[3] = { lx, ly, 0};
+ return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& w_cforces,
+ const Matrix3d& wRl)
+{
+ Vector3d wf = w_cforces.segment<3>(0) + w_cforces.segment<3>(3) + w_cforces.segment<3>(6) + w_cforces.segment<3>(9);
+ Vector3d lf = wRl.inverse() * wf;
+ // torque at point l (center of the foot) in world frame
+ Vector3d w_tau_l = contacts[0].cross(w_cforces.segment<3>(0))
+ + contacts[1].cross(w_cforces.segment<3>(3))
+ + contacts[2].cross(w_cforces.segment<3>(6))
+ + contacts[3].cross(w_cforces.segment<3>(9));
+ Vector3d l_tau_l = wRl.inverse() * w_tau_l;
+ Matrix<double, 6, 1> l_wrench; l_wrench << lf, l_tau_l;
+ return l_wrench;
+}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc4dc1c95c046eb0aabccb5593e26de894ca693e
--- /dev/null
+++ b/demos/mcapi_utils.h
@@ -0,0 +1,39 @@
+#include "Eigen/Dense"
+
+using namespace Eigen;
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ *
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+*
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates.
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
+ const Matrix<double, 12, 1>& w_cforces,
+ const Matrix3d& wRl);
diff --git a/demos/processor_imu_mcapi_demo.yaml b/demos/processor_imu_mcapi_demo.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..12152d1e68d9ae38fb12b9e59a9e748734e1d9b3
--- /dev/null
+++ b/demos/processor_imu_mcapi_demo.yaml
@@ -0,0 +1,11 @@
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00000001
+keyframe_vote:
+ max_time_span: 1 # seconds
+ max_buff_length: 500 # motion deltas
+ dist_traveled: 20000.0 # meters
+ angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
+ voting_active: true
+ voting_aux_active: false
\ No newline at end of file