Use frame ids as it apllies both the joint and frames (more general)

demos/mcapi_povcdl_estimation.cpp

@@ -11,6 +11,7 @@
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
 #include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 
 // MCAPI
 #include <curves/fwd.h>
@@ -254,16 +255,9 @@ int main (int argc, char **argv) {
 
         // update and retrieve kinematics
         forwardKinematics(model,data,q);
-        // auto oTll = data.oTf[left_leg_sole_idx];
-        // auto oTrl = data.oTf[right_leg_sole_idx];
-        std::string left_force_sensor_joint_name  = "leg_left_6_joint";
-        std::string right_force_sensor_joint_name = "leg_right_6_joint";
-        int left_force_sensor_joint_id = model.getJointId(left_force_sensor_joint_name);
-        int right_force_sensor_joint_id = model.getJointId(right_force_sensor_joint_name);
-        auto oTll = data.oMi[left_force_sensor_joint_id];
-        auto oTrl = data.oMi[right_force_sensor_joint_id];
-        // std::cout << "oTll.translation(): " << oTll.translation().transpose() << std::endl;
-        // std::cout << "oTrl.translation(): " << oTrl.translation().transpose() << std::endl;
+        updateFramePlacements(model,data);
+        auto oTll = data.oMf[left_leg_sole_idx];
+        auto oTrl = data.oMf[right_leg_sole_idx];
         auto bMll = oTb.inverse() * oTll;
         auto bMrl = oTb.inverse() * oTrl;
         Vector3d b_p_ll = bMll.translation();                        // meas
@@ -272,23 +266,10 @@ int main (int argc, char **argv) {
         Vector4d b_qvec_rl = Quaterniond(bMrl.rotation()).coeffs();  // meas
 
         // get local feet wrenchs
-        Vector6d l_wrench_ll = contact_force_to_wrench(contacts, cforce_ll);  // meas
-        Vector6d l_wrench_rl = contact_force_to_wrench(contacts, cforce_rl);  // meas
-
-        // // TEST
-        // // turn these wrench to centroidal wrenches
-        // Force l_spf_ll(l_wrench_ll.head<3>(), l_wrench_ll.tail<3>());
-        // Force l_spf_rl(l_wrench_rl.head<3>(), l_wrench_rl.tail<3>());
-        // SE3 oTc(Matrix3d::Identity(), c);  // global frame pose
-        // SE3 cTll = oTc.inverse() * oTll;
-        // SE3 cTrl = oTc.inverse() * oTrl;
-        // Force c_spf_ll = cTll.act(l_spf_ll);
-        // Force c_spf_rl = cTrl.act(l_spf_rl);
-        // std::cout << "\n\n\ncTll\n" << cTll << std::endl; 
-        // std::cout << "cTrl\n" << cTrl << std::endl; 
-        // std::cout << "c_spf_ll\n" << c_spf_ll << std::endl; 
-        // std::cout << "c_spf_rl\n" << c_spf_rl << std::endl; 
-        // std::cout << "c_spf_ll+c_spf_rl\n" << c_spf_ll + c_spf_rl << std::endl; 
+        // Vector6d l_wrench_ll = contact_force_to_wrench(contacts, cforce_ll);  // meas
+        // Vector6d l_wrench_rl = contact_force_to_wrench(contacts, cforce_rl);  // meas
+        Vector6d l_wrench_ll = cforce_ll;  // if directly stored in cs traj
+        Vector6d l_wrench_rl = cforce_rl;  // if directly stored in cs traj
 
         //////////////////////////////////////////////
         // COM relative position and velocity measures

demos/mcapi_povcdl_estimation.yaml

@@ -35,7 +35,8 @@ bias_pbc_prior: [0,0,0]
 # files
 # contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/sinY_waist.cs"
 # contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/sinY_nowaist.cs"
-contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/sinY_nowaist_R3SO3.cs"
+# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/sinY_nowaist_R3SO3.cs"
+contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj_R3SO3.cs"
 
 # contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/data/multicontact-api-master-examples/examples/com_motion_above_feet_WB.cs"
 # contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/tests/centroidalkin/data/multicontact-api-master-examples/examples/walk_20cm_quasistatic_WB.cs"