From 95a774bb30e23af49b1efe29509640d39faaaaaf Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Fri, 19 Aug 2022 15:56:12 +0200
Subject: [PATCH] Fixing errors and new rotation test
---
...problem_force_torque_inertial_dynamics.cpp | 176 +++++++++++++++---
1 file changed, 147 insertions(+), 29 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 125a14a..a0d944d 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -36,6 +36,7 @@
#include <core/state_block/factory_state_block.h>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
+#include <core/tree_manager/tree_manager_sliding_window.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -153,9 +154,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
+ Vector3d cdm_guess (0.01, 0.01, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_true));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
+
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -222,10 +224,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
- S->getStateBlock('m')->setState(Vector1d(1.900));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
- double mass_guess = S->getStateBlock('m')->getState()(0);
+ Vector3d cdm_guess (0.005, 0.005, 0.05);
+ double mass_guess = 1.9;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -306,9 +308,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.02, 0.02, 0.033));
+ Vector3d cdm_guess (0.02, 0.02, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_true));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -378,10 +380,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_mom_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.05, 0.05, 0.05));
- S->getStateBlock('m')->setState(Vector1d(1.50));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
- double mass_guess = S->getStateBlock('m')->getState()(0);
+ Vector3d cdm_guess (0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -467,10 +469,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
// Here we only fix P and O from each key frame
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
{
- S->getStateBlock('C')->setState(Vector3d(0.05, 0.05, 0.05));
- S->getStateBlock('m')->setState(Vector1d(1.50));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
- double mass_guess = S->getStateBlock('m')->getState()(0);
+ Vector3d cdm_guess (0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -559,8 +561,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
// Here we only fix P and O from each key frame
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis_test)
{
- S->getStateBlock('i')->setState(Vector3d(0.01, 0.01, 0.02));
- Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ Vector3d inertia_guess (0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -588,7 +590,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
C_KF0->getFrame()->getStateBlock('P')->fix();
C_KF0->getFrame()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
C_KF0->getFrame()->getStateBlock('O')->fix();
- C_KF0->getFrame()->getStateBlock('O')->setState((Quaterniond(0,0,0,1)).coeffs());
+ C_KF0->getFrame()->getStateBlock('O')->setState((Quaterniond(1, 0, 0, 0)).coeffs());
// initialiation of all the variables needed in the following iteration process
Vector3d torque_true(0, 0, 1);
@@ -596,10 +598,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
Vector3d L_true(0, 0, 0);
Vector3d ang_vel_true(0, 0, 0);
Vector3d ang_acc_true;
- Vector3d angle_true;
- Quaterniond q_true(0,0,0,1);
+ Vector3d angle_true;
+ Quaterniond q_true(1, 0, 0, 0);
double dt = 1; // time increment
- Vector3d position_data (0,0,0);
+ Vector3d position_data(0, 0, 0);
Quaterniond q_data;
std::string report;
@@ -612,18 +614,17 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
// actualize L = L + dt
L_true = L_true + dL; // L = L + dt
// w = L*i_inv we actualize the data
- ang_vel_true(0) = L_true(0) / inertia_true(0);
- ang_vel_true(1) = L_true(1) / inertia_true(1);
- ang_vel_true(2) = L_true(2) / inertia_true(2);
+ ang_vel_true(0) = L_true(0) / inertia_true(0);
+ ang_vel_true(1) = L_true(1) / inertia_true(1);
+ ang_vel_true(2) = L_true(2) / inertia_true(2);
data.segment<3>(3) = ang_vel_true;
ang_acc_true(0) = torque_true(0) / inertia_true(0);
ang_acc_true(1) = torque_true(1) / inertia_true(1);
ang_acc_true(2) = torque_true(2) / inertia_true(2);
- angle_true = ang_vel_true*dt + 0.5*ang_acc_true*dt*dt;
- q_true = q_true * exp_q(angle_true);
-
+ angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
+ q_true = q_true * exp_q(angle_true);
// ESTIMATOR
@@ -652,9 +653,126 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
}
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing)
+{
+
+ Vector3d cdm_guess (0.005, 0.005, 0.1);
+ S->getStateBlock('C')->setState(cdm_guess);
+ Vector3d inertia_guess (0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
+ double mass_guess = 2.0;
+ S->getStateBlock('m')->setState(Vector1d(2.0));
+
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true (0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true (0, 0, 0.01);
+ Vector3d L_true (0, 0, 0);
+ Vector3d position_true (0, 0, 0);
+ Quaterniond q_true (1, 0, 0, 0);
+ Vector3d ang_acc_true;
+ Vector3d angle_true;
+ Vector3d position_data = position_true;
+ Quaterniond q_data = q_true;
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(3) = ang_vel_true;
+ data.segment<3>(6) = -mass_true * gravity();
+ data.segment<3>(9) = torque_true;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->unfix();
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
+
+ // initialiation of all the variables needed in the following iteration process
+
+ Vector3d dL;
+ double dt = 0.25; // time increment
+ std::string report;
+
+ for (TimeStamp t = 1.0; t <= 75.0; t += 0.25)
+ {
+ // SIMULATOR
+
+ data.segment<3>(0) = acc_true;
+
+ // calculate dL = torque*dt
+ dL = torque_true * dt;
+ // actualize L = L + dt
+ L_true = L_true + dL; // L = L + dt
+ // w = L*i_inv we actualize the data
+ ang_vel_true(0) = L_true(0) / inertia_true(0);
+ ang_vel_true(1) = L_true(1) / inertia_true(1);
+ ang_vel_true(2) = L_true(2) / inertia_true(2);
+ data.segment<3>(3) = ang_vel_true;
+
+ data.segment<3>(6) = force_true;
+
+ data.segment<3>(9) = torque_true;
+
+ ang_acc_true(0) = torque_true(0) / inertia_true(0);
+ ang_acc_true(1) = torque_true(1) / inertia_true(1);
+ ang_acc_true(2) = torque_true(2) / inertia_true(2);
+
+ position_data = position_true;
+
+ angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
+ q_true = q_true * exp_q(angle_true);
+ q_data = q_true;
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
+
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cneter of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.rotation_around_z_axis_test";
+ ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.rotation_test_unfixing";
return RUN_ALL_TESTS();
}
--
GitLab