diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index ef96579b3859d1ad649b5ef290f290e1379a9163..ce0f3ecb7bf989be57688faae510ac3320114188 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -2,39 +2,12 @@
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") # necessary for files using boost
FIND_PACKAGE(pinocchio QUIET)
-FIND_PACKAGE(multicontact-api QUIET)
if (pinocchio_FOUND)
# SYSTEM disables warnings from library headers
include_directories(
SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
)
-
- add_library(mcapi_utils mcapi_utils.cpp)
-
- # add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
- # target_link_libraries(mcapi_povcdl_estimation
- # mcapi_utils
- # ${wolfcore_LIBRARIES}
- # ${wolfimu_LIBRARIES}
- # ${PLUGIN_NAME}
- # ${multicontact-api_LIBRARIES}
- # ${pinocchio_LIBRARIES}
- # )
- # target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
-
-
- # add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
- # target_link_libraries(mcapi_pov_estimation
- # mcapi_utils
- # ${wolfcore_LIBRARIES}
- # ${wolfimu_LIBRARIES}
- # ${PLUGIN_NAME}
- # ${multicontact-api_LIBRARIES}
- # ${pinocchio_LIBRARIES}
- # )
- # target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
-
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
mcapi_utils
diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
deleted file mode 100644
index b465845c1c6b6c75d596dff953f0fd287a975768..0000000000000000000000000000000000000000
--- a/demos/mcapi_pov_estimation.cpp
+++ /dev/null
@@ -1,752 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <iostream>
-// #include <random>
-#include <yaml-cpp/yaml.h>
-#include <Eigen/Dense>
-#include <ctime>
-#include "eigenmvn.h"
-
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/joint-configuration.hpp"
-#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/center-of-mass.hpp"
-#include "pinocchio/algorithm/centroidal.hpp"
-#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-
-// MCAPI
-#include <curves/fwd.h>
-#include <curves/so3_linear.h>
-#include <curves/se3_curve.h>
-#include <curves/polynomial.h>
-#include <curves/bezier_curve.h>
-#include <curves/piecewise_curve.h>
-#include <curves/exact_cubic.h>
-#include <curves/cubic_hermite_spline.h>
-#include "multicontact-api/scenario/contact-sequence.hpp"
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/capture/capture_pose.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/processor/processor_odom_3d.h>
-#include <core/sensor/sensor_odom_3d.h>
-
-// IMU
-#include "imu/sensor/sensor_imu.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/capture/capture_leg_odom.h"
-#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-
-// UTILS
-#include "mcapi_utils.h"
-
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinocchio;
-using namespace multicontact_api::scenario;
-
-typedef pinocchio::SE3Tpl<double> SE3;
-typedef pinocchio::ForceTpl<double> Force;
-
-
-int main (int argc, char **argv) {
- // retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
- bool noisy_measurements = config["noisy_measurements"].as<bool>();
-
- // estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
- double std_odom3d_est = config["std_odom3d_est"].as<double>();
-
- // simulated sensor noises
- double std_acc_sim = config["std_acc_sim"].as<double>();
- double std_gyr_sim = config["std_gyr_sim"].as<double>();
- double std_pbc_sim = config["std_pbc_sim"].as<double>();
- double std_vbc_sim = config["std_vbc_sim"].as<double>();
- double std_f_sim = config["std_f_sim"].as<double>();
- double std_tau_sim = config["std_tau_sim"].as<double>();
- // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
-
- // priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
- Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
-
- double fz_thresh = config["fz_thresh"].as<double>();
- double scale_dist = config["scale_dist"].as<double>();
- bool base_dist_only = config["base_dist_only"].as<bool>();
- bool mass_dist = config["mass_dist"].as<bool>();
- bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
- bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
- bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
-
- std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
-
- ContactSequence cs;
- std::cout << "Loading file " << contact_sequence_file << std::endl;
- cs.loadFromBinary(contact_sequence_file);
-
- auto q_traj = cs.concatenateQtrajectories();
- auto dq_traj = cs.concatenateDQtrajectories();
- auto ddq_traj = cs.concatenateDDQtrajectories();
- auto c_traj = cs.concatenateCtrajectories();
- auto dc_traj = cs.concatenateDCtrajectories();
- auto L_traj = cs.concatenateLtrajectories();
- auto tau_traj = cs.concatenateTauTrajectories();
- std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
- unsigned int nbc = ee_names.size();
- std::vector<curves::piecewise_t> cforce_traj_lst;
- for (auto ee_name: ee_names){
- std::cout << ee_name << std::endl;
- auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
- cforce_traj_lst.push_back(cforce_traj);
- }
-
- if (min_t < 0){
- min_t = q_traj.min();
- }
- if (max_t < 0){
- max_t = q_traj.max();
- }
-
- // initialize some vectors and fill them with dicretized data
- std::vector<double> t_arr; // timestamps
- std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
- std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
- std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
- std::vector<VectorXd> c_traj_arr; // w_p_wc
- std::vector<VectorXd> dc_traj_arr; // w_v_wc
- std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
- std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
- int N = (max_t - min_t)/dt;
- int i_t = 0;
- 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));
- ddq_traj_arr.push_back(ddq_traj(t));
- c_traj_arr.push_back(c_traj(t));
- dc_traj_arr.push_back(dc_traj(t));
- L_traj_arr.push_back(L_traj(t));
- tau_traj_arr.push_back(tau_traj(t));
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
- }
- i_t++;
- }
- std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
-
- // Creating measurements from simulated trajectory
- // Load the urdf model
- Model model;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
- std::cout << "model name: " << model.name << std::endl;
- Data data(model);
-
- // distorted model (modified inertias)
- Model model_dist;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
-
- Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
- Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
-
- // distortion
- if (base_dist_only){
- if (!mass_dist){
- int root_joint_id = model.getJointId("root_joint");
- Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
- }
- }
- else{
- for (int i=0; i < model_dist.inertias.size(); i++){
- if (mass_dist){
- double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
- model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
- std::cout << "mass model " << model.inertias[i].mass() << std::endl;
- std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
- }
- else {
- Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
- }
- }
- }
- Data data_dist(model_dist);
-
- std::vector<int> ee_ids;
- std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
- ee_ids.push_back(model.getFrameId(ee_name));
- std::cout << ee_name << std::endl;
- }
- std::vector<Vector3d> contacts = contacts_from_footrect_center();
-
- ///////////////////////////////////////////////
- // Create some vectors to aggregate the groundtruth and measurements
- // Groundtruth
- std::vector<Vector3d> p_ob_gtr_v;
- std::vector<Vector4d> q_ob_gtr_v;
- std::vector<Vector3d> v_ob_gtr_v;
- // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
- // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
- // std::vector<Vector3d> L_traj_arr; // already have it directly
-
-
- // Measurements
- std::vector<Vector3d> acc_b_meas_v;
- std::vector<Vector3d> w_b_meas_v;
- std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
- std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
- std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
- std::vector<Vector3d> p_bc_meas_v;
- std::vector<Vector3d> v_bc_meas_v;
- std::vector<Vector3d> Lq_meas_v;
- std::vector<Matrix3d> Iq_meas_v;
- // define vectors to aggregate ground truth and simulated data
- for (unsigned int i = 0; i < q_traj_arr.size(); i++)
- {
- /**
- * Groundtruth to recover (in world frame, vels/inertial frame):
- * b position --> OK
- * b orientation (quat) --> OK
- * b velocity: linear classical one --> OK
- * CoM posi --> OK
- * CoM vel --> OK
- * Angular momentum --> OK
- *
- * Measures to recover (in local frames):
- * IMU readings --> OK
- * Kinematics readings --> OK
- * Wrench readings --> OK
- * CoM relative position/vel --> OK
- * Iq, Lq --> OK
- *
- **/
-
- // retrieve traj data
- VectorXd q = q_traj_arr[i];
- VectorXd dq = dq_traj_arr[i];
- VectorXd ddq = ddq_traj_arr[i];
- // Vector3d c = c_traj_arr[i]; // gtr
- // VectorXd dc = dc_traj_arr[i]; // gtr
- // VectorXd L = L_traj_arr[i]; // gtr
- VectorXd tau = tau_traj_arr[i];
-
- // std::cout << "q " << q.transpose() << std::endl;
- // std::cout << "dq " << dq.transpose() << std::endl;
- // std::cout << "ddq " << ddq.transpose() << std::endl;
-
- //////////////////////////////////
- //////////////////////////////////
- // Set ground truth variables
- Vector3d p_wb = q.head<3>(); // gtr
- Vector4d quat_wb = q.segment<4>(3); // gtr
- SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
- // body vel is expressed in body frame
- Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
- Vector3d b_w_b = dq.segment<3>(3); // meas
- Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
-
- Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
-
- ////////////////////////////////////
- ////////////////////////////////////
- // Get measurements
-
- // IMU readings
- // 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 = b_asp_b + b_w_b.cross(b_v_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 - oTb.rotation().transpose() * gravity(); // meas
-
- // update and retrieve kinematics
- forwardKinematics(model, data, q);
- updateFramePlacements(model, data);
-
- // compute all linear jacobians (only for quadruped)
- MatrixXd Jlinvel_all(12, model.nv); Jlinvel_all.setZero();
- for (int i_ee=0; i_ee < nbc; i_ee++){
- MatrixXd Jee(6, model.nv); Jee.setZero();
- computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- Jlinvel_all.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
- }
- // remove free flyer (not needed for force reconstruction)
- MatrixXd Jlinvel_noff = Jlinvel_all.rightCols(model.nv-6);
-
- // estimate forces from torques
- VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- VectorXd tau_joints = tau_cf.tail(model.nv-6) - tau; // remove measured joint torque (not on the free flyer)
- VectorXd forces = Jlinvel_noff.transpose().lu().solve(tau_joints);
-
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- auto oTl = data.oMf[ee_ids[i_ee]];
- auto bTl = oTb.inverse() * oTl;
- Vector3d b_p_l = bTl.translation(); // meas
- Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
- p_bl_meas_v[i_ee].push_back(b_p_l);
- q_bl_meas_v[i_ee].push_back(b_qvec_l);
- // TODO: Only for 6D wrench or 3D force
-
- // Compute either from torques or from given force and compare
- // std::cout << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
- // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]);
- wrench_meas_v[i_ee].push_back(forces.segment(3*i_ee, 3));
- }
-
-
- //////////////////////////////////////////////
- // COM relative position and velocity measures
- 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;
- // compute for both perfect and distorted models to get the bias
- centerOfMass(model, data, q_static, dq_static);
- centerOfMass(model_dist, data_dist, q_static, dq_static);
-
- // c =def w_p_wc
- // Vector3d b_p_bc = oTb.inverse().act(c); // meas
- Vector3d b_p_bc = data.com[0]; // meas
- Vector3d b_p_bc_dist = data_dist.com[0]; // meas
- Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
-
- // 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
- Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
- Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
-
- /////////////////////////
- // Centroidal inertia and gesticulation kinetic momentum
- Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
- Matrix3d bias_Iw = b_Iw_dist - b_Iw;
-
- // 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();
- Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
- Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
-
-
- // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
- // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
- // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
- // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
-
- /////////////////////////
- // Agreggate everything in vectors for easier WOLF consumption
- p_ob_gtr_v.push_back(p_wb);
- q_ob_gtr_v.push_back(quat_wb);
- v_ob_gtr_v.push_back(w_v_wb);
- bp_traj_arr.push_back(bias_pbc);
- // Lc_gtr_v;
-
- // Measurements
- acc_b_meas_v.push_back(b_proper_acc);
- w_b_meas_v.push_back(b_w_b);
- p_bc_meas_v.push_back(b_p_bc_dist);
- if (vbc_is_dist){
- v_bc_meas_v.push_back(b_v_bc_dist);
- }
- else {
- v_bc_meas_v.push_back(b_v_bc);
- }
- if (Lgest_is_dist){
- Lq_meas_v.push_back(b_Lc_gesti_dist);
- }
- else{
- Lq_meas_v.push_back(b_Lc_gesti);
- }
- if (Iw_is_dist){
- Iq_meas_v.push_back(b_Iw_dist);
- }
- else {
- Iq_meas_v.push_back(b_Iw);
- }
- }
-
- /////////////////////////
- // WOLF enters the scene
- // SETUP PROBLEM/SENSORS/PROCESSORS
- std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- ProblemPtr problem = Problem::create("POV", 3);
-
- // CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
- ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
-
- // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
- Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
- SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
- SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
- ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
- // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
- ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
- ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
- //////////////////////////////
-
- TimeStamp t0(t_arr[0]);
- VectorComposite x_origin("POV", {p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0]});
- VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
-
- proc_imu->setOrigin(KF1);
- proc_legodom->setOrigin(KF1);
-
-
- ///////////////////////////////////////////
- // process measurements in sequential order
- double ts_since_last_kf = 0;
-
- // Add gaussian noises
- std::time_t epoch = std::time(nullptr);
- int64_t seed = (int64_t)epoch;
- Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
-
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
- //////////////////////////////////////////
-
- unsigned int nb_kf = problem->getTrajectory()->size();
- for (unsigned int i=0; i < t_arr.size(); i++){
- // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
- TimeStamp ts(t_arr[i]); // works best with pyb trajectories
-
- // IMU
- Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
-
- if (noisy_measurements){
- acc_gyr_meas.head<3>() += noise_acc.samples(1);
- acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
- }
-
- Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
-
- ////////////////////
- /////////////////
-
- CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
- CImu->process();
-
-
- if (i > 0){
- // Leg odometry
- // 1) detect feet in contact
- std::vector<int> feet_int_contact;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
- // basic contact detection based on normal foot vel, things break if no foot is in contact
- // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
- if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
- feet_int_contact.push_back(i_ee);
- }
- }
- // 2) fill the leg odometry data matrix, depending on the contact dimension
- Eigen::MatrixXd data_legodom;
- if (dimc == 6){
- // cols organized as:
- // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
- data_legodom.resize(7,feet_int_contact.size()*2);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_int_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
- j++;
- }
- }
- else if (dimc == 3){
- // cols organized as:
- // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
- data_legodom.resize(3,feet_int_contact.size()*2 + 1);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_int_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
- j++;
- }
- data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
- }
- // std::cout << "data_legodom\n" << data_legodom << std::endl;
- // 3) process the data and its cov
- Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
- CLO->process();
- }
-
- // if new KF add
- // unsigned int new_kf_nb = problem->getTrajectory()->size();
- // if (new_kf_nb > nb_kf){
- // auto last_kf = problem->getTrajectory()->getLastFrame();
- // nb_kf = new_kf_nb;
- // // ADD ABSOLUTE FACTOR (GPS LIKE)
- // }
-
-
- ts_since_last_kf += dt;
- if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- // problem->print(4,true,true,true);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
- ts_since_last_kf = 0;
- }
-
- // Store current state estimation
- VectorComposite curr_state = problem->getState(ts);
- p_ob_fbk_v.push_back(curr_state['P']);
- q_ob_fbk_v.push_back(curr_state['O']);
- v_ob_fbk_v.push_back(curr_state['V']);
-
- }
-
- ////////////////////////////////////////////
- // BIAS FACTORS
- // Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
- FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
-
-
- ///////////////////////////////////////////
- //////////////// SOLVING //////////////////
- ///////////////////////////////////////////
- problem->print(4,true,true,true);
- if (solve_end){
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
- std::cout << report << std::endl;
- }
-
- //////////////////////////////////////
- //////////////////////////////////////
- // STORE DATA //
- //////////////////////////////////////
- //////////////////////////////////////
- std::fstream file_gtr;
- std::fstream file_est;
- std::fstream file_fbk;
- std::fstream file_kfs;
- std::fstream file_cov;
- file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
- file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
- file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
- file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
- file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
- std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
- file_est << header_est;
- file_fbk << header_est;
- std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz\n";
- file_kfs << header_kfs;
- file_gtr << header_kfs;
- std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz\n";
- file_cov << header_cov;
-
- VectorComposite state_est;
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_gtr_v[i](0) << ","
- << p_ob_gtr_v[i](1) << ","
- << p_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](0) << ","
- << q_ob_gtr_v[i](1) << ","
- << q_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](3) << ","
- << v_ob_gtr_v[i](0) << ","
- << v_ob_gtr_v[i](1) << ","
- << v_ob_gtr_v[i](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- state_est = problem->getState(t_arr[i]);
- file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << state_est['P'](0) << ","
- << state_est['P'](1) << ","
- << state_est['P'](2) << ","
- << state_est['O'](0) << ","
- << state_est['O'](1) << ","
- << state_est['O'](2) << ","
- << state_est['O'](3) << ","
- << state_est['V'](0) << ","
- << state_est['V'](1) << ","
- << state_est['V'](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_fbk_v[i](0) << ","
- << p_ob_fbk_v[i](1) << ","
- << p_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](0) << ","
- << q_ob_fbk_v[i](1) << ","
- << q_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](3) << ","
- << v_ob_fbk_v[i](0) << ","
- << v_ob_fbk_v[i](1) << ","
- << v_ob_fbk_v[i](2)
- << "\n";
- }
-
- VectorComposite kf_state;
- CaptureBasePtr cap_imu;
- VectorComposite bi_bias_est;
- auto frame_map = problem->getTrajectory()->getFrameMap();
- for (auto& elt : frame_map){
- auto kf = elt.second;
- kf_state = kf->getState();
- cap_imu = kf->getCaptureOf(sen_imu);
- bi_bias_est = cap_imu->getState();
-
- file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << kf_state['P'](0) << ","
- << kf_state['P'](1) << ","
- << kf_state['P'](2) << ","
- << kf_state['O'](0) << ","
- << kf_state['O'](1) << ","
- << kf_state['O'](2) << ","
- << kf_state['O'](3) << ","
- << kf_state['V'](0) << ","
- << kf_state['V'](1) << ","
- << kf_state['V'](2) << ","
- << bi_bias_est['I'](0) << ","
- << bi_bias_est['I'](1) << ","
- << bi_bias_est['I'](2) << ","
- << bi_bias_est['I'](3) << ","
- << bi_bias_est['I'](4) << ","
- << bi_bias_est['I'](5)
- << "\n";
-
- // compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
- problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
- problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
- problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
-
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
- file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << Qp(0,0) << ","
- << Qp(1,1) << ","
- << Qp(2,2) << ","
- << Qo(0,0) << ","
- << Qo(1,1) << ","
- << Qo(2,2) << ","
- << Qv(0,0) << ","
- << Qv(1,1) << ","
- << Qv(2,2) << ","
- << Qbi(0,0) << ","
- << Qbi(1,1) << ","
- << Qbi(2,2) << ","
- << Qbi(3,3) << ","
- << Qbi(4,4) << ","
- << Qbi(5,5)
- << "\n";
- }
-
- file_gtr.close();
- file_est.close();
- file_fbk.close();
- file_kfs.close();
- file_cov.close();
-
-
- // // Print factor energy
- // for (auto kf: problem->getTrajectory()->getFrameMap()){
- // for (auto cap: kf->getCaptureList()){
- // for (auto feat: cap->getFeatureList()){
- // for (auto fac: feat->getFactorList()){
- // if (fac->getType() == "FactorForceTorquePreint"){
- // std::cout << "Type: FactorForceTorquePreint" << std::endl;
- // auto fac_ftp = std::static_pointer_cast<FactorForceTorquePreint>(fac);
- // std::cout << "try the cost" << std::endl;
- // std::cout << fac_ftp->cost() << std::endl;
- // }
- // }
- // }
- // }
- // }
-
-
-}
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
deleted file mode 100644
index 0596592b213d5459d66db552ba3b7ef9eb670235..0000000000000000000000000000000000000000
--- a/demos/mcapi_povcdl_estimation.cpp
+++ /dev/null
@@ -1,970 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <iostream>
-// #include <random>
-#include <yaml-cpp/yaml.h>
-#include <Eigen/Dense>
-#include <ctime>
-#include "eigenmvn.h"
-
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/joint-configuration.hpp"
-#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/center-of-mass.hpp"
-#include "pinocchio/algorithm/centroidal.hpp"
-#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-
-// MCAPI
-#include <curves/fwd.h>
-#include <curves/so3_linear.h>
-#include <curves/se3_curve.h>
-#include <curves/polynomial.h>
-#include <curves/bezier_curve.h>
-#include <curves/piecewise_curve.h>
-#include <curves/exact_cubic.h>
-#include <curves/cubic_hermite_spline.h>
-#include "multicontact-api/scenario/contact-sequence.hpp"
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/capture/capture_pose.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/processor/processor_odom_3d.h>
-#include <core/sensor/sensor_odom_3d.h>
-
-// IMU
-#include "imu/sensor/sensor_imu.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/capture/capture_leg_odom.h"
-#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-
-// UTILS
-#include "mcapi_utils.h"
-
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinocchio;
-using namespace multicontact_api::scenario;
-
-typedef pinocchio::SE3Tpl<double> SE3;
-typedef pinocchio::ForceTpl<double> Force;
-
-
-int main (int argc, char **argv) {
- // retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/mcapi_povcdl_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
- double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
- bool noisy_measurements = config["noisy_measurements"].as<bool>();
-
- // estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
- double std_foot_nomove = config["std_foot_nomove"].as<double>();
-
- // simulated sensor noises
- double std_acc_sim = config["std_acc_sim"].as<double>();
- double std_gyr_sim = config["std_gyr_sim"].as<double>();
- double std_pbc_sim = config["std_pbc_sim"].as<double>();
- double std_vbc_sim = config["std_vbc_sim"].as<double>();
- double std_f_sim = config["std_f_sim"].as<double>();
- double std_tau_sim = config["std_tau_sim"].as<double>();
- // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
-
- // priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
- Eigen::Map<Vector3d> bias_pbc_prior(bias_pbc_prior_vec.data());
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
- Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
-
- double fz_thresh = config["fz_thresh"].as<double>();
- double scale_dist = config["scale_dist"].as<double>();
- bool base_dist_only = config["base_dist_only"].as<bool>();
- bool mass_dist = config["mass_dist"].as<bool>();
- bool vbc_is_dist = config["vbc_is_dist"].as<bool>();
- bool Iw_is_dist = config["Iw_is_dist"].as<bool>();
- bool Lgest_is_dist = config["Lgest_is_dist"].as<bool>();
-
- std::string contact_sequence_file = config["contact_sequence_file"].as<std::string>();
-
- ContactSequence cs;
- std::cout << "Loading file " << contact_sequence_file << std::endl;
- cs.loadFromBinary(contact_sequence_file);
-
- auto q_traj = cs.concatenateQtrajectories();
- auto dq_traj = cs.concatenateDQtrajectories();
- auto ddq_traj = cs.concatenateDDQtrajectories();
- auto c_traj = cs.concatenateCtrajectories();
- auto dc_traj = cs.concatenateDCtrajectories();
- auto L_traj = cs.concatenateLtrajectories();
- auto tau_traj = cs.concatenateTauTrajectories();
- std::vector<std::string> ee_names = cs.getAllEffectorsInContact();
- unsigned int nbc = ee_names.size();
- std::vector<curves::piecewise_t> cforce_traj_lst;
- for (auto ee_name: ee_names){
- std::cout << ee_name << std::endl;
- auto cforce_traj = cs.concatenateContactForceTrajectories(ee_name);
- cforce_traj_lst.push_back(cforce_traj);
- }
-
- if (min_t < 0){
- min_t = q_traj.min();
- }
- if (max_t < 0){
- max_t = q_traj.max();
- }
-
- // initialize some vectors and fill them with dicretized data
- std::vector<double> t_arr; // timestamps
- std::vector<VectorXd> q_traj_arr; // [p_ob, q_ob, qA]
- std::vector<VectorXd> dq_traj_arr; // [spvel_b, qA_dot]
- std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
- std::vector<VectorXd> c_traj_arr; // w_p_wc
- std::vector<VectorXd> dc_traj_arr; // w_v_wc
- std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> tau_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
- std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
- std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
- int N = (max_t - min_t)/dt;
- int i_t = 0;
- 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));
- ddq_traj_arr.push_back(ddq_traj(t));
- c_traj_arr.push_back(c_traj(t));
- dc_traj_arr.push_back(dc_traj(t));
- L_traj_arr.push_back(L_traj(t));
- tau_traj_arr.push_back(tau_traj(t));
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- cforce_traj_arr_lst[i_ee].push_back(cforce_traj_lst[i_ee](t));
- }
- i_t++;
- }
- std::cout << "Trajectory extracted, proceed to create sensor data" << std::endl;
-
- // Creating measurements from simulated trajectory
- // Load the urdf model
- Model model;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
- std::cout << "model name: " << model.name << std::endl;
- Data data(model);
-
- // distorted model (modified inertias)
- Model model_dist;
- pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
- Eigen::EigenMultivariateNormal<double> noise_mass = Eigen::EigenMultivariateNormal<double>(Vector1d::Zero(), Matrix1d::Identity(), false);
- Eigen::EigenMultivariateNormal<double> noise_lever = Eigen::EigenMultivariateNormal<double>(Vector3d::Zero(), Matrix3d::Identity(), false);
- if (base_dist_only){
- if (!mass_dist){
- int root_joint_id = model.getJointId("root_joint");
- Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
- }
- }
- else{
- for (int i=0; i < model_dist.inertias.size(); i++){
- if (mass_dist){
- double mass_dist = model.inertias[i].mass() + scale_dist * model.inertias[i].mass() * noise_mass.samples(1)(0,0);
- model_dist.inertias[i] = pinocchio::Inertia(mass_dist, model.inertias[i].lever(), model.inertias[i].inertia());
- std::cout << "mass model " << model.inertias[i].mass() << std::endl;
- std::cout << "mass model_dist " << model_dist.inertias[i].mass() << std::endl;
- }
- else {
- Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_lever.samples(1);
- model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
- }
- }
- }
- Data data_dist(model_dist);
-
- // Recover end effector frame ids
- std::vector<int> ee_ids;
- std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
- ee_ids.push_back(model.getFrameId(ee_name));
- std::cout << ee_name << std::endl;
- }
- // std::vector<Vector3d> contacts = contacts_from_footrect_center(); // for Humanoid only TODO
-
-
- ///////////////////////////////////////////////
- // Create some vectors to aggregate the groundtruth and measurements
- // Groundtruth
- std::vector<Vector3d> p_ob_gtr_v;
- std::vector<Vector4d> q_ob_gtr_v;
- std::vector<Vector3d> v_ob_gtr_v;
- // std::vector<VectorXd>& c_gtr_v = c_traj_arr; // already have it directly
- // std::vector<VectorXd>& cd_gtr_v = dc_traj_arr; // already have it directly
- // std::vector<Vector3d> L_traj_arr; // already have it directly
-
- // Measurements
- std::vector<Vector3d> acc_b_meas_v;
- std::vector<Vector3d> w_b_meas_v;
- std::vector<std::vector<Vector3d>> p_bl_meas_v(nbc);
- std::vector<std::vector<Vector4d>> q_bl_meas_v(nbc);
- std::vector<std::vector<VectorXd>> wrench_meas_v(nbc);
- std::vector<Vector3d> p_bc_meas_v;
- std::vector<Vector3d> v_bc_meas_v;
- std::vector<Vector3d> Lq_meas_v;
- std::vector<Matrix3d> Iq_meas_v;
- // define vectors to aggregate ground truth and simulated data
- for (unsigned int i = 0; i < q_traj_arr.size(); i++)
- {
- /**
- * Groundtruth to recover (in world frame, vels/inertial frame):
- * b position --> OK
- * b orientation (quat) --> OK
- * b velocity: linear classical one --> OK
- * CoM posi --> OK
- * CoM vel --> OK
- * Angular momentum --> OK
- *
- * Measures to recover (in local frames):
- * IMU readings --> OK
- * Kinematics readings --> OK
- * Wrench readings --> OK
- * CoM relative position/vel --> OK
- * Iq, Lq --> OK
- *
- **/
-
- // retrieve traj data
- VectorXd q = q_traj_arr[i];
- VectorXd dq = dq_traj_arr[i];
- VectorXd ddq = ddq_traj_arr[i];
- // Vector3d c = c_traj_arr[i]; // gtr
- // VectorXd dc = dc_traj_arr[i]; // gtr
- // VectorXd L = L_traj_arr[i]; // gtr
- VectorXd tau = tau_traj_arr[i];
-
- // std::cout << "q " << q.transpose() << std::endl;
- // std::cout << "dq " << dq.transpose() << std::endl;
- // std::cout << "ddq " << ddq.transpose() << std::endl;
-
- //////////////////////////////////
- //////////////////////////////////
- // Set ground truth variables
- Vector3d p_wb = q.head<3>(); // gtr
- Vector4d quat_wb = q.segment<4>(3); // gtr
- SE3 oTb(Quaterniond(quat_wb).toRotationMatrix(), p_wb); // global frame pose
- // body vel is expressed in body frame
- Vector3d b_v_b = dq.head<3>(); // vsp: spatial linear velocity
- Vector3d b_w_b = dq.segment<3>(3); // meas
- Vector3d b_asp_b = ddq.head<3>(); // body spatial acceleration in body plucker frame
-
- Vector3d w_v_wb = oTb.rotation() * b_v_b; // gtr
-
- ////////////////////////////////////
- ////////////////////////////////////
- // Get measurements
-
- // IMU readings
- // 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 = b_asp_b + b_w_b.cross(b_v_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 - oTb.rotation().transpose() * gravity(); // meas
-
- // update and retrieve kinematics
- forwardKinematics(model, data, q);
- updateFramePlacements(model, data);
-
- // compute all linear jacobians (only for quadruped)
- MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
- for (int i_ee=0; i_ee < nbc; i_ee++){
- MatrixXd Jee(6, model.nv); Jee.setZero();
- computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
- }
-
- // estimate forces from torques
- VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
-
- // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
- // Solve in Least square sense (3 ways):
- // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
- VectorXd forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
- // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
-
- Vector3d o_f_tot = Vector3d::Zero();
- std::cout << "" << std::endl;
- std::cout << "t: " << t_arr[i] << std::endl;
- // std::cout << "ddq: " << ddq.transpose() << std::endl;
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- auto oTl = data.oMf[ee_ids[i_ee]];
- auto bTl = oTb.inverse() * oTl;
- Vector3d b_p_l = bTl.translation(); // meas
- Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
- p_bl_meas_v[i_ee].push_back(b_p_l);
- q_bl_meas_v[i_ee].push_back(b_qvec_l);
- // TODO: Only for 6D wrench or 3D force
-
- // Compute either from torques or from given force and compare
- std::cout << "f_force_from_tau - f_force_from_cs_file: " << forces.segment(3*i_ee, 3).transpose() - cforce_traj_arr_lst[i_ee][i].transpose() << std::endl;
-
- Vector3d l_f_l = forces.segment(3*i_ee, 3);
- // wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]); // EITHER
- wrench_meas_v[i_ee].push_back(l_f_l);
-
- Vector3d o_f_l = oTl.rotation() * l_f_l;
- // std::cout << "o_p_" << ee_names[i_ee] << " " << oTl.translation().transpose() << std::endl;
- // std::cout << "o_f_" << ee_names[i_ee] << " " << o_f_l.transpose() << std::endl;
- o_f_tot += o_f_l;
- }
- // std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl;
- std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
- std::cout << "pin.gravity - wolf gravity: " << model.gravity.linear() - wolf::gravity() << std::endl;
-
-
- //////////////////////////////////////////////
- // COM relative position and velocity measures
- 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;
- // compute for both perfect and distorted models to get the bias
- centerOfMass(model, data, q_static, dq_static);
- centerOfMass(model_dist, data_dist, q_static, dq_static);
-
- // c =def w_p_wc
- // Vector3d b_p_bc = oTb.inverse().act(c); // meas
- Vector3d b_p_bc = data.com[0]; // meas
- Vector3d b_p_bc_dist = data_dist.com[0]; // meas
- Vector3d bias_pbc = b_p_bc_dist - b_p_bc;
-
- // 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
- Vector3d b_v_bc_dist = data_dist.vcom[0]; // meas
- Vector3d bias_vbc = b_v_bc_dist - b_v_bc;
-
- /////////////////////////
- // Centroidal inertia and gesticulation kinetic momentum
- Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d b_Iw_dist = compute_Iw(model_dist, data_dist, q_static, b_p_bc_dist);
- Matrix3d bias_Iw = b_Iw_dist - b_Iw;
-
- // 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();
- Vector3d b_Lc_gesti_dist = computeCentroidalMomentum(model_dist, data_dist, q_static, dq_static).angular();
- Vector3d bias_Lc = b_Lc_gesti_dist - b_Lc_gesti;
-
-
- // std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
- // std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
- // std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
- // std::cout << "bias_Lc: " << bias_Lc.transpose() << std::endl;
-
- /////////////////////////
- // Agreggate everything in vectors for easier WOLF consumption
- p_ob_gtr_v.push_back(p_wb);
- q_ob_gtr_v.push_back(quat_wb);
- v_ob_gtr_v.push_back(w_v_wb);
- bp_traj_arr.push_back(bias_pbc);
- // Lc_gtr_v;
-
- // Measurements
- acc_b_meas_v.push_back(b_proper_acc);
- w_b_meas_v.push_back(b_w_b);
- p_bc_meas_v.push_back(b_p_bc_dist);
- if (vbc_is_dist){
- v_bc_meas_v.push_back(b_v_bc_dist);
- }
- else {
- v_bc_meas_v.push_back(b_v_bc);
- }
- if (Lgest_is_dist){
- Lq_meas_v.push_back(b_Lc_gesti_dist);
- }
- else{
- Lq_meas_v.push_back(b_Lc_gesti);
- }
- if (Iw_is_dist){
- Iq_meas_v.push_back(b_Iw_dist);
- }
- else {
- Iq_meas_v.push_back(b_Iw);
- }
- }
-
- /////////////////////////
- // WOLF enters the scene
- // SETUP PROBLEM/SENSORS/PROCESSORS
- std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- ProblemPtr problem = Problem::create("POVCDL", 3);
-
- // CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- //===================================================== INITIALIZATION
- // SENSOR + PROCESSOR INERTIAL KINEMATICS
- ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = std_pbc_est;
- intr_ik->std_vbc = std_vbc_est;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
- SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
- // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
- SensorInertialKinematicsPtr sen_ikin = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
-
- ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
- params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = std_bp_drift;
- ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
- ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_mcapi_demo.yaml");
- ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
-
- // SENSOR + PROCESSOR FT PREINT
- ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->std_f = std_f_est;
- intr_ft->std_tau = std_tau_est;
- intr_ft->mass = data.mass[0];
- std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
- SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
- // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
- SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
- ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
- params_sen_ft->sensor_ikin_name = "SenIK";
- params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->nbc = nbc;
- params_sen_ft->dimc = dimc;
- params_sen_ft->time_tolerance = 0.0005;
- params_sen_ft->unmeasured_perturbation_std = 0.000001;
- params_sen_ft->max_time_span = 1000;
- params_sen_ft->max_buff_length = 500;
- params_sen_ft->dist_traveled = 20000.0;
- params_sen_ft->angle_turned = 1000;
- params_sen_ft->voting_active = false;
- ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
- ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
-
- // SENSOR + PROCESSOR ODOM3D FOR LEG ODOMETRY
- Vector7d extr_senlegodom; extr_senlegodom << 0,0,0, 0,0,0,1; // not used in practice
- SensorBasePtr sen_odom3d_base = problem->installSensor("SensorOdom3d", "SenLegOdom", extr_senlegodom, bodydynamics_root_dir + "/demos/sensor_odom_3d.yaml");
- SensorOdom3dPtr sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen_odom3d_base);
- ParamsProcessorOdom3dPtr params_sen_odom3d = std::make_shared<ParamsProcessorOdom3d>();
- // ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", sen_odom3d, params_sen_odom3d);
- ProcessorBasePtr proc_legodom_base = problem->installProcessor("ProcessorOdom3d", "ProcLegOdom", "SenLegOdom", bodydynamics_root_dir + "/demos/processor_odom_3d.yaml");
- ProcessorOdom3dPtr proc_legodom = std::static_pointer_cast<ProcessorOdom3d>(proc_legodom_base);
- //////////////////////////////
-
-
- // SETPRIOR RETRO-ENGINEERING
- // We are not using setPrior because of processors initial captures problems so we have to
- // - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors MotionProvider 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_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0];
- MatrixXd P_origin(9,9); P_origin.setIdentity();
- P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
- FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
- // Prior pose factor
- CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
- pose_prior_capture->emplaceFeatureAndFactor();
- // Prior velocity factor
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
-
- // Special trick to make things work in the IMU + IKin + FTPreint case
- // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
- // 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
- // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
- // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
- proc_inkin->keyFrameCallback(KF1, 0.0005);
- proc_imu->setOrigin(KF1);
- proc_legodom->setOrigin(KF1);
-
- // INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
- // momentum parameters
- Matrix<double, 9, 1> meas_ikin; meas_ikin << p_bc_meas_v[0], v_bc_meas_v[0], w_b_meas_v[0];
- auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, Iq_meas_v[0], Lq_meas_v[0]);
- CIKin0->process();
- proc_ftpreint->setOrigin(KF1);
-
-
- ////////////////////////////////////////////
- // INITIAL BIAS FACTORS
- // Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
- FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
-
- // Add loose absolute factor on initial bp bias since dynamical trajectories
- Matrix3d Q_bp = pow(std_abs_bias_pbc, 2)* Matrix3d::Identity();
- CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
- FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
-
-
- ///////////////////////////////////////////
- // process measurements in sequential order
- // SE3 oTb_prev(Quaterniond(q_ob_gtr_v[0]).toRotationMatrix(), p_ob_gtr_v[0]);
- // FrameBasePtr KF_prev = KF1;
- double ts_since_last_kf = 0;
-
- // Add gaussian noises
- std::time_t epoch = std::time(nullptr);
- int64_t seed = (int64_t)epoch;
- Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f_sim, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau_sim, 2)*Matrix3d::Identity(), false, seed);
-
- //////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
- // fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
- std::vector<Vector3d> c_ob_fbk_v;
- std::vector<Vector3d> cd_ob_fbk_v;
- std::vector<Vector3d> Lc_ob_fbk_v;
- //////////////////////////////////////////
-
- for (unsigned int i=0; i < t_arr.size(); i++){
- // TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
- TimeStamp ts(t_arr[i]); // works best with pyb trajectories
-
- // IMU
- Vector6d acc_gyr_meas; acc_gyr_meas << acc_b_meas_v[i], w_b_meas_v[i];
-
- if (noisy_measurements){
- acc_gyr_meas.head<3>() += noise_acc.samples(1);
- acc_gyr_meas.tail<3>() += noise_gyr.samples(1);
- }
-
- Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
-
- // Inertial kinematics
- meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
- // meas_ikin << p_bc_meas_v[ii], v_bc_meas_v[ii], w_b_meas_v[ii];
- // meas_ikin << p_bc_meas_v[ii+1], v_bc_meas_v[ii+1], w_b_meas_v[ii+1];
-
- if (noisy_measurements){
- meas_ikin.segment<3>(0) += noise_pbc.samples(1);
- meas_ikin.segment<3>(3) += noise_vbc.samples(1);
- }
-
- ////////////////////
- // Force Torque
- ////////////////////
-
-
- VectorXd f_meas(nbc*3);
- VectorXd tau_meas(nbc*3);
- VectorXd kin_meas(nbc*7);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- if (dimc == 3){
- f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i];
- }
- else if (dimc == 6){
- f_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].head<3>();
- tau_meas.segment<3>(3*i_ee) = wrench_meas_v[i_ee][i].tail<3>();
- }
- kin_meas.segment<3>(i_ee*3) = p_bl_meas_v[i_ee][i];
- kin_meas.segment<4>(nbc*3 + i_ee*4) = q_bl_meas_v[i_ee][i];
- }
-
- if (noisy_measurements){
- f_meas.segment<3>(0) += noise_f.samples(1);
- f_meas.segment<3>(3) += noise_f.samples(1);
- tau_meas.segment<3>(0) += noise_tau.samples(1);
- tau_meas.segment<3>(3) += noise_tau.samples(1);
- }
-
- VectorXd cap_ftp_data(dimc*nbc+3+3+nbc*7);
- MatrixXd Qftp(dimc*nbc+3+3,dimc*nbc+3+3); // kin not in covariance mat
- if (dimc == 3){
- cap_ftp_data << f_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
- Qftp.setIdentity();
- Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
- Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
- }
- if (dimc == 6){
- cap_ftp_data << f_meas, tau_meas, p_bc_meas_v[i], w_b_meas_v[i], kin_meas;
- Qftp.setIdentity();
- Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
- Qftp.block(nbc*3,nbc*3, nbc*3,nbc*3) *= pow(std_tau_est, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc_est, 2);
- Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
- }
- ////////////////////
- /////////////////
-
- 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();
-
-
-
- if (i > 0){
- // Leg odometry
- // 1) detect feet in contact
- std::vector<int> feet_in_contact;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
- // basic contact detection based on normal foot vel, things break if no foot is in contact
- if (wrench_meas_v[i_ee][i].norm() > fz_thresh){
- feet_in_contact.push_back(i_ee);
- }
- }
- // 2) fill the leg odometry data matrix, depending on the contact dimension
- Eigen::MatrixXd data_legodom;
- if (dimc == 6){
- // cols organized as:
- // [pbl1_km; qbl1_k], [pbl2_km; qbl2_k], ...
- data_legodom.resize(7,feet_in_contact.size()*2);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_in_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1], q_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ], q_bl_meas_v[i_ee_c][i];
- j++;
- }
- }
- else if (dimc == 3){
- // cols organized as:
- // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
- data_legodom.resize(3,feet_in_contact.size()*2 + 1);
- data_legodom.setZero();
- int j = 0;
- for (int i_ee_c: feet_in_contact){
- data_legodom.col(2*j ) << p_bl_meas_v[i_ee_c][i-1];
- data_legodom.col(2*j+1) << p_bl_meas_v[i_ee_c][i ];
- j++;
- }
- data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
- }
- // std::cout << "data_legodom\n" << data_legodom << std::endl;
- // 3) process the data and its cov
- Eigen::Matrix6d data_legodom3D_cov = pow(std_foot_nomove, 2) * Eigen::Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
- CLO->process();
- }
-
- ts_since_last_kf += dt;
- if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- // problem->print(4,true,true,true);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
- ts_since_last_kf = 0;
- }
-
-
- // Store current state estimation
- VectorComposite curr_state = problem->getState(ts);
- p_ob_fbk_v.push_back(curr_state['P']);
- q_ob_fbk_v.push_back(curr_state['O']);
- v_ob_fbk_v.push_back(curr_state['V']);
- c_ob_fbk_v.push_back(curr_state['C']);
- cd_ob_fbk_v.push_back(curr_state['D']);
- Lc_ob_fbk_v.push_back(curr_state['L']);
- }
-
-
-
- ///////////////////////////////////////////
- //////////////// SOLVING //////////////////
- ///////////////////////////////////////////
- problem->print(4,true,true,true);
- if (solve_end){
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
- std::cout << report << std::endl;
- }
-
- //////////////////////////////////////
- //////////////////////////////////////
- // STORE DATA //
- //////////////////////////////////////
- //////////////////////////////////////
- std::fstream file_gtr;
- std::fstream file_est;
- std::fstream file_fbk;
- std::fstream file_kfs;
- std::fstream file_cov;
- std::fstream file_wre;
- file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
- file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
- file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
- file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
- file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
- file_wre.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/wre.csv", std::fstream::out);
- std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz\n";
- file_est << header_est;
- file_fbk << header_est;
- std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lcx,Lcy,Lcz,bpx,bpy,bpz\n";
- file_kfs << header_kfs;
- file_gtr << header_kfs; // at the same frequency as "est" but ground truth biases added for comparison with kfs
- std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLcx,QLcy,QLcz,Qbpx,Qbpy,Qbpz\n";
- file_cov << header_cov;
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- // TODO: change if simulate a non zero imu bias
- file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_gtr_v[i](0) << ","
- << p_ob_gtr_v[i](1) << ","
- << p_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](0) << ","
- << q_ob_gtr_v[i](1) << ","
- << q_ob_gtr_v[i](2) << ","
- << q_ob_gtr_v[i](3) << ","
- << v_ob_gtr_v[i](0) << ","
- << v_ob_gtr_v[i](1) << ","
- << v_ob_gtr_v[i](2) << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << 0.0 << ","
- << c_traj_arr[i](0) << ","
- << c_traj_arr[i](1) << ","
- << c_traj_arr[i](2) << ","
- << dc_traj_arr[i](0) << ","
- << dc_traj_arr[i](1) << ","
- << dc_traj_arr[i](2) << ","
- << L_traj_arr[i](0) << ","
- << L_traj_arr[i](1) << ","
- << L_traj_arr[i](2) << ","
- << bp_traj_arr[i](0) << ","
- << bp_traj_arr[i](1) << ","
- << bp_traj_arr[i](2)
- << std::endl;
- }
-
- VectorComposite state_est;
- for (unsigned int i=0; i < t_arr.size(); i++) {
- state_est = problem->getState(t_arr[i]);
- file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << state_est['P'](0) << ","
- << state_est['P'](1) << ","
- << state_est['P'](2) << ","
- << state_est['O'](0) << ","
- << state_est['O'](1) << ","
- << state_est['O'](2) << ","
- << state_est['O'](3) << ","
- << state_est['V'](0) << ","
- << state_est['V'](1) << ","
- << state_est['V'](2) << ","
- << state_est['C'](0) << ","
- << state_est['C'](1) << ","
- << state_est['C'](2) << ","
- << state_est['D'](0) << ","
- << state_est['D'](1) << ","
- << state_est['D'](2) << ","
- << state_est['L'](0) << ","
- << state_est['L'](1) << ","
- << state_est['L'](2)
- << "\n";
- }
-
- for (unsigned int i=0; i < t_arr.size(); i++) {
- file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_fbk_v[i](0) << ","
- << p_ob_fbk_v[i](1) << ","
- << p_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](0) << ","
- << q_ob_fbk_v[i](1) << ","
- << q_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](3) << ","
- << v_ob_fbk_v[i](0) << ","
- << v_ob_fbk_v[i](1) << ","
- << v_ob_fbk_v[i](2) << ","
- << c_ob_fbk_v[i](0) << ","
- << c_ob_fbk_v[i](1) << ","
- << c_ob_fbk_v[i](2) << ","
- << cd_ob_fbk_v[i](0) << ","
- << cd_ob_fbk_v[i](1) << ","
- << cd_ob_fbk_v[i](2) << ","
- << Lc_ob_fbk_v[i](0) << ","
- << Lc_ob_fbk_v[i](1) << ","
- << Lc_ob_fbk_v[i](2)
- << "\n";
- }
-
- VectorComposite kf_state;
- CaptureBasePtr cap_ikin;
- VectorComposite bp_bias_est;
- CaptureBasePtr cap_imu;
- VectorComposite bi_bias_est;
- for (auto elt : problem->getTrajectory()->getFrameMap())
- {
- auto kf = elt.second;
- kf_state = kf->getState();
- cap_ikin = kf->getCaptureOf(sen_ikin);
- bp_bias_est = cap_ikin->getState();
- cap_imu = kf->getCaptureOf(sen_imu);
- bi_bias_est = cap_imu->getState();
-
- file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << kf_state['P'](0) << ","
- << kf_state['P'](1) << ","
- << kf_state['P'](2) << ","
- << kf_state['O'](0) << ","
- << kf_state['O'](1) << ","
- << kf_state['O'](2) << ","
- << kf_state['O'](3) << ","
- << kf_state['V'](0) << ","
- << kf_state['V'](1) << ","
- << kf_state['V'](2) << ","
- << bi_bias_est['I'](0) << ","
- << bi_bias_est['I'](1) << ","
- << bi_bias_est['I'](2) << ","
- << bi_bias_est['I'](3) << ","
- << bi_bias_est['I'](4) << ","
- << bi_bias_est['I'](5) << ","
- << kf_state['C'](0) << ","
- << kf_state['C'](1) << ","
- << kf_state['C'](2) << ","
- << kf_state['D'](0) << ","
- << kf_state['D'](1) << ","
- << kf_state['D'](2) << ","
- << kf_state['L'](0) << ","
- << kf_state['L'](1) << ","
- << kf_state['L'](2) << ","
- << bp_bias_est['I'](0) << ","
- << bp_bias_est['I'](1) << ","
- << bp_bias_est['I'](2)
- << "\n";
-
- // compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
- // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
- problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
- problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
- problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
- problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
- problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
- problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
-
- solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
- // std::cout << "Qbp\n" << Qbp << std::endl;
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
- problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
- file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << Qp(0,0) << ","
- << Qp(1,1) << ","
- << Qp(2,2) << ","
- << Qo(0,0) << ","
- << Qo(1,1) << ","
- << Qo(2,2) << ","
- << Qv(0,0) << ","
- << Qv(1,1) << ","
- << Qv(2,2) << ","
- << Qbi(0,0) << ","
- << Qbi(1,1) << ","
- << Qbi(2,2) << ","
- << Qbi(3,3) << ","
- << Qbi(4,4) << ","
- << Qbi(5,5) << ","
- << Qc(0,0) << ","
- << Qc(1,1) << ","
- << Qc(2,2) << ","
- << Qd(0,0) << ","
- << Qd(1,1) << ","
- << Qd(2,2) << ","
- << QL(0,0) << ","
- << QL(1,1) << ","
- << QL(2,2) << ","
- << Qbp(0,0) << ","
- << Qbp(1,1) << ","
- << Qbp(2,2)
- << "\n";
- }
-
- file_gtr.close();
- file_est.close();
- file_kfs.close();
- file_cov.close();
- file_wre.close();
-
-
-}
diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml
deleted file mode 100644
index 5752a15407f05dca4aed1ea035ca776f4fce9b7f..0000000000000000000000000000000000000000
--- a/demos/mcapi_povcdl_estimation.yaml
+++ /dev/null
@@ -1,76 +0,0 @@
-# trajectory handling
-dt: 0.001
-min_t: -1.0 # -1 means from the beginning of the trajectory
-max_t: -1 # -1 means until the end of the trajectory
-solve_every_sec: -1 # < 0 strict --> no solve
-solve_end: False
-# solve_every_sec: 0.3 # < 0 strict --> no solve
-# solve_end: True
-
-# estimator sensor noises
-std_acc_est: 0.001
-std_gyr_est: 0.001
-std_pbc_est: 0.001
-std_vbc_est: 0.001 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
-std_f_est: 0.001
-std_tau_est: 0.001
-# std_foot_nomove: 0.000001
-std_foot_nomove: 100000
-
-# simulated sensor noises
-std_acc_sim: 0.0001
-std_gyr_sim: 0.0001
-std_pbc_sim: 0.0001
-std_vbc_sim: 0.0001
-std_f_sim: 0.0001
-std_tau_sim: 0.0001
-std_odom3d_sim: 100000000
-
-# some controls
-fz_thresh: 1
-noisy_measurements: False
-
-# priors
-std_prior_p: 0.01
-std_prior_o: 0.1
-std_prior_v: 0.1
-
-# std_abs_biasimu: 100000
-std_abs_biasimu: 0.0000001 # almost fixed
-std_abs_bias_pbc: 10000 # noprior
-# std_abs_bias_pbc: 0.00001 # almost fixed
-std_bp_drift: 10000 # All the drift you want
-# std_bp_drift: 0.1 # no drift
-
-bias_pbc_prior: [0,0,0]
-
-
-# model disturbance
-scale_dist: 0.00 # disturbance of link inertia
-mass_dist: False
-base_dist_only: False
-
-# which measurement/parameter is disturbed?
-vbc_is_dist: False
-Iw_is_dist: False
-Lgest_is_dist: False
-
-
-# Robot model
-# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
-# dimc: 6
-robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
-dimc: 3
-
-
-#Â Trajectory files
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
-contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
-
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
-
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
-# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
deleted file mode 100644
index d893b4669ce2514f3f15b3bf73500f243923e19c..0000000000000000000000000000000000000000
--- a/demos/mcapi_utils.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-
-#include "mcapi_utils.h"
-
-Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
-{
- return ddr + w.cross(dr);
-}
-
-
-std::vector<Vector3d> contacts_from_footrect_center()
-{
- // compute feet corners coordinates like from the leg_X_6_joint
- double lx = 0.1;
- double ly = 0.065;
- double lz = 0.107;
- std::vector<Vector3d> contacts; contacts.resize(4);
- contacts[0] = {-lx, -ly, -lz};
- contacts[1] = {-lx, ly, -lz};
- contacts[2] = { lx, -ly, -lz};
- contacts[3] = { lx, ly, -lz};
- return contacts;
-}
-
-
-Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
- const Matrix<double, 12, 1>& cf12)
-{
- Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
- Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
- + contacts[1].cross(cf12.segment<3>(3))
- + contacts[2].cross(cf12.segment<3>(6))
- + contacts[3].cross(cf12.segment<3>(9));
- Matrix<double, 6, 1> wrench; wrench << f, tau;
- return wrench;
-}
-
-
-Matrix3d compute_Iw(pinocchio::Model& model,
- pinocchio::Data& data,
- VectorXd& q_static,
- Vector3d& b_p_bc)
-{
- MatrixXd b_Mc = pinocchio::crba(model, data, q_static); // mass matrix at b frame expressed in b frame
- // MatrixXd b_Mc = crba(model_dist, data_dist, q_static); // 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
- pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
- pinocchio::SE3 cTb = bTc.inverse();
- // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
- // c_I_c block diagonal:
- // [m*Id3, 03;
- // 0, Iw]
- MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
- Matrix3d b_Iw = c_I_c.bottomRightCorner<3,3>(); // meas
- return b_Iw;
-}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
deleted file mode 100644
index c62c4026a1dac41733350f2b96fb554285f30715..0000000000000000000000000000000000000000
--- a/demos/mcapi_utils.h
+++ /dev/null
@@ -1,76 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <vector>
-#include "Eigen/Dense"
-#include "pinocchio/algorithm/crba.hpp"
-
-
-using namespace Eigen;
-
-
-/**
- * \brief Compute a 3D acceleration from 6D spatial acceleration
- *
- * \param ddr spatial acc linear part
- * \param w spatial acc rotational part
- * \param dr spatial velocity linear part
-**/
-Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
-
-
-/**
-* \brief Compute the relative contact points from foot center of a Talos foot.
-*
-* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
-* Expressed in local foot coordinates.
-**/
-std::vector<Vector3d> contacts_from_footrect_center();
-
-
-/**
-* \brief Compute the wrench at the end effector center expressed in world coordinates.
-
-* Each contact force (at a foot for instance) is represented in MAPI as a set of
-* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
-* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
-* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
-
-* \param contacts std vector of 3D contact forces
-* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
-* \param wRl rotation matrix, orientation from world frame to foot frame
-**/
-Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
- const Matrix<double, 12, 1>& cf12);
-
-
-/**
-* \brief Compute the centroidal angular inertia used to compute the angular momentum.
-
-* \param model: pinocchio robot model used
-* \param data: pinocchio robot data used
-* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
-* \param b_p_bc: measure of the CoM position relative to the base
-**/
-Matrix3d compute_Iw(pinocchio::Model& model,
- pinocchio::Data& data,
- VectorXd& q_static,
- Vector3d& b_p_bc);