Rename demo executables

CMakeLists.txt

@@ -79,19 +79,15 @@ ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-# option(BUILD_EXAMPLES "Build examples" OFF)
 set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
 
 # Does this has any other interest
 # but for the examples ?
 # yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
+IF(BUILD_DEMOS OR BUILD_TESTS)
   string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
   set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-message("UPPER_NAME ${UPPER_NAME}")
+ENDIF(BUILD_DEMOS OR BUILD_TESTS)
 
 
 #find dependencies.
@@ -212,7 +208,6 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
 # TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfimu_INCLUDE_DIRS)
 
 IF(BUILD_DEMOS)
-  #Build examples
   MESSAGE("Building demos.")
   ADD_SUBDIRECTORY(demos)
 ENDIF(BUILD_DEMOS)

demos/CMakeLists.txt

@@ -45,8 +45,8 @@ target_link_libraries(solo_real_povcdl_estimation
     z
 )
 
-add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
-target_link_libraries(solo_real_pov_estimation 
+add_executable(solo_imu_kine solo_imu_kine.cpp)
+target_link_libraries(solo_imu_kine 
     mcapi_utils
     ${wolfcore_LIBRARIES}
     ${wolfimu_LIBRARIES}
@@ -56,8 +56,19 @@ target_link_libraries(solo_real_pov_estimation
     z
 )
 
-add_executable(solo_mocap_imu solo_mocap_imu.cpp)
-target_link_libraries(solo_mocap_imu 
+add_executable(solo_kine_mocap solo_kine_mocap.cpp)
+target_link_libraries(solo_kine_mocap 
+    mcapi_utils
+    ${wolfcore_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${pinocchio_LIBRARIES}    
+    /usr/local/lib/libcnpy.so
+    z
+)
+
+add_executable(solo_imu_mocap solo_imu_mocap.cpp)
+target_link_libraries(solo_imu_mocap 
     ${wolfcore_LIBRARIES}
     ${wolfimu_LIBRARIES}
     ${PLUGIN_NAME}

demos/solo_real_pov_estimation.cpp → demos/solo_imu_kine.cpp

@@ -75,15 +75,17 @@ 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 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>();
     
@@ -106,12 +108,16 @@ int main (int argc, char **argv) {
     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> 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());
+
+
+    // 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>();
@@ -125,8 +131,6 @@ 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));
@@ -185,12 +189,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;
@@ -216,15 +221,15 @@ int main (int argc, char **argv) {
     //////////////////////
     // 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
+    // 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
 
@@ -299,14 +304,6 @@ int main (int argc, char **argv) {
     // 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

demos/solo_mocap_imu.cpp → demos/solo_imu_mocap.cpp

@@ -246,7 +246,11 @@ int main (int argc, char **argv) {
     double* extr_mocap_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; 
@@ -256,35 +260,44 @@ int main (int argc, char **argv) {
     for (unsigned int i=0; i < N; i++){
         TimeStamp ts(t_arr[i]);
 
-        ////////////////////////////////////
-        ////////////////////////////////////
+        ////////////////
+        // PROCESS MOCAP
+        ////////////////
         // 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
 
+        // 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 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)
+
         // 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, acc_gyr_cov);
         CImu->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();
+        //////////////
+        //////////////
+
 
         // solve every new KF
         if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
@@ -294,16 +307,43 @@ int main (int argc, char **argv) {
 
             // recover covariances at this point
             auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
-            
-            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();
+            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);
+
+            CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu); 
             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(); 
 
+            // 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);
 
             // std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
             // std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
@@ -395,7 +435,11 @@ 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];
@@ -444,7 +488,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
@@ -514,8 +562,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");