diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 0df254eb5030e03fcc00e6cb6b976f3b48ffa97f..2218f18ba11c0cc34dc72060da2fa0f8945fc25e 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -50,6 +50,8 @@
#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
@@ -65,7 +67,6 @@ 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");
@@ -77,19 +78,33 @@ int main (int argc, char **argv) {
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>();
- double std_pbc = config["std_pbc"].as<double>();
- double std_vbc = config["std_vbc"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- double std_f = config["std_f"].as<double>();
- double std_tau = config["std_tau"].as<double>();
- double std_acc = config["std_acc"].as<double>();
- double std_gyr = config["std_gyr"].as<double>();
- double std_abs_biasp = config["std_abs_biasp"].as<double>();
- double std_abs_biasimu = config["std_abs_biasimu"].as<double>();
- double std_odom3d = config["std_odom3d"].as<double>();
+
+ // estimator sensor noises
+ double std_acc_est = config["std_acc_est"].as<double>();
+ double std_gyr_est = config["std_gyr_est"].as<double>();
+ 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_biasp = config["std_abs_biasp"].as<double>();
+ double std_abs_biasimu = config["std_abs_biasimu"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+
double fz_thresh = config["fz_thresh"].as<double>();
std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
Vector3d bias_pbc_prior;
@@ -97,6 +112,10 @@ int main (int argc, char **argv) {
bias_pbc_prior(1) = bias_pbc_prior_vec[1];
bias_pbc_prior(2) = bias_pbc_prior_vec[2];
double scale_dist = config["scale_dist"].as<double>();
+ bool base_dist_only = config["base_dist_only"].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>();
@@ -170,16 +189,16 @@ int main (int argc, char **argv) {
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model_dist);
Eigen::EigenMultivariateNormal<double> noise_iner(Vector3d::Zero(), Matrix3d::Identity(), false);
// distortion
- for (int i=0; i < model_dist.inertias.size(); i++){
- // Vector3d lever_dist = model.inertias.lever() + scale_dist * iner.lever().norm() * noise_iner.samples(1);
- Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_iner.samples(1);
- std::cout << "i: " << i << std::endl;
- model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
+ if (base_dist_only){
+ int root_joint_id = model.getJointId("root_joint");
+ Vector3d lever_dist = model.inertias[root_joint_id].lever() + scale_dist * noise_iner.samples(1);
+ model_dist.inertias[root_joint_id] = pinocchio::Inertia(model.inertias[root_joint_id].mass(), lever_dist, model.inertias[root_joint_id].inertia());
}
- // as a check
- std::cout << "\n\n\n" << std::endl;
- for (auto iner: model_dist.inertias){
- std::cout << iner.lever().transpose() << std::endl;
+ else{
+ for (int i=0; i < model_dist.inertias.size(); i++){
+ Vector3d lever_dist = model.inertias[i].lever() + scale_dist * noise_iner.samples(1);
+ model_dist.inertias[i] = pinocchio::Inertia(model.inertias[i].mass(), lever_dist, model.inertias[i].inertia());
+ }
}
Data data_dist(model_dist);
@@ -237,14 +256,9 @@ int main (int argc, char **argv) {
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
+ // Vector3d c = c_traj_arr[i]; // gtr
VectorXd dc = dc_traj_arr[i]; // gtr
VectorXd L = L_traj_arr[i]; // gtr
- // for (unsigned int i_ee; i_ee < nbc; i_ee++){
- // wrench_meas_v[i_ee]
- // }
- // VectorXd cforce_ll = ll_cforce_traj_arr[i];
- // VectorXd cforce_rl = rl_cforce_traj_arr[i];
//////////////////////////////////
//////////////////////////////////
@@ -277,39 +291,15 @@ int main (int argc, char **argv) {
auto bTl = oTb.inverse() * oTl;
Vector3d b_p_l = bTl.translation(); // meas
Vector4d b_qvec_l = Quaterniond(bTl.rotation()).coeffs(); // meas
- // std::cout << "b_p_l: " << b_p_l.transpose() << std::endl;
- // std::cout << "b_qvec_l: " << b_qvec_l.transpose() << std::endl;
-
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
wrench_meas_v[i_ee].push_back(cforce_traj_arr_lst[i_ee][i]);
}
- // 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
- // Vector6d l_wrench_ll = cforce_ll; // meas
- // Vector6d l_wrench_rl = 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;
//////////////////////////////////////////////
// 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;
@@ -317,61 +307,65 @@ int main (int argc, char **argv) {
// 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);
- // 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 = data_dist.vcom[0]; // meas
// c =def w_p_wc
- // same as robot.com(q_static)
- // Vector3d b_p_bc = data_dist.com[0]; // meas
- Vector3d b_p_bc = data_dist.com[0]; // meas
// 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;
+ std::cout << "bias_pbc: " << bias_pbc.transpose() << std::endl;
- // compute bias ground truth from the difference between real model and disturbed one
- bp_traj_arr.push_back(b_p_bc - data.com[0]);
+ // 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;
+ std::cout << "" << std::endl;
+ std::cout << "bias_vbc: " << bias_vbc.transpose() << std::endl;
/////////////////////////
- // bIomg, Lcm
- MatrixXd b_Mc = crba(model_dist, data_dist, q); // mass matrix at b frame expressed in b frame
- // MatrixXd b_Mc = crba(model, data, q); // mass matrix at b frame expressed in b frame
- MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>(); // inertia matrix at b frame expressed in b frame
- SE3 bTc (oTb.rotation().transpose(), b_p_bc);
- 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();
- // std::cout << "c_I_c\n" << c_I_c << std::endl;
- Matrix3d b_Iw = oTb.rotation().transpose() * c_I_c.bottomRightCorner<3,3>() * oTb.rotation(); // meas
+ // 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;
+ std::cout << "bias_Iw: \n" << bias_Iw << std::endl;
+
// 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();
-
- // // Alternative method
- // MatrixXd b_Mc_art = cTb.toDualActionMatrix() * b_Mc.block(0,6, 6,model.nv-6);
- // std::cout << b_Mc_art.rows() << " " << b_Mc_art.cols() << std::endl;
- // Vector3d c_Lc_gesti = b_Mc_art.bottomRows<3>() * dq.tail(model.nv - 6); // qa_dot
- // Vector3d b_Lc_gesti_other= oTb.rotation().transpose() * c_Lc_gesti;
-
- // std::cout << "b_Lc_gesti - b_Lc_gesti_other" << std::endl;
- // std::cout << (b_Lc_gesti - b_Lc_gesti_other).transpose() << std::endl;
+ 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_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);
- v_bc_meas_v.push_back(b_v_bc);
- Lq_meas_v.push_back(b_Lc_gesti);
- Iq_meas_v.push_back(b_Iw);
+ 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);
+ }
}
/////////////////////////
@@ -387,8 +381,8 @@ int main (int argc, char **argv) {
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = std_pbc;
- intr_ik->std_vbc = std_vbc;
+ 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!
@@ -409,8 +403,8 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR FT PREINT
ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->std_f = std_f;
- intr_ft->std_tau = std_tau;
+ intr_ft->std_f = std_f_est;
+ intr_ft->std_tau = std_tau_est;
intr_ft->mass = data.mass[0];
std::cout << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
@@ -507,15 +501,14 @@ int main (int argc, char **argv) {
// 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, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f, 2)*Matrix3d::Identity(), false, seed);
- Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau, 2)*Matrix3d::Identity(), false, seed);
+ 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);
- int traj_size = problem->getTrajectory()->getFrameList().size();
for (unsigned int ii=1; ii < t_arr.size(); ii++){
TimeStamp ts(t_arr[ii]);
unsigned int i = ii - 1;
@@ -529,8 +522,8 @@ int main (int argc, char **argv) {
}
Matrix6d acc_gyr_cov = Matrix6d::Identity();
- acc_gyr_cov.topLeftCorner<3,3>() = pow(std_acc, 2) * Matrix3d::Identity();
- acc_gyr_cov.bottomRightCorner<3,3>() = pow(std_gyr, 2) * Matrix3d::Identity();
+ acc_gyr_cov.topLeftCorner<3,3>() = pow(std_acc_est, 2) * Matrix3d::Identity();
+ acc_gyr_cov.bottomRightCorner<3,3>() = pow(std_gyr_est, 2) * Matrix3d::Identity();
// Inertial kinematics
meas_ikin << p_bc_meas_v[i], v_bc_meas_v[i], w_b_meas_v[i];
@@ -573,16 +566,16 @@ int main (int argc, char **argv) {
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, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc, 2);
+ 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, 2);
- Qftp.block(nbc*3,nbc*3, nbc*3,nbc*3) *= pow(std_tau, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(std_pbc, 2);
+ 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>();
}
////////////////////
@@ -635,30 +628,11 @@ int main (int argc, char **argv) {
data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
}
// 3) process the data and its cov
- Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d, 2) * Eigen::Matrix6d::Identity();
+ 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);
CLO->process();
}
-
- // ///////////////////////////////////////////
- // // Manually create an Odom3d factor when a new KF is detected (will be put in a processorLegOdom3d)
- // if (problem->getTrajectory()->getFrameList().size() > traj_size){
- // FrameBasePtr KF_now = problem->getTrajectory()->getLastKeyFrame();
- // SE3 oTb_now(Quaterniond(q_ob_gtr_v[i]).toRotationMatrix(), p_ob_gtr_v[i]);
- // SE3 bprev_T_bnow = oTb_prev.inverse() * oTb_now;
- // Vector7d prev_pose_now; prev_pose_now << bprev_T_bnow.translation(), Quaterniond(bprev_T_bnow.rotation()).coeffs();
- // Matrix6d rel_pose_cov = pow(std_odom3d, 2) * Matrix6d::Identity();
-
- // CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF_now, KF_now->getTimeStamp(), nullptr, prev_pose_now, rel_pose_cov);
- // FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_now, rel_pose_cov);
- // FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF_prev, nullptr, false);
-
- // oTb_prev = oTb_now;
- // KF_prev = KF_now;
- // traj_size = problem->getTrajectory()->getFrameList().size();
- // }
-
ts_since_last_kf += dt;
if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -878,4 +852,21 @@ int main (int argc, char **argv) {
file_wre.close();
+ // // Print factor energy
+ // for (auto kf: problem->getTrajectory()->getFrameList()){
+ // 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.yaml b/demos/mcapi_povcdl_estimation.yaml
index 0d16e98919d1136885cedd9a18d6eefdde1dbb21..716993edfc100f7c970c5288665f48ab671b7e0e 100644
--- a/demos/mcapi_povcdl_estimation.yaml
+++ b/demos/mcapi_povcdl_estimation.yaml
@@ -7,39 +7,61 @@ max_t: -1 # -1 means until the end of the trajectory
solve_every_sec: 0.3 # < 0 strict --> no solve
solve_end: True
-# sensor noises
-std_acc: 0.0001
-# std_gyr: 0.0001
-std_gyr: 0.0001
-std_pbc: 0.0001
-std_vbc: 0.0001
-std_f: 0.0001
-std_tau: 0.0001
+# estimator sensor noises
+std_acc_est: 0.0001
+std_gyr_est: 0.0001
+std_pbc_est: 0.0001
+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.0001
+std_tau_est: 0.0001
+std_odom3d_est: 100000000
+# 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.0001
+std_prior_o: 0.0001
+std_prior_v: 0.0001
+
# std_abs_biasimu: 100000
std_abs_biasimu: 0.0000001 # almost fixed
std_abs_biasp: 10000000 # noprior
-# std_abs_biasp: 0.0000001 # almost fixed
+# std_abs_biasp: 0.00001 # almost fixed
std_bp_drift: 10000000 # All the drift you want
-# std_bp_drift: 0.00001 # no drift
-std_odom3d: 0.0001
-# std_odom3d: 100000000
-std_prior_p: 0.0001
-std_prior_o: 0.0001
-std_prior_v: 0.0001
+# std_bp_drift: 0.0001 # no drift
bias_pbc_prior: [0,0,0]
-scale_dist: 0.001 # disturbance of link inertia levers
+
+
+# model disturbance
+scale_dist: 0.01 # disturbance of link inertia levers
+base_dist_only: True
+
+# which measurement/parameter is disturbed?
+vbc_is_dist: False
+Iw_is_dist: True
+Lgest_is_dist: True
+
# 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
-#Â files
+
+
+#Â 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"
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
index 50a31e622a9de029d316dc45fe81e2763253cc77..d9de7b04b13d36549484f7ef67b086cd49ab16ff 100644
--- a/demos/mcapi_utils.cpp
+++ b/demos/mcapi_utils.cpp
@@ -24,7 +24,7 @@ std::vector<Vector3d> contacts_from_footrect_center()
Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
- const Matrix<double, 12, 1>& cf12)
+ 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))
@@ -34,3 +34,23 @@ Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contac
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
index 2fda5867a1f35f090e59f8ca492fb163bfb1682b..01b551b1153ef0d88c2f1d88a901d065f40023b8 100644
--- a/demos/mcapi_utils.h
+++ b/demos/mcapi_utils.h
@@ -1,5 +1,7 @@
#include <vector>
#include "Eigen/Dense"
+#include "pinocchio/algorithm/crba.hpp"
+
using namespace Eigen;
@@ -37,3 +39,17 @@ std::vector<Vector3d> contacts_from_footrect_center();
**/
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);
\ No newline at end of file