From f75edfffab4111aadc859489c57ec68211e1a207 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Fri, 3 Jul 2020 11:59:13 +0200
Subject: [PATCH] Use frame ids as it apllies both the joint and frames (more
general)
---
demos/mcapi_povcdl_estimation.cpp | 35 +++++++-----------------------
demos/mcapi_povcdl_estimation.yaml | 3 ++-
2 files changed, 10 insertions(+), 28 deletions(-)
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 127d90d..d1a966b 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 1cc6341..57fee2b 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"
--
GitLab