Completed measurements for Lc and bIq, still not converging

demos/mcapi_povcdl_estimation.cpp

@@ -11,6 +11,7 @@
 #include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/crba.hpp"
 
 // MCAPI
 #include <curves/fwd.h>
@@ -138,8 +139,8 @@ int main (int argc, char **argv) {
     ///////////////////////////////////////////////
     // 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> p_ob_gtruth_v; 
+    std::vector<Vector4d> q_ob_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
@@ -195,27 +196,25 @@ int main (int argc, char **argv) {
         // 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);
+        // body vel is expressed in body frame
+        // 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 ??
 
         ////////////////////////////////////
         ////////////////////////////////////
         // Get measurements
 
         // IMU readings
-        // Vector3d b_v_b = dq.head<3>(); 
+        Vector3d b_vsp_b = dq.head<3>();  // vsp: spatial linear velocity
         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
+        // 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 = computeAccFromSpatial(ddq.head<3>(), b_w_b, b_vsp_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 - oMb.inverse().rotation() * gravity();  // meas
 
         // update and retrieve kinematics
         forwardKinematics(model,data,q);
@@ -234,23 +233,33 @@ int main (int argc, char **argv) {
 
         //////////////////////////////////////////////
         // COM relative position and velocity measures
-        Vector3d b_p_bc =  oMb.inverse().act(c);  // meas
+        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  (???)
+        // 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
 
-        // Iq, Lq
-        computeCentroidalMap(model,data,q);
+        /////////////////////////
+        // bIomg, Lcm
+        MatrixXd b_Mc = crba(model, data, q);         // mass matrix at b frame expressed in b frame
+        MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>();   // inertia matrix at b frame expressed in b frame 
+        SE3 bTc (oMb.rotation().transpose(), b_p_bc);
+        // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc 
+        MatrixXd c_I_c = bTc.toActionMatrix().transpose() * b_I_b * bTc.toActionMatrix();
+        Matrix3d b_Iw  = bTc.rotation() * c_I_c.bottomRightCorner<3,3>() * bTc.rotation().transpose();  // meas
+        // 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();
 
         /////////////////////////
         // Agreggate everything in vectors for easier WOLF consumption
 
-        p_wb_gtruth_v.push_back(p_wb); 
-        q_wb_gtruth_v.push_back(quat_wb); 
+        p_ob_gtruth_v.push_back(p_wb); 
+        q_ob_gtruth_v.push_back(quat_wb); 
         // v_wb_gtruth_v; 
         // Lc_gtruth_v; 
 
@@ -265,8 +274,8 @@ int main (int argc, char **argv) {
         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
+        Lq_meas_v.push_back(b_Lc_gesti);
+        Iq_meas_v.push_back(b_Iw);
     }
 
     /////////////////////////
@@ -315,7 +324,7 @@ int main (int argc, char **argv) {
     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->time_tolerance = 0.025;
     params_sen_ft->unmeasured_perturbation_std = 1e-4;
     params_sen_ft->max_time_span = 1000;
     params_sen_ft->max_buff_length = 500;
@@ -334,7 +343,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_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]
+    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]
     MatrixXd P_origin = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
     FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
     // Prior pose factor
@@ -363,12 +372,10 @@ int main (int argc, char **argv) {
     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]);
+    SE3 oMb_prev(Quaterniond(q_ob_gtruth_v[0]).toRotationMatrix(), p_ob_gtruth_v[0]);
     FrameBasePtr KF_prev = KF1;
     for (int i=1; i < t_arr.size(); i++){
         TimeStamp ts(t_arr[i]);
@@ -410,7 +417,7 @@ int main (int argc, char **argv) {
         // 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 oMb_now(Quaterniond(q_ob_gtruth_v[i]).toRotationMatrix(), p_ob_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();

demos/mcapi_utils.cpp

@@ -2,7 +2,7 @@
 
 #include "mcapi_utils.h"
 
-Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
 {
     return ddr + w.cross(dr);
 }

demos/mcapi_utils.h

@@ -10,7 +10,7 @@ using namespace Eigen;
  * \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);
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
 
 
 /**

include/bodydynamics/factor/factor_inertial_kinematics.h

@@ -33,12 +33,13 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
         }
 
        /*
+        Factor state blocks:
         _pb: W_p_WB -> base position in world frame
         _qb: W_q_B  -> base orientation in world frame
         _vb: W_v_WB -> base velocity in world frame
         _c: W_p_WC -> COM position in world frame
         _cd: W_v_WC -> COM velocity in world frame
-        _Lc: W_Lc   -> angular momentum at the COM in world frame
+        _Lc: W_Lc   -> angular momentum at the COM in body frame
         _bp: B_b_BC -> bias on relative body->COM position kinematic measurement
         _baw: 6d Imu bias containing [acc bias, gyro bias]
         */
@@ -124,8 +125,8 @@ bool FactorInertialKinematics::operator () (
 
     err_c = pBC_m - (qb.inverse()*(c - pb) + bp);
     err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.inverse()*(cd - vb);
-    // err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() -  qb.inverse()*Lc;
-    err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.inverse()*Lc;   // w_m is actually used to compute BLcm beforehand
+    err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() -  qb.inverse()*Lc;
+    // err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.inverse()*Lc;   // w_m is actually used to compute BLcm beforehand
 
     res = getMeasurementSquareRootInformationUpper() * err;
 

test/gtest_processor_force_torque_preint.cpp

@@ -311,6 +311,7 @@ public:
         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);
+        // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
         FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic());   
  
 }