From 90f5566d634286726f0ef632383852f679d529fc Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Thu, 18 Aug 2022 16:45:20 +0200
Subject: [PATCH] Errors fixed and passing z axis rotation gtest
---
...problem_force_torque_inertial_dynamics.cpp | 66 +++++++++++--------
1 file changed, 39 insertions(+), 27 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index ad158ed..25ff34b 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -35,6 +35,7 @@
#include <core/state_block/state_block_derived.h>
#include <core/state_block/factory_state_block.h>
#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -586,31 +587,43 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
C_KF0->getFrame()->unfix();
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());
// initialiation of all the variables needed in the following iteration process
- Vector3d dL;
- Vector3d L_time_actual;
- Vector3d ang_vel_actual;
- Vector3d inertia_actual;
- double dt = 1; // time increment
- CaptureMotionPtr C_time_before = C_KF0;
- std::string report;
-
- for (TimeStamp t = 1.0; t <= 50.0; t += 1.0)
+ Vector3d torque_true(0, 0, 1);
+ Vector3d dL;
+ 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);
+ double dt = 1; // time increment
+ Vector3d position_data (0,0,0);
+ Quaterniond q_data;
+ std::string report;
+
+ for (TimeStamp t = 1.0; t <= 18.0; t += 1.0)
{
// SIMULATOR
// calculate dL = torque*dt
- dL = data.segment<3>(9) * dt;
+ dL = torque_true * dt;
// actualize L = L + dt
- L_time_actual = C_time_before->getFrame()->getStateBlock('L')->getState();
- L_time_actual = L_time_actual + dL; // L = L + dt
+ L_true = L_true + dL; // L = L + dt
// w = L*i_inv we actualize the data
- inertia_actual = p->getSensor()->getStateBlock('i')->getState();
- ang_vel_actual(0) = L_time_actual(0) / inertia_actual(0);
- ang_vel_actual(1) = L_time_actual(1) / inertia_actual(1);
- ang_vel_actual(2) = L_time_actual(2) / inertia_actual(2);
- data.segment<3>(3) = ang_vel_actual;
+ 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);
+
// ESTIMATOR
@@ -619,22 +632,21 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
C->process();
p->getOrigin()->getFrame()->unfix();
- p->getOrigin()->getStateBlock('P')->fix();
- p->getOrigin()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ q_data = q_true;
+ 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 inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
WOLF_INFO("-----------------------------");
-
- WOLF_INFO("True inertia : ", inertia_true, " m^2 Kg.");
- WOLF_INFO("Guess inertia : ", inertia_guess, " m^2 Kg.");
- WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
- WOLF_INFO("-----------------------------");
-
- // saving of the actual capture to use it in the next iteration
- C_time_before = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
}
+ WOLF_INFO("True inertia : ", inertia_true, " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess, " m^2 Kg.");
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("-----------------------------");
auto inertia_estimated = S->getStateBlock('i')->getState();
ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
--
GitLab