diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp index 127d90dd798e6556354286fa9b0a4ac8d161d21f..d1a966b5e51ca1e6adddd9831b79316ae46b08af 100644 --- a/demos/mcapi_povcdl_estimation.cpp +++ b/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 diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml index 1cc6341d84351a82037ed1642d83f17da20e282d..57fee2bfb82416dd5c5ae217d9f61a50fb83291d 100644 --- a/demos/mcapi_povcdl_estimation.yaml +++ b/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"