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

Up to date (misc small changes)

parent 1b5a3f90
No related branches found
No related tags found
2 merge requests!18Release after RAL,!17After 2nd RAL submission
# trajectory handling
dt: 0.001
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_end: False
# solve_every_sec: 0.3 # < 0 strict --> no solve
......@@ -12,10 +12,10 @@ std_acc_est: 0.001
std_gyr_est: 0.001
std_pbc_est: 0.001
std_vbc_est: 0.001 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
std_f_est: 0.01
std_tau_est: 0.01
std_odom3d_est: 0.001
# std_odom3d_est: 10000000
std_f_est: 0.001
std_tau_est: 0.001
# std_odom3d_est: 0.000001
std_odom3d_est: 100000
# simulated sensor noises
std_acc_sim: 0.0001
......@@ -24,29 +24,26 @@ std_pbc_sim: 0.0001
std_vbc_sim: 0.0001
std_f_sim: 0.0001
std_tau_sim: 0.0001
std_odom3d_est: 0.001
# std_odom3d_sim: 100000000
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_p: 0.01
std_prior_o: 0.1
std_prior_v: 0.1
bias_pbc_prior: [0,0,0]
std_abs_bias_pbc: 1 # noprior
# std_abs_biasimu: 100000
std_abs_biasimu: 0.0000001 # almost fixed
std_abs_bias_pbc: 10000 # noprior
# std_abs_bias_pbc: 0.00001 # almost fixed
# std_bp_drift: 10000 # All the drift you want
std_bp_drift: 0.0001 # small drift
std_bp_drift: 10000 # All the drift you want
# std_bp_drift: 0.1 # no drift
bias_pbc_prior: [0,0,0]
bias_imu_prior: [0,0,0, 0,0,0]
# bias_imu_prior: [0,0,0,-0.000079251, 0.00439540765, -0.0002120267]
# bias_imu_prior: [0,0,0, 0.1, 0.2, 0.3]
std_abs_bias_acc: 1000
std_abs_bias_gyro: 1000
# model disturbance
scale_dist: 0.00 # disturbance of link inertia
......@@ -54,34 +51,26 @@ mass_dist: False
base_dist_only: False
# which measurement/parameter is disturbed?
vbc_is_dist: true
Iw_is_dist: true
Lgest_is_dist: true
vbc_is_dist: False
Iw_is_dist: False
Lgest_is_dist: False
# Robot model
# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
# dimc: 6
robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12_nofeet.urdf"
dimc: 3
# Trajectory files
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs" # tested on real sequence?
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_nomove.cs" # tested on real sequence
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_rp+y.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
# PYB
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_nomove_Pyb_Nofeet.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk_Pyb_Nofeet.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_trunk_Pyb_Nofeet.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_rp+y_Pyb_Nofeet.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping_nofeet_Pyb_Nofeet.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
config:
problem:
tree_manager:
fix_first_frame: true
n_frames: 10
type: TreeManagerSlidingWindow
viral_remove_empty_parent: true
type: "ProcessorForceTorquePreint" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "PFTPreint" # This is ignored. The name provided to the SensorFactory prevails
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00000001
time_tolerance: 0.0005 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0000000
sensor_ikin_name: "SenIK"
sensor_angvel_name: "SenImu"
keyframe_vote:
max_time_span: 100000000000 # seconds
max_buff_length: 500 # motion deltas
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
......
......@@ -3,7 +3,8 @@ name: "Main imu" # This is ignored. The name provided to the SensorF
time_tolerance: 0.0005 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.000001
keyframe_vote:
max_time_span: 0.29999 # seconds
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)
......
......@@ -2,21 +2,29 @@
dt: 0.001
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: 4.8 # -1 means until the end of the trajectory
# solve_every_sec: -1 # < 0 strict --> no solve
# solve_end: False
solve_every_sec: 0.3 # < 0 strict --> no solve
solve_every_sec: 0.05 # < 0 strict --> no solve
solve_end: True
# Ceres setup
max_num_iterations: 1000
# estimator sensor noises
std_pbc_est: 0.001
std_vbc_est: 1000 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
std_f_est: 1000
std_pbc_est: 0.01
std_vbc_est: 0.01
std_f_est: 1
std_tau_est: 1000
std_odom3d_est: 0.001
std_odom3d_est: 0.01
# std_odom3d_est: 10000000
# some controls
fz_thresh: 3
fz_thresh: 1 # slow walk
# fz_thresh: 3 # slow walk
# fz_thresh: 3 # slow traj
# fz_thresh: 4 # stamping
# fz_thresh: 5.5 # walking
# priors
std_prior_p: 0.00001
......@@ -24,34 +32,40 @@ std_prior_o: 1
std_prior_v: 1
bias_pbc_prior: [0,0,0]
std_abs_bias_pbc: 0.001 # noprior
# bias_pbc_prior: [0.0139338 , 0.02786759, 0.01857839]
std_abs_bias_pbc: 0.1 # noprior
# std_abs_bias_pbc: 0.00001 # almost fixed
std_bp_drift: 0.1 # All the drift you want
# std_bp_drift: 0.01 # small drift
# std_bp_drift: 1000 # All the drift you want
std_bp_drift: 0.005 # small drift
bias_imu_prior: [0,0,0.0, 0,0,0]
bias_imu_prior: [0,0,0, 0,0,0]
# bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267]
# bias_imu_prior: [0,0,-0.11, 0,0,0]
# bias_imu_prior: [0,0,0, 0.1, 0.2, 0.3]
std_abs_bias_acc: 100
std_abs_bias_gyro: 100
# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12_nofeet.urdf"
robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
dimc: 3
mass_offset: -0.00405
mass_offset: 0.0
# mass_offset: -0.00405
# mass_offset: -0.005
nb_feet: 4
l_p_lg: [0.0, 0, 0]
# l_p_lg: [0, 0, -0.016]
# l_p_lg: [0.014, 0, 0]
# l_p_lg: [0.05, 0, 0]
# l_p_lg: [0, 0, -0.031] # for sin traj, point to which position estimation on z starts to be less good
# base to IMU transformation
# b_p_bi: [0.0, 0.0, 0.0]
b_p_bi: [-0.1, 0.0, 0.0]
# b_p_bi: [0.1163, 0.0, 0.02]
# b_q_i: [0.0, 0.0, 0.0, 1.0]
b_q_i: [0.00000592745, -0.03255761280, -0.00025745595, 0.706732091]
# b_p_bi: [-0.2, 0.0, 0.0]
b_p_bi: [0.1163, 0.0, 0.02]
b_q_i: [0.0, 0.0, 0.0, 1.0]
# b_q_i: [0.00000592745, -0.03255761280, -0.00025745595, 0.706732091]
artificial_bias_p: [0.0, 0.0, 0.0]
# artificial_bias_p: [0.03, 0.06, 0.04]
# artificial_bias_p: [3, 6, 4]
# Data files
......@@ -74,11 +88,14 @@ b_q_i: [0.00000592745, -0.03255761280, -0.00025745595, 0.706732091]
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_05_format.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_09_10_20_soir/data_2020_10_09_19_06_format.npz"
# data_2020_10_15_14_34 standing still
# data_2020_10_15_14_36 sin traj
# data_2020_10_15_14_38 solo stamping
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format.npz"
data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr.npz"
......@@ -87,12 +104,46 @@ data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTmean_Filtereddqa.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTacc_0gyr_Filtereddqa.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CST_0gyr_Gacc.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_shortened_CSTqa.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_VERY_shortened.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format.npz"
# data_2020_10_15_18_21: ...
# data_2020_10_15_18_23: walk
# data_2020_10_15_18_24: walk
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_21.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_24.npz"
# same but with precomputed forces
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_34_format_forces.npz" # standing still
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_36_format_forces.npz" # sinusoid
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_15_10_2020/data_2020_10_15_14_38_format_forces.npz" # stamping
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Log_15_10_2020_part2/data_2020_10_15_18_23_format_forces.npz" # fast walk
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_31_20_12_format_forces.npz" # 0.48s walk
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_18_format_forces.npz" # standing still+walk 0.48 FAIL
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Logs_05_10_2020_18h/data_2020_11_05_18_20_format_forces.npz" # standing still+walk 0.48
# POINT FEET
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Point_feet_27_11_20/data_2020_11_27_14_46_format.npz" # standing still+walk 0.48
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_17_format.npz" # stamping
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_18_format.npz" # sin
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_22_format.npz" # walking
data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_54_format.npz" # sin
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/SinStamping_Corrected_09_12_2020/data_2020_12_09_17_56_format.npz" # stamping
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_25_format.npz" # point feet walk with corrected kinematics
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Walk_17_12_2020/data_2020_12_17_14_29_format.npz" # point feet walk with corrected kinematics
out_npz_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments_results/out.npz"
\ No newline at end of file
This diff is collapsed.
This diff is collapsed.
......@@ -133,22 +133,10 @@ bool FactorPointFeetNomove::operator () (
Map<const Quaternion<T> > qbm(_qbm);
// Measurements retrieval
// auto& meas = getMeasurement();
Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
Matrix<T,3,1> bm_p_bml = meas.topRows(3); // Bprev_p_BprevL
Matrix<T,3,1> bm_p_bml = meas.topRows(3); // Bprev_p_BprevL
Matrix<T,3,1> b_p_bl = meas.bottomRows(3); // B_p_BL
// // error
// Eigen::Matrix<T,3,1> er;
// er(0) = meas(0) - _p[0];
// er(1) = meas(1) - _p[1];
// er(2) = pi2pi(meas(2) - _o[0]);
// // residual
// Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
// res = getMeasurementSquareRootInformationUpper() * er;
Matrix<T,3,1> err = (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
res = getMeasurementSquareRootInformationUpper() * err;
......
......@@ -125,7 +125,7 @@ inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureI
// 5. Create bias drift factor if an origin capture exists -> zero difference + covariance as integration of a Brownian noise
if (_cap_ikin_origin){
double dt_drift = _cap_ikin->getTimeStamp() - _cap_ikin_origin->getTimeStamp();
Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->std_bp_drift, 2) * sqrt(dt_drift);
Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->std_bp_drift, 2) * dt_drift;
FeatureBasePtr feat_diff = FeatureBase::emplace<FeatureBase>(_cap_ikin, "ComBiasDrift", Eigen::Vector3d::Zero(), cov_drift,
FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic();
......
......@@ -45,24 +45,35 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
// store the initial contact/kinematic meas map
auto cap1 = std::static_pointer_cast<CapturePointFeetNomove>(cap1_it->second);
auto kin_incontact_from_t1 = cap1->kin_incontact_;
std::cout << kin_incontact_from_t1.size() << " ";
auto cap_it = std::next(cap1_it);
auto it_after_capt_t2 = std::next(cap2_it);
// loop throught the captures between t1 and t2
while (cap_it != it_after_capt_t2){
// for each new capture, filter the initial contact/kin meas map to track which feet stay in contact throughout
auto cap_pfeet = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
if (!cap_pfeet){
}
auto kin_incontact_current = cap_pfeet->kin_incontact_;
for (auto elt: kin_incontact_from_t1){
if (kin_incontact_current.find(elt.first) == kin_incontact_current.end()){
// std::cout << kin_incontact_current.size() << " ";
for (auto elt_it = kin_incontact_from_t1.cbegin(); elt_it != kin_incontact_from_t1.cend(); /* no increment */){
if (kin_incontact_current.find(elt_it->first) == kin_incontact_current.end()){
// The foot has lost contact with the ground since t1
kin_incontact_from_t1.erase(elt.first);
elt_it = kin_incontact_from_t1.erase(elt_it); // erase and jump (from stackoverflow)
}
else{
elt_it++;
}
}
cap_it++;
}
// std::cout << std::endl;
if (!kin_incontact_from_t1.empty())
{
......@@ -77,7 +88,7 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second;
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove());
FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, false);
FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true);
}
}
......
#include <core/utils/utils_gtest.h>
#include <core/utils/logging.h>
#include <core/common/wolf.h>
#include <core/problem/problem.h>
#include <core/state_block/state_block.h>
#include <core/state_block/state_quaternion.h>
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "bodydynamics/capture/capture_force_torque_preint.h"
#include "bodydynamics/processor/processor_force_torque_preint.h"
class FeatureForceTorquePreint_test : public testing::Test
{
public: //These can be accessed in fixtures
wolf::ProblemPtr problem_;
wolf::TimeStamp ts_;
wolf::CaptureForceTorquePreintPtr ftp_cap_ptr_;
Eigen::VectorXd state_vec_;
Eigen::VectorXd delta_preint_;
Eigen::Matrix<double,12,12> delta_preint_cov_;
std::shared_ptr<wolf::FeatureForceTorquePreint> feat_ftp_;
wolf::FrameBasePtr last_frame_;
wolf::FrameBasePtr origin_frame_;
Eigen::MatrixXd J_delta_bias_;
wolf::ProcessorBasePtr processor_ptr_;
//a new of this will be created for each new test
virtual void SetUp()
{
using namespace wolf;
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
// Wolf problem_
problem_ = Problem::create("POV", 3);
Eigen::VectorXd ForceTorque_extrinsics(7);
ForceTorque_extrinsics << 0,0,0, 0,0,0,1;
ParamsSensorForceTorquePreintPtr sen_imu_params = std::make_shared<ParamsSensorForceTorquePreint>();
SensorBasePtr sensor_ptr = problem_->installSensor("SensorForceTorquePreint", "Main FTPreint", ForceTorque_extrinsics, sen_imu_params);
ParamsProcessorForceTorquePreintPtr prc_imu_params = std::make_shared<ParamsProcessorForceTorquePreint>();
prc_imu_params->max_time_span = 0.5;
prc_imu_params->max_buff_length = 10;
processor_ptr_ = problem_->installProcessor("ProcessorForceTorquePreint", "FT pre-integrator", sensor_ptr, prc_imu_params);
// Time and data variables
TimeStamp t;
Eigen::Matrix<double, 32, 1> data_;
t.set(0);
// Set the origin
Eigen::VectorXd x0(13);
x0 << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1;
MatrixXd P0; P0.setIdentity(12,12);
origin_frame_ = problem_->setPrior(x0, P0, 0.0, 0.01);
Eigen::Vector3d zero3; zero3.setZero();
StateBlockPtr sbc = make_shared<StateBlock>(zero3); origin_frame_->addStateBlock("C", sbc); problem_->notifyStateBlock(sbc,ADD);
StateBlockPtr sbd = make_shared<StateBlock>(zero3); origin_frame_->addStateBlock("D", sbd); problem_->notifyStateBlock(sbd,ADD);
StateBlockPtr sbL = make_shared<StateBlock>(zero3); origin_frame_->addStateBlock("L", sbL); problem_->notifyStateBlock(sbL,ADD);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_); KFA_->addStateBlock("B", sbbA); problem_->notifyStateBlock(sbbA,ADD);
// StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock("I", sbbimuA); problem_->notifyStateBlock(sbbimuA,ADD);
// Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
ftp_cap_ptr_ = std::static_pointer_cast<CaptureForceTorquePreint>(
CaptureBase::emplace<CaptureForceTorquePreint>(origin_frame_,
t,
sensor_ptr,
data_,
Eigen::Matrix6d::Identity(),
Eigen::Vector6d::Zero()) );
//process data
Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Vector3d tau1; tau1 << 0,0,0;
Vector3d pbl1; pbl1 << 0,0,0;
Vector4d bql1; bql1 << 0,0,0,1;
Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Vector3d tau2; tau2 << 0,0,0;
Vector3d pbl2; pbl2 << 0,0,0;
Vector4d bql2; bql2 << 0,0,0,1;
Vector3d pbc; pbc << 0,0,0;
Vector3d wb; wb << 0,0,0;
// aggregated meas
Vector6d f_meas; f_meas << f1, f2;
Vector6d tau_meas; tau_meas << tau1, tau2;
Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
data_ << f_meas, tau_meas, pbc, wb, kin_meas;
Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
Matrix3d info_bias_drift = 1e6 * Matrix3d::Identity();
t.set(0.1);
// Expected state after one integration
//x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
// assign data to capture
ftp_cap_ptr_->setData(data_);
ftp_cap_ptr_->setTimeStamp(t);
// process data in capture
sensor_ptr->process(ftp_cap_ptr_);
//emplace Frame
ts_ = problem_->getProcessorIsMotion()->getBuffer().back().ts_;
state_vec_ = problem_->getProcessorIsMotion()->getCurrentState();
last_frame_ = problem_->emplaceKeyFrame( state_vec_, ts_);
//emplace a feature
delta_preint_ = problem_->getProcessorIsMotion()->getMotion().delta_integr_;
delta_preint_cov_ = problem_->getProcessorIsMotion()->getMotion().delta_integr_cov_ + MatrixXd::Identity(12,12)*1e-08;
VectorXd calib_preint = problem_->getProcessorIsMotion()->getLast()->getCalibrationPreint();
J_delta_bias_ = problem_->getProcessorIsMotion()->getMotion().jacobian_calib_;
feat_ftp_ = std::static_pointer_cast<FeatureForceTorquePreint>(
FeatureBase::emplace<FeatureForceTorquePreint>(ftp_cap_ptr_,
delta_preint_,
delta_preint_cov_,
calib_preint,
J_delta_bias_,
ftp_cap_ptr_) );
}
virtual void TearDown(){}
};
TEST_F(FeatureForceTorquePreint_test, check_frame)
{
// set variables
using namespace wolf;
FrameBasePtr left_frame = feat_ftp_->getFrame();
wolf::TimeStamp t;
left_frame->getTimeStamp(t);
origin_frame_->getTimeStamp(ts_);
ASSERT_NEAR(t.get(), ts_.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts_ << std::endl;
ASSERT_TRUE(origin_frame_->isKey());
ASSERT_TRUE(last_frame_->isKey());
ASSERT_TRUE(left_frame->isKey());
wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
origin_pptr = origin_frame_->getP();
origin_optr = origin_frame_->getO();
origin_vptr = origin_frame_->getV();
left_pptr = left_frame->getP();
left_optr = left_frame->getO();
left_vptr = left_frame->getV();
ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
Eigen::Map<const Eigen::Quaterniond> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), wolf::Constants::EPS_SMALL);
ASSERT_EQ(origin_frame_->id(), left_frame->id());
}
// TEST_F(FeatureForceTorquePreint_test, access_members)
// {
// using namespace wolf;
// Eigen::VectorXd delta(10);
// //dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
// delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
// ASSERT_MATRIX_APPROX(feat_ftp_->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
// }
//TEST_F(FeatureForceTorquePreint_test, addFactor)
//{
// using namespace wolf;
//
// FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame_);
// auto cap_imu = last_frame_->getCaptureList().back();
// FactorImuPtr factor_imu = std::make_shared<FactorImu>(feat_ftp_, std::static_pointer_cast<CaptureForceTorquePreint>(cap_imu), processor_ptr_);
// feat_ftp_->addFactor(factor_imu);
// origin_frame_->addConstrainedBy(factor_imu);
//}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
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