Solved measurement simulation bugs

demos/mcapi_povcdl_estimation.cpp

@@ -91,7 +91,8 @@ int main (int argc, char **argv) {
     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();
+    // double max_t = q_traj.max();
+    double max_t = 0.25;  // only the beginning
 
     // initialize some vectors and fill them with dicretized data
     std::vector<double> t_arr;
@@ -104,7 +105,7 @@ int main (int argc, char **argv) {
     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){
+    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));
@@ -131,8 +132,10 @@ int main (int argc, char **argv) {
     // 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");
+    int left_leg_sole_idx = model.getFrameId("leg_left_sole_fix_joint");
+    int right_leg_sole_idx = model.getFrameId("leg_right_sole_fix_joint");
+    std::cout << "left_leg_sole_idx: " << left_leg_sole_idx << std::endl;
+    std::cout << "right_leg_sole_idx: " << right_leg_sole_idx << std::endl;
     std::vector<Vector3d> contacts = contacts_from_footrect_center();
 
 
@@ -141,7 +144,7 @@ int main (int argc, char **argv) {
     // Groundtruth
     std::vector<Vector3d> p_ob_gtruth_v; 
     std::vector<Vector4d> q_ob_gtruth_v; 
-    // std::vector<Vector3d> v_wb_gtruth_v; 
+    std::vector<Vector3d> v_ob_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; 
@@ -166,13 +169,13 @@ int main (int argc, char **argv) {
          * Groundtruth to recover (in world frame, vels/inertial frame):
          * b position                                  --> OK
          * b orientation (quat)                        --> OK
-         * b velocity: linear classical one -> careful
+         * b velocity: linear classical one            --> OK
          * CoM posi                                    --> OK
          * CoM vel                                     
          * Angular momentum         
          * 
          * Measures to recover (in local frames):
-         * IMU readings                                --> ALMOST OK
+         * IMU readings                                --> OK
          * Kinematics readings                         --> OK
          * Wrench readings                             --> OK
          * CoM relative position/vel                   --> OK
@@ -185,7 +188,7 @@ int main (int argc, char **argv) {
         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 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];
@@ -199,7 +202,7 @@ int main (int argc, char **argv) {
         // global frame pose
         SE3 oMb(Quaterniond(quat_wb).toRotationMatrix(), p_wb);
         // body vel is expressed in body frame
-        // Vector3d w_v_wb =  oMb.rotation() * dq.head<3>();  // NOT USED
+        Vector3d w_v_wb =  oMb.rotation() * dq.head<3>();  // NOT USED
         // com velocity is already expressed in world frame
         // Vector3d w_v_wc = dc;                              // TO STORE
         // Lc ??
@@ -218,8 +221,14 @@ int main (int argc, char **argv) {
 
         // update and retrieve kinematics
         forwardKinematics(model,data,q);
-        auto oMll = data.oMf[ll_sole_idx];
-        auto oMrl = data.oMf[rl_sole_idx];
+        // auto oMll = data.oMf[left_leg_sole_idx];
+        // auto oMrl = data.oMf[right_leg_sole_idx];
+        auto frame_ll_sole_idx = model.frames[left_leg_sole_idx];
+        auto oMll = data.oMi[frame_ll_sole_idx.parent].act(frame_ll_sole_idx.placement);
+        auto frame_rl_sole_idx = model.frames[right_leg_sole_idx];
+        auto oMrl = data.oMi[frame_rl_sole_idx.parent].act(frame_rl_sole_idx.placement);
+        std::cout << "oMll.translation(): " << oMll.translation().transpose() << std::endl;
+        std::cout << "oMrl.translation(): " << oMrl.translation().transpose() << std::endl;
         auto bMll = oMb.inverse() * oMll;
         auto bMrl = oMb.inverse() * oMrl;
         Vector3d b_p_ll = bMll.translation();                        // meas
@@ -260,7 +269,7 @@ int main (int argc, char **argv) {
 
         p_ob_gtruth_v.push_back(p_wb); 
         q_ob_gtruth_v.push_back(quat_wb); 
-        // v_wb_gtruth_v; 
+        v_ob_gtruth_v.push_back(w_v_wb); 
         // Lc_gtruth_v; 
 
         // Measurements
@@ -343,7 +352,7 @@ int main (int argc, char **argv) {
     // - 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_ob_gtruth_v[0], q_ob_gtruth_v[0], ZERO3, ZERO3, ZERO3, ZERO3;  // TODO: origin vel shoulde not be zero  but v_wb_gtruth_v[0]
+    x_origin << p_ob_gtruth_v[0], q_ob_gtruth_v[0], v_ob_gtruth_v[0], 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
@@ -389,7 +398,7 @@ int main (int argc, char **argv) {
 
         // 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>();
+        Vector6d tau_meas; tau_meas << wrench_ll_meas_v[i].tail<3>(), wrench_rl_meas_v[i].tail<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];
 
@@ -405,13 +414,24 @@ int main (int argc, char **argv) {
         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();
+        std::cout << "\nw_b_meas_v[i]:     " << w_b_meas_v[i].transpose() << std::endl;
+        std::cout << "Lq_meas_v[i]:        " << Lq_meas_v[i].transpose() << std::endl;
+        std::cout << "Iq_meas_v[i]\n" << Iq_meas_v[i] << std::endl;
+        std::cout << "wrench_ll_meas_v[i]: " << wrench_ll_meas_v[i].transpose() << std::endl;
+        std::cout << "wrench_rl_meas_v[i]: " << wrench_rl_meas_v[i].transpose() << std::endl;
+        std::cout << "p_bc_meas_v[i]:      " << p_bc_meas_v[i].transpose() << std::endl;
+        std::cout << "p_bll_meas_v[i]:     " << p_bll_meas_v[i].transpose() << std::endl;
+        std::cout << "p_brl_meas_v[i]:     " << p_brl_meas_v[i].transpose() << std::endl;
+        std::cout << "q_bll_meas_v[i]:     " << q_bll_meas_v[i].transpose() << std::endl;
+        std::cout << "q_brl_meas_v[i]:     " << q_brl_meas_v[i].transpose() << std::endl;
+        
+
+        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();
 
         /////////////////////////////////////////////
         // Manually create an Odom3d factor when a new KF is detected (will be put in a processorLegOdom3d)
@@ -455,9 +475,14 @@ int main (int argc, char **argv) {
     ///////////////////////////////////////////
     /////////// SOLVING AND PRINTING //////////
     ///////////////////////////////////////////
-    problem->print(4,true,true,true);
     std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+    problem->print(4,true,true,true);
     std::cout << report << std::endl;
 
+    std::cout << "Ground truth end: " << std::endl;
+    std::cout << p_ob_gtruth_v[p_ob_gtruth_v.size()-1].transpose() << std::endl;
+    std::cout << q_ob_gtruth_v[q_ob_gtruth_v.size()-1].transpose() << std::endl;
+    std::cout << v_ob_gtruth_v[v_ob_gtruth_v.size()-1].transpose() << std::endl;
+
 
 }
\ No newline at end of file

demos/mcapi_utils.cpp

@@ -28,11 +28,10 @@ Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contac
     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;
+    Vector3d l_tau_l = contacts[0].cross(wRl.inverse() * w_cforces.segment<3>(0))
+                     + contacts[1].cross(wRl.inverse() * w_cforces.segment<3>(3))
+                     + contacts[2].cross(wRl.inverse() * w_cforces.segment<3>(6))
+                     + contacts[3].cross(wRl.inverse() * w_cforces.segment<3>(9));
     Matrix<double, 6, 1> l_wrench; l_wrench << lf, l_tau_l;
     return l_wrench;
 }