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

Compute ALL covariances instead of particular ones because the latter does not...

Compute ALL covariances instead of particular ones because the latter does not seem to work properly. Bp covariance block not found though.
parent e5958877
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"
// debug
#include <iostream> #include <iostream>
#include <random> // #include <random>
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <ctime>
#include "eigenmvn.h" #include "eigenmvn.h"
#include "pinocchio/parsers/urdf.hpp" #include "pinocchio/parsers/urdf.hpp"
...@@ -138,7 +138,8 @@ int main (int argc, char **argv) { ...@@ -138,7 +138,8 @@ int main (int argc, char **argv) {
std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot] std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
std::vector<VectorXd> c_traj_arr; // w_p_wc std::vector<VectorXd> c_traj_arr; // w_p_wc
std::vector<VectorXd> dc_traj_arr; // w_v_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> L_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); std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
int N = (max_t - min_t)/dt; int N = (max_t - min_t)/dt;
int N_inter = N/10; int N_inter = N/10;
...@@ -309,6 +310,11 @@ int main (int argc, char **argv) { ...@@ -309,6 +310,11 @@ int main (int argc, char **argv) {
Vector3d b_p_bc = data.com[0]; // meas Vector3d b_p_bc = data.com[0]; // meas
// Vector3d b_p_bc = oTb.inverse().act(c); // meas // Vector3d b_p_bc = oTb.inverse().act(c); // meas
// compute bias ground truth from the difference between real model and disturbed one
// TODO: for the moment, constant bias put in the traj
bp_traj_arr.push_back(bias_pbc);
///////////////////////// /////////////////////////
// bIomg, Lcm // bIomg, Lcm
MatrixXd b_Mc = crba(model, data, 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
...@@ -483,15 +489,15 @@ int main (int argc, char **argv) { ...@@ -483,15 +489,15 @@ int main (int argc, char **argv) {
double ts_since_last_kf = 0; double ts_since_last_kf = 0;
// Add gaussian noises // Add gaussian noises
Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc, 2)*Matrix3d::Identity()); std::time_t epoch = std::time(nullptr);
Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr, 2)*Matrix3d::Identity()); int64_t seed = (int64_t)epoch;
Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc, 2)*Matrix3d::Identity()); Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc, 2)*Matrix3d::Identity()); Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f, 2)*Matrix3d::Identity()); Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau, 2)*Matrix3d::Identity()); 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);
// wolf::setGravityMagnitude(9.81);
// wolf::setGravityMagnitude(9.806);
int traj_size = problem->getTrajectory()->getFrameList().size(); int traj_size = problem->getTrajectory()->getFrameList().size();
for (unsigned int ii=1; ii < t_arr.size(); ii++){ for (unsigned int ii=1; ii < t_arr.size(); ii++){
...@@ -666,16 +672,20 @@ int main (int argc, char **argv) { ...@@ -666,16 +672,20 @@ int main (int argc, char **argv) {
std::fstream file_gtr; std::fstream file_gtr;
std::fstream file_est; std::fstream file_est;
std::fstream file_kfs; std::fstream file_kfs;
std::fstream file_cov;
std::fstream file_wre; std::fstream file_wre;
file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/gtr.csv", std::fstream::out); file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
file_est.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/est.csv", std::fstream::out); file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/kfs.csv", std::fstream::out); file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
file_wre.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/wre.csv", std::fstream::out); file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n"; file_wre.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/wre.csv", std::fstream::out);
file_gtr << header; std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n";
file_est << header; file_est << header_est;
std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n"; std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
file_kfs << header_kfs; 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,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLx,QLy,QLz,Qbpx,Qbpy,Qbpz\n";
file_cov << header_cov;
std::string header_wre = "t,plfx,plfy,plfz,prfx,prfy,prfz,f_lfx,f_lfy,f_lfz,f_rfx,f_rfy,f_rfz,tau_lfx,tau_lfy,tau_lfz,tau_rfx,tau_rfy,tau_rfz\n"; std::string header_wre = "t,plfx,plfy,plfz,prfx,prfy,prfz,f_lfx,f_lfy,f_lfz,f_rfx,f_rfy,f_rfz,tau_lfx,tau_lfy,tau_lfz,tau_rfx,tau_rfy,tau_rfz\n";
file_wre << header_wre; file_wre << header_wre;
...@@ -700,7 +710,10 @@ int main (int argc, char **argv) { ...@@ -700,7 +710,10 @@ int main (int argc, char **argv) {
<< dc_traj_arr[i](2) << "," << dc_traj_arr[i](2) << ","
<< L_traj_arr[i](0) << "," << L_traj_arr[i](0) << ","
<< L_traj_arr[i](1) << "," << L_traj_arr[i](1) << ","
<< L_traj_arr[i](2) << L_traj_arr[i](2) << ","
<< bp_traj_arr[i](0) << ","
<< bp_traj_arr[i](1) << ","
<< bp_traj_arr[i](2)
<< std::endl; << std::endl;
} }
...@@ -757,11 +770,14 @@ int main (int argc, char **argv) { ...@@ -757,11 +770,14 @@ int main (int argc, char **argv) {
// } // }
VectorComposite kf_state; VectorComposite kf_state;
CaptureBasePtr cap_ikin;
VectorComposite bp_bias_est; VectorComposite bp_bias_est;
solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
for (auto kf: problem->getTrajectory()->getFrameList()){ for (auto kf: problem->getTrajectory()->getFrameList()){
if (kf->isKey()){ if (kf->isKey()){
kf_state = kf->getState(); kf_state = kf->getState();
bp_bias_est = kf->getCaptureOf(sen_ikin)->getState(); cap_ikin = kf->getCaptureOf(sen_ikin);
bp_bias_est = cap_ikin->getState();
file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< kf->getTimeStamp().get() << "," << kf->getTimeStamp().get() << ","
...@@ -788,12 +804,59 @@ int main (int argc, char **argv) { ...@@ -788,12 +804,59 @@ int main (int argc, char **argv) {
<< bp_bias_est['I'](1) << "," << bp_bias_est['I'](1) << ","
<< bp_bias_est['I'](2) << bp_bias_est['I'](2)
<< "\n"; << "\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 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(cap_ikin->getStateBlockVec());
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);
problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
// std::cout << "Qbp\n" << Qbp << std::endl;
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) << ","
<< 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_gtr.close();
file_est.close(); file_est.close();
file_kfs.close(); file_kfs.close();
file_cov.close();
file_wre.close(); file_wre.close();
......
# trajectory handling # trajectory handling
dt: 0.001 dt: 0.001
min_t: -1.0 # -1 means from the beginning of the trajectory min_t: -1.0 # -1 means from the beginning of the trajectory
max_t: -1.0 # -1 means until the end of the trajectory max_t: -1 # -1 means until the end of the trajectory
# solve_every_sec: -1 # < 0 strict --> no solve # solve_every_sec: -1 # < 0 strict --> no solve
solve_every_sec: 0.5 # < 0 strict --> no solve # solve_end: False
solve_every_sec: 1 # < 0 strict --> no solve
solve_end: True solve_end: True
# sensor noises # sensor noises
std_acc: 0.0001 std_acc: 0.0001
# std_gyr: 0.0001
std_gyr: 0.0001 std_gyr: 0.0001
std_pbc: 0.0001 std_pbc: 0.0001
std_vbc: 0.0001 std_vbc: 0.0001
...@@ -21,17 +23,16 @@ noisy_measurements: True ...@@ -21,17 +23,16 @@ noisy_measurements: True
std_abs_biasimu: 0.0000001 # almost fixed std_abs_biasimu: 0.0000001 # almost fixed
std_abs_biasp: 10000000 # noprior std_abs_biasp: 10000000 # noprior
# std_abs_biasp: 0.0000001 # almost fixed # std_abs_biasp: 0.0000001 # almost fixed
std_bp_drift: 0.05 # no drift std_bp_drift: 1000000000 # small drift
# std_bp_drift: 100000 # small drift
std_odom3d: 0.0001 std_odom3d: 0.0001
# std_odom3d: 100000000 # std_odom3d: 100000000
std_prior_p: 0.0001 std_prior_p: 0.0001
std_prior_o: 0.0001 std_prior_o: 0.0001
std_prior_v: 0.0001 std_prior_v: 0.0001
bias_pbc: [0.01,0.02,0.03] # bias_pbc: [0.01,0.02,0.03]
# bias_pbc_prior: [0.01,0.02,0.03] # bias_pbc_prior: [0.01,0.02,0.03]
# bias_pbc: [0.0,0.0,0.0] bias_pbc: [0.0,0.0,0.0]
bias_pbc_prior: [0,0,0] bias_pbc_prior: [0,0,0]
# Robot model # Robot model
......
#include <vector>
#include "Eigen/Dense" #include "Eigen/Dense"
using namespace Eigen; using namespace Eigen;
......
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