diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml index 3ef24102e36a6aafa04839f277ec6eb8dac69c4b..d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8 100644 --- a/demos/processor_ft_preint.yaml +++ b/demos/processor_ft_preint.yaml @@ -9,5 +9,4 @@ keyframe_vote: max_buff_length: 50000 # motion deltas dist_traveled: 20000.0 # meters angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) - voting_active: false - voting_aux_active: false \ No newline at end of file + voting_active: false \ No newline at end of file diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml index 9ff74a3a27de6086489f87597829377cc7c6db36..f92b74c06167c96ef4294e00600c7270bd0d20e2 100644 --- a/demos/processor_imu_solo12.yaml +++ b/demos/processor_imu_solo12.yaml @@ -1,11 +1,10 @@ -type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. -name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails -time_tolerance: 0.0005 # Time tolerance for joining KFs -unmeasured_perturbation_std: 0.000001 keyframe_vote: - max_time_span: 0.05 # seconds - # max_time_span: 0.075 # slower walking - max_buff_length: 100000000000 # motion deltas - dist_traveled: 20000.0 # meters - angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) - voting_active: true \ No newline at end of file + angle_turned: 1000 + dist_traveled: 20000.0 + max_buff_length: 100000000000 + max_time_span: 0.19999 + voting_active: true +name: Main imu +time_tolerance: 0.0005 +type: ProcessorImu +unmeasured_perturbation_std: 1.0e-06 diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml index 4bc4fb3e78808ae3d254454cf1e6c490d6ae8ac7..62258179f3d786b1093ca314d2e9895bf765b7c3 100644 --- a/demos/solo_real_estimation.yaml +++ b/demos/solo_real_estimation.yaml @@ -40,6 +40,9 @@ std_bp_drift: 0.005 # small drift std_pose_p: 0.001 std_pose_o_deg: 1 +std_mocap_extr_o_deg: 0.1 +std_mocap_extr_p: 10 +unfix_extr_sensor_pose: true bias_imu_prior: [0,0,0, 0,0,0] # bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267] @@ -71,6 +74,9 @@ artificial_bias_p: [0.0, 0.0, 0.0] # artificial_bias_p: [3, 6, 4] + + + # Data files # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz" # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz" diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp index 7e52c950c1179cdd14f38ceb4e734d1a22e1818f..310f973c0346d4da0db46f17f120f376750d50c7 100644 --- a/demos/solo_real_pov_estimation.cpp +++ b/demos/solo_real_pov_estimation.cpp @@ -32,6 +32,9 @@ #include "core/tree_manager/tree_manager_sliding_window_dual_rate.h" #include "core/yaml/parser_yaml.h" +#include <core/sensor/sensor_pose.h> +#include <core/processor/processor_pose.h> + // IMU #include "imu/sensor/sensor_imu.h" #include "imu/capture/capture_imu.h" @@ -107,6 +110,15 @@ int main (int argc, char **argv) { Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data()); double fz_thresh = config["fz_thresh"].as<double>(); + // MOCAP + double std_pose_p = config["std_pose_p"].as<double>(); + double std_pose_o_deg = config["std_pose_o_deg"].as<double>(); + double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>(); + double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>(); + // + + bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>(); + std::string data_file_path = config["data_file_path"].as<std::string>(); std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>(); @@ -114,6 +126,8 @@ int main (int argc, char **argv) { unsigned int nbc = ee_names.size(); // Base to IMU transform Quaterniond b_q_i(b_qvec_i); + assert(b_q_i.normalized().isApprox(b_q_i)); + Quaterniond i_q_b = b_q_i.conjugate(); SE3 b_T_i(b_q_i, b_p_bi); SE3 i_T_b = b_T_i.inverse(); Matrix3d b_R_i = b_T_i.rotation(); @@ -139,6 +153,9 @@ int main (int argc, char **argv) { // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"]; cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"]; cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"]; + // cnpy::NpyArray contact_npyarr = arr_map["contact"]; + // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"]; + // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"]; double* t_arr = t_npyarr.data<double>(); double* imu_acc_arr = imu_acc_npyarr.data<double>(); @@ -155,6 +172,10 @@ int main (int argc, char **argv) { // double* o_R_i_arr = o_R_i_npyarr.data<double>(); double* w_p_wm_arr = w_p_wm_npyarr.data<double>(); double* w_q_m_arr = w_q_m_npyarr.data<double>(); + + // double* contact_arr = contact_npyarr.data<double>(); + // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>(); + // double* w_R_m_arr = w_R_m_npyarr.data<double>(); unsigned int N = t_npyarr.shape[0]; if (max_t > 0){ @@ -184,7 +205,7 @@ int main (int argc, char **argv) { ProblemPtr problem = Problem::create("POV", 3); // add a tree manager for sliding window optimization - ParserYaml parser = ParserYaml("demos/params_tree_manager.yaml", bodydynamics_root_dir); + ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir); ParamsServer server = ParamsServer(parser.getParams()); auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server); problem->setTreeManager(tree_manager); @@ -212,19 +233,41 @@ int main (int argc, char **argv) { //===================================================== INITIALIZATION // 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_solo12.yaml"); + SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.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_solo12.yaml"); ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base); - // SENSOR + PROCESSOR POINT FEET NOMOVE - ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>(); - intr_pfn->std_foot_nomove_ = std_odom3d_est; - VectorXd extr; // default size for dynamic vector is 0-0 -> what we need - SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn); - SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base); - ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>(); - ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm); + // // SENSOR + PROCESSOR POINT FEET NOMOVE + // ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>(); + // intr_pfn->std_foot_nomove_ = std_odom3d_est; + // VectorXd extr; // default size for dynamic vector is 0-0 -> what we need + // SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn); + // SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base); + // ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>(); + // ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm); + + // SENSOR + PROCESSOR POSE (for mocap) + // pose sensor and proc (to get extrinsics in the prb) + auto intr_sp = std::make_shared<ParamsSensorPose>(); + intr_sp->std_p = std_pose_p; + intr_sp->std_o = toRad(std_pose_o_deg); + Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs(); + auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp); + auto params_proc = std::make_shared<ParamsProcessorPose>(); + params_proc->time_tolerance = 0.005; + auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc); + // somehow by default the sensor extrinsics is fixed + Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity(); + Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity(); + sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p); + sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o); + if (unfix_extr_sensor_pose){ + sensor_pose->unfixExtrinsics(); + } + else{ + sensor_pose->fixExtrinsics(); + } @@ -233,15 +276,14 @@ int main (int argc, char **argv) { // - Manually create the first KF and its pose and velocity priors // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later) - // VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()}); - Vector4d q_identity; q_identity << 0,0,0,1; - VectorComposite x_origin("POV", {Vector3d::Zero(), q_identity, Vector3d::Zero()}); + VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()}); 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, 0.0005); + // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005); + FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005); // if mocap used proc_imu->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(); @@ -249,8 +291,9 @@ int main (int argc, char **argv) { 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); - sen_imu->getStateBlock('I')->setState(bias_imu_prior); + + /////////////////////////////////////////// // process measurements in sequential order double ts_since_last_kf = 0; @@ -259,32 +302,27 @@ int main (int argc, char **argv) { std::vector<Vector4d> i_qvec_l_vec_prev; std::vector<bool> feet_in_contact_prev; - // for point feet factor - std::vector<Vector3d> i_p_il_vec_origin; - FrameBasePtr KF_origin = KF1; - ////////////////////////////////////////// // 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; - double o_p_ob_carr[3*N]; - double o_q_b_carr[4*N]; - double o_v_ob_carr[3*N]; - double imu_bias_carr[6*N]; + double o_p_ob_fbk_carr[3*N]; + double o_q_b_fbk_carr[4*N]; + double o_v_ob_fbk_carr[3*N]; + double imu_bias_fbk_carr[6*N]; + double extr_mocap_fbk_carr[7*N]; std::vector<Vector3d> i_omg_oi_v; ////////////////////////////////////////// - - unsigned int nb_kf = 1; for (unsigned int i=0; i < N; i++){ TimeStamp ts(t_arr[i]); //////////////////////////////////// - // Retrieve current state + // Retrieve current state VectorComposite curr_state = problem->getState(); Vector4d o_qvec_i_curr = curr_state['O']; Quaterniond o_q_i_curr(o_qvec_i_curr); @@ -299,51 +337,54 @@ int main (int argc, char **argv) { Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i); Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i); Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i); + // Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work! + // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work! + Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i); + Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i); + w_qvec_wm.normalize(); // not close enough to wolf eps sometimes // store i_omg_oi for later computation of o_v_ob from o_v_oi i_omg_oi_v.push_back(i_omg_oi); - Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); - // std::cout << "bias_acc_gyro: " << bias_acc_gyro.transpose() << std::endl; - Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.tail<3>()); + Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); + Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>()); //////////////////////////////////// // IMU Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi; - // Vector6d acc_gyr_meas; acc_gyr_meas << 0,0,9.806, 0,0,0; Matrix6d acc_gyr_cov = sen_imu->getNoiseCov(); //////////////////////////////////// // Kinematics + forces - SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); - Matrix3d o_R_i = o_T_i.rotation(); - Matrix3d i_R_o = o_R_i.transpose(); - Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr; - Vector3d i_acc = imu_acc + i_R_o * gravity(); - Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr); - - VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa; - VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa; - VectorXd ddq(model.nv); ddq.setZero(); - ddq.head<3>() = i_asp_i; - // TODO: add other terms to compute forces (ddqa...) - - // update and retrieve kinematics - forwardKinematics(model, data, q); - updateFramePlacements(model, data); - - // compute all linear jacobians in feet frames (only for quadruped) - MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero(); - for (unsigned 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>(); - } + // SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); + // Matrix3d o_R_i = o_T_i.rotation(); + // Matrix3d i_R_o = o_R_i.transpose(); + // Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr; + // Vector3d i_acc = imu_acc + i_R_o * gravity(); + // Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr); + + // VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa; + // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa; + // VectorXd ddq(model.nv); ddq.setZero(); + // ddq.head<3>() = i_asp_i; + // // TODO: add other terms to compute forces (ddqa...) + + // // update and retrieve kinematics + // forwardKinematics(model, data, q); + // updateFramePlacements(model, data); + + // // compute all linear jacobians in feet frames (only for quadruped) + // MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero(); + // for (unsigned 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) + // // 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): @@ -352,187 +393,86 @@ int main (int argc, char **argv) { // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2) // Or else, retrieve from preprocessed dataset - Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i); - - Vector3d o_f_tot = Vector3d::Zero(); - std::vector<Vector3d> l_f_l_vec; - std::vector<Vector3d> o_f_l_vec; - std::vector<Vector3d> i_p_il_vec; - std::vector<Vector4d> i_qvec_l_vec; - // needing relative kinematics measurements - VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1; - VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0; - forwardKinematics(model, data, q_static, dq_static); - updateFramePlacements(model, data); - for (unsigned int i_ee=0; i_ee < nbc; i_ee++){ - auto b_T_l = data.oMf[ee_ids[i_ee]]; - auto i_T_l = i_T_b * b_T_l; - Vector3d i_p_il = i_T_l.translation(); // meas - Matrix3d i_R_l = i_T_l.rotation(); - Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas - i_p_il = i_p_il + i_R_l*l_p_lg; - - Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas - Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test - o_f_tot += o_f_l; - - l_f_l_vec.push_back(l_f_l); - o_f_l_vec.push_back(o_f_l); - i_p_il_vec.push_back(i_p_il); - i_qvec_l_vec.push_back(i_qvec_l); - } - - // initialization for point feet factor - if (i == 0){ - i_p_il_vec_origin = i_p_il_vec; - } - // 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; + // Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i); + + // Vector3d o_f_tot = Vector3d::Zero(); + // std::vector<Vector3d> l_f_l_vec; + // std::vector<Vector3d> o_f_l_vec; + // std::vector<Vector3d> i_p_il_vec; + // std::vector<Vector4d> i_qvec_l_vec; + // // needing relative kinematics measurements + // VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1; + // VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0; + // forwardKinematics(model, data, q_static, dq_static); + // updateFramePlacements(model, data); + // // std::cout << "" << std::endl; + // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){ + // // std::cout << i_ee << std::endl; + // auto b_T_l = data.oMf[ee_ids[i_ee]]; + + // // overides with value from mocap + // // std::cout << b_T_l.translation().transpose() << std::endl; + // b_T_l.translation(b_p_bl_mocap.segment<3>(3*i_ee)); + // // std::cout << b_T_l.translation().transpose() << std::endl; + + // auto i_T_l = i_T_b * b_T_l; + // Vector3d i_p_il = i_T_l.translation(); // meas + // Matrix3d i_R_l = i_T_l.rotation(); + // Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas + // i_p_il = i_p_il + i_R_l*l_p_lg; + + // Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas + // // Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test + + // l_f_l_vec.push_back(l_f_l); + // // o_f_l_vec.push_back(o_f_l); + // i_p_il_vec.push_back(i_p_il); + // i_qvec_l_vec.push_back(i_qvec_l); + // } CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov); CImu->process(); - - //////////////////////////////////////// - //////////////////////////////////////// - //////////////////////////////////////// - // Integrate leg odom in 4 separate factors - //////////////////////////////////////// - // 2) fill the leg odometry data matrix, depending on the contact dimension - // forwardKinematics and updateFramePlacements have been called on b centered q and dq - - // gyro - // // .) nothing - // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0; - // ..) direct gyro - // data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0; - // ...) gyro compensated - // for (int i_ee=0; i_ee < 4; i_ee++) - // { - // Eigen::MatrixXd data_legodom(10,2); - // // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0; - // data_legodom.rightCols<1>() << i_omg_oi_unbiased, 0,0,0,0, 0,0,0; - - // Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee]).linear(); - // Vector3d l_v_il = l_v_bl; - // data_legodom.col(0) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_il; - // // data_legodom.col(0) << 0,0,0, 0,0,0,1, 0,0,0; - - // // 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_lst[i_ee], data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN); - // CLO->process(); - // } - - //////////////////////////////////////// - //////////////////////////////////////// - // 1) detect feet in contact - std::vector<int> feet_in_contact; - int nb_feet_in_contact = 0; - std::unordered_map<int, Vector3d> kin_incontact; - 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; - // nb_feet: just for debugging/testing kinematic factor - if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){ - feet_in_contact.push_back(i_ee); - nb_feet_in_contact++; - kin_incontact.insert({i_ee, i_p_il_vec[i_ee]}); - } - } - - CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact); - CPF->process(); - - - // // 2) fill the leg odometry data matrix, depending on the contact dimension - // // forwardKinematics and updateFramePlacements have been called on b centered q and dq - - // // Option a) Differential kinematics - // Eigen::MatrixXd data_legodom(10,feet_in_contact.size()+1); - // // gyro - // // // .) nothing - // // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0; - // // ..) direct gyro - // data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0; - // // ...) gyro compensated - // // Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); - // // data_legodom.rightCols<1>() << (acc_gyr_meas.tail<3>()-bias_acc_gyro.tail<3>()), 0,0,0,0, 0,0,0; - - // unsigned int j = 0; - // for (int i_ee_c: feet_in_contact){ - // Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee_c]).linear(); - // // data_legodom.col(j) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_bl; - // data_legodom.col(j) << 0,0,0, 0,0,0,1, 0,0,0; - // j++; - // } - // // 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_DIFFERENTIAL_KIN); - // CLO->process(); - - - - - // // Option b) Relative kinematics - // if (i > 0){ - // // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b - // Eigen::MatrixXd data_legodom(3,2*nb_feet_in_contact+1); - - // // data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>(); - // // data_legodom.rightCols<1>() = Vector3d::Zero(); - // // Compensate bias - // Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); - // data_legodom.rightCols<1>() = (acc_gyr_meas.tail<3>() - bias_acc_gyro.tail<3>()); - - // for (unsigned int j=0; j < feet_in_contact.size(); j++){ - // data_legodom.col(2*j ) << i_p_il_vec_prev[j]; - // data_legodom.col(2*j+1) << i_p_il_vec[j]; - // // data_legodom.col(2*j ) << 0,0,0; - // // data_legodom.col(2*j+1) << 0,0,0; + // int nb_feet_in_contact = 0; + // std::unordered_map<int, Vector3d> kin_incontact; + // 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; + // // nb_feet: just for debugging/testing kinematic factor + + // // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){ + // if (contact_gtr(i_ee)){ + // nb_feet_in_contact++; + // kin_incontact.insert({i_ee, i_p_il_vec[i_ee]}); // } + // } - // // 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(); + // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact); + // CPF->process(); + + // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm; + Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm; + CapturePosePtr CP = std::make_shared<CapturePose>(ts, sensor_pose, pose_meas, sensor_pose->getNoiseCov()); + CP->process(); + + // ts_since_last_kf += dt; + // if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){ + // // problem->print(4,true,true,true); + // auto kf = problem->emplaceFrame(ts); + // problem->keyFrameCallback(kf, nullptr, 0.0005); + // std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); + // std::cout << report << std::endl; + // ts_since_last_kf = 0; + // // std::cout << "Extr SPose: " << sensor_pose->getP()->getState().transpose() << " " << sensor_pose->getO()->getState().transpose() << std::endl; // } - // i_p_il_vec_prev = i_p_il_vec; - // i_qvec_l_vec_prev = i_qvec_l_vec; - - - // add zero vel artificial factor - if (problem->getTrajectory()->getFrameMap().size() > nb_kf){ - auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin(); - std::cout << "New KF " << kf_pair->first << std::endl; - auto kf = kf_pair->second; - - // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first); - // FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones()); - // FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false); - - // point feet factor for each foot (short cut to avoid using a processor...) - for (int i_ee=0; i_ee < 4; i_ee++) - { - Vector6d meas; meas << i_p_il_vec_origin[i_ee], i_p_il_vec[i_ee]; - Matrix3d cov = pow(std_odom3d_est, 2)*Matrix3d::Identity(); - // CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(kf, "PointFeet", kf_pair->first); - // FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "PointFeet", meas, cov); - // FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, KF_origin, nullptr, false); - } - i_p_il_vec_origin = i_p_il_vec; - - nb_kf++; - KF_origin = kf; - } - ts_since_last_kf += dt; - if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){ - // problem->print(4,true,true,true); + // solve every new KF + if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); + std::cout << ts << " "; std::cout << report << std::endl; - ts_since_last_kf = 0; + nb_kf = problem->getTrajectory()->getFrameMap().size(); } // Store current state estimation @@ -542,18 +482,23 @@ int main (int argc, char **argv) { Vector3d o_v_oi = state_fbk['V']; // IMU to base frame - o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); + Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); + // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); Matrix3d o_R_b = o_R_i * i_R_b; Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs(); - Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; + Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi); + // Vector3d o_p_ob = o_p_oi; + // Vector3d o_v_ob = o_v_oi; - Vector6d imu_bias = sen_imu->getIntrinsic()->getState(); + imu_bias = sen_imu->getIntrinsic()->getState(); + Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState(); - mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double)); - mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double)); - mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double)); - mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double)); + mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double)); + mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double)); + mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double)); + mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double)); + mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double)); p_ob_fbk_v.push_back(o_p_ob); q_ob_fbk_v.push_back(o_qvec_b); @@ -565,159 +510,40 @@ int main (int argc, char **argv) { /////////////////////////////////////////// //////////////// 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 OFFLINE // - // NOT WORKING WITH SLIDING WINDOW // - ////////////////////////////////////// - ////////////////////////////////////// - - // std::fstream file_est; - // std::fstream file_fbk; - // std::fstream file_kfs; - // std::fstream file_cov; - // 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; - // 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; - // double o_p_ob_carr[3*N]; - // double o_q_b_carr[4*N]; - // double o_v_ob_carr[3*N]; - // for (unsigned int i=0; i < N; i++) { - // state_est = problem->getState(t_arr[i]); - // Vector3d i_omg_oi = i_omg_oi_v[i]; - // Vector3d o_p_oi = state_est['P']; - // Vector4d o_qvec_i = state_est['O']; - // Vector3d o_v_oi = state_est['V']; - - // Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); - // Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.tail<3>()); - - // Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); - // Matrix3d o_R_b = o_R_i * i_R_b; - // Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs(); - // Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; - // Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi); - - // mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double)); - // mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double)); - // mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double)); - - // file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - // << t_arr[i] << "," - // << o_p_ob(0) << "," - // << o_p_ob(1) << "," - // << o_p_ob(2) << "," - // << o_qvec_b(0) << "," - // << o_qvec_b(1) << "," - // << o_qvec_b(2) << "," - // << o_qvec_b(3) << "," - // << o_v_oi(0) << "," - // << o_v_oi(1) << "," - // << o_v_oi(2) - // << "\n"; - // } - - // VectorComposite kf_state; - // CaptureBasePtr cap_imu; - // VectorComposite bi_bias_est; - // for (auto& elt: problem->getTrajectory()->getFrameMap()){ - // auto kf = elt.second; - // if (kf->isKey()){ - // 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"; - // } - // } - - // for (unsigned int i=0; i < N; 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"; - // } - - // file_est.close(); - // file_kfs.close(); - // file_cov.close(); + double o_p_ob_carr[3*N]; + double o_q_b_carr[4*N]; + double o_v_ob_carr[3*N]; + double imu_bias_carr[6*N]; + for (unsigned int i=0; i < N; i++) { + VectorComposite state_est = problem->getState(t_arr[i]); + Vector3d i_omg_oi = i_omg_oi_v[i]; + Vector3d o_p_oi = state_est['P']; + Vector4d o_qvec_i = state_est['O']; + Vector3d o_v_oi = state_est['V']; + + Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); + std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl; + Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>()); + + Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); + Matrix3d o_R_b = o_R_i * i_R_b; + Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs(); + Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; + Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi); + // Vector3d o_p_ob = o_p_oi; + // Vector3d o_v_ob = o_v_oi; + + mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double)); + mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double)); + mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double)); + mempcpy(imu_bias_carr+3*i, imu_bias.data(), 3*sizeof(double)); + } // Save trajectories in npz file @@ -731,12 +557,16 @@ int main (int argc, char **argv) { cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a"); // save estimates + cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a"); + cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a"); + cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a"); cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a"); cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a"); cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a"); - // and biases - - cnpy::npz_save(out_npz_file_path, "imu_biases", imu_bias_carr, {N,6}, "a"); + // and biases/extrinsics + cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a"); + cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a"); + cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a"); } diff --git a/demos/params_tree_manager.yaml b/demos/tree_manager.yaml similarity index 66% rename from demos/params_tree_manager.yaml rename to demos/tree_manager.yaml index 6355024a0ba3875a0a5981422864e7828eb04f26..ceb0dd77261e7361210a690b2ba563aaa58e7779 100644 --- a/demos/params_tree_manager.yaml +++ b/demos/tree_manager.yaml @@ -1,7 +1,7 @@ config: problem: tree_manager: - fix_first_frame: true - n_frames: 5 + n_fix_first_frames: 3 + n_frames: 100000000000 type: TreeManagerSlidingWindow viral_remove_empty_parent: true