From a6221b11c415ae56666f48e89c1132b44f012c18 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 11 Sep 2022 21:06:58 +0200
Subject: [PATCH] Use Sensor/ProcessorPose to account for pose measurements
---
...problem_force_torque_inertial_dynamics.cpp | 90 +++++++++----------
...que_inertial_dynamics_simulation_test.yaml | 24 +++++
2 files changed, 68 insertions(+), 46 deletions(-)
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 9c0922f..88dd905 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -38,6 +38,8 @@
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/tree_manager/tree_manager_sliding_window.h>
+#include <core/capture/capture_pose.h>
+
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -56,17 +58,27 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
{
public:
ProblemPtr P;
+ SolverCeresPtr solver;
+
+ // FTI sensor
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
- SolverCeresPtr solver;
Vector6d bias_true;
Vector3d cdm_true;
Vector3d inertia_true;
double mass_true;
- std::vector<double> time_stamp;
+
double dt;
+
+ // Pose sensor
+ SensorBasePtr SP;
+ Vector7d pose;
+
+ // Reading csv data
+ std::vector<double> time_stamp;
std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
std::vector<Quaterniond> quaternion;
+
// Fitxer CSV
std::fstream fout;
@@ -218,9 +230,12 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
// solver->getSolverOptions().gradient_tolerance = 1e-15;
// solver->getSolverOptions().parameter_tolerance = 1e-15;
- S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ // FTI sensor and processor
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->findSensor("sensor FTI"));
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+ // Pose sensor
+ SP = P->findSensor("sensor Pose");
}
};
@@ -380,11 +395,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
int i_init = 0;
- // fix measured position and orientation
- p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
- p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+ // Pose
+ pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+ CP->process();
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
@@ -402,25 +416,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
data.segment<3>(9) = torque[i];
MatrixXd data_cov = S->getNoiseCov();
+ pose << position[i] , quaternion[i].coeffs();
+
// ESTIMATOR
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
- // check if new KF
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+ CP->process();
+
+ // check if new KF and solve
if (last_kf_id != p->getOrigin()->getFrame()->id())
{
last_kf_id = p->getOrigin()->getFrame()->id();
- // fix measured position and orientation
- p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
- p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
-
// solve!
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
// results of this iteration
WOLF_INFO("Time : ", t, " s.");
@@ -434,12 +446,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
}
}
- report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
-
-
// FINAL RESULTS
- WOLF_INFO(report);
WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
@@ -455,7 +462,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
WOLF_INFO("-----------------------------");
ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.11);
- ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-2);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 2e-2);
}
@@ -493,27 +500,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
// S->addPriorParameter('m', // mass
// Vector1d(mass_guess), // mass
- // 0.1 * 0.1 * Matrix1d::Identity()); // cov
+ // 0.01 * 0.01 * Matrix1d::Identity()); // cov
- // Force processor to make a KF at t=0
+ int i_init = 0;
+
+ // Force FTI processor to make a KF at t=0
CaptureMotionPtr C0 =
std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
C0->process();
- int i_init = 0;
-
- // fix measured position and orientation
- p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
- p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
-
- unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ // Pose
+ pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+ CP->process();
for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
{
// SIMULATOR
-
TimeStamp t = time_stamp[i];
// Data
@@ -523,23 +526,18 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
data.segment<3>(6) = force[i];
data.segment<3>(9) = torque[i];
MatrixXd data_cov = S->getNoiseCov();
+
+ pose << position[i] , quaternion[i].coeffs();
// ESTIMATOR
+ // FTI
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
- // check if new KF
- if (last_kf_id != p->getOrigin()->getFrame()->id())
- {
- last_kf_id = p->getOrigin()->getFrame()->id();
-
- // fix measured position and orientation
- p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
- p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
- }
+ // Pose
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+ CP->process();
}
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
@@ -565,7 +563,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.21);
ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 2e-3);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
- ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 3e-2);
+ ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 4e-2);
}
int main(int argc, char** argv)
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
index d2878a2..3dc2877 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
@@ -49,6 +49,19 @@ config:
inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2.00 # mass [kg]
+ -
+ name: sensor Pose
+ type: SensorPose
+ plugin: core
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ std_px: 0.001
+ std_py: 0.001
+ std_pz: 0.001
+ std_ox: 0.001
+ std_oy: 0.001
+ std_oz: 0.001
+
processors:
-
name: "proc FTID"
@@ -67,3 +80,14 @@ config:
max_buff_length: 999 # motion deltas
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ -
+ name: prc Pose
+ type: ProcessorPose
+ plugin: core
+ sensor_name: sensor Pose
+ time_tolerance: 0.01
+ keyframe_vote:
+ voting_active: true
+ apply_loss_function: false
+
--
GitLab