Skip to content
Snippets Groups Projects
Commit 9b278c84 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Refactoring, difference between simulated and estimation noise, disturbance...

Refactoring, difference between simulated and estimation noise, disturbance selection for vbc, Iw and Lgesti
parent c1a8daa8
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!5WIP: Resolve "Any contact type/number refactoring"
......@@ -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;
// }
// }
// }
// }
// }
}
......@@ -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"
......
......@@ -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;
}
#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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment