From 3708ca0ca28d56be524cc1aca76578d4ac6e60e9 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Thu, 18 Aug 2022 12:33:37 +0200
Subject: [PATCH] Structure modification of the z axis rotation test
---
...problem_force_torque_inertial_dynamics.cpp | 69 ++++++++++---------
...e_torque_inertial_dynamics_solve_test.yaml | 2 +-
2 files changed, 37 insertions(+), 34 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 940adf9..ad158ed 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -564,16 +564,16 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
- Vector3d initial_ang_vel(0, 0, 0);
- data.segment<3>(3) = initial_ang_vel;
+ Vector3d ang_vel_measure_initial(0, 0, 0);
+ data.segment<3>(3) = ang_vel_measure_initial;
data.segment<3>(6) = -mass_true * gravity();
- Vector3d initial_torque(0, 0, 1);
- data.segment<3>(9) = initial_torque;
+ Vector3d torque_measure_constant(0, 0, 1);
+ data.segment<3>(9) = torque_measure_constant;
MatrixXd data_cov = MatrixXd::Identity(12, 12);
S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix();
- S->getStateBlock('I')->unfix();
+ S->getStateBlock('I')->fix();
S->getStateBlock('C')->fix();
S->getStateBlock('i')->unfix();
S->getStateBlock('m')->fix();
@@ -585,32 +585,34 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
C_KF0->getFrame()->unfix();
C_KF0->getFrame()->getStateBlock('P')->fix();
- C_KF0->getFrame()->getStateBlock('O')->fix();
-
- WOLF_INFO("======== SOLVE PROBLEM =======")
- std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO(report); // should show a very low iteration number (possibly 1)
-
- 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("-----------------------------");
+ C_KF0->getFrame()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
- Vector3d dL = data.segment<3>(9); //dL = torque*dt
- Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState();
- ang_mom = ang_mom + dL; //L = L + dt
-
- Vector3d ang_vel;
- Vector3d actual_inertia;
+ // 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)
{
- // w = L*i_inv we actualize the data
- actual_inertia = p->getSensor()->getStateBlock('i')->getState();
- ang_vel(0) = ang_mom(0) / actual_inertia(0);
- ang_vel(1) = ang_mom(1) / actual_inertia(1);
- ang_vel(2) = ang_mom(2) / actual_inertia(2);
- data.segment<3>(3) = ang_vel;
+ // SIMULATOR
+
+ // calculate dL = torque*dt
+ dL = data.segment<3>(9) * 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
+ // 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;
+
+ // ESTIMATOR
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
@@ -618,18 +620,19 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
p->getOrigin()->getFrame()->unfix();
p->getOrigin()->getStateBlock('P')->fix();
- p->getOrigin()->getStateBlock('O')->fix();
+ p->getOrigin()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
WOLF_INFO("-----------------------------");
- //L increment
- //dL = torque*dt
- //dL in this specific problem is always the same because torque is not changing
- ang_mom = p->getOrigin()->getFrame()->getStateBlock('L')->getState();
- ang_mom = ang_mom + dL;
+ 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());
}
auto inertia_estimated = S->getStateBlock('i')->getState();
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 8eb8204..12fd236 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -43,7 +43,7 @@ config:
# Dynamics
com: [0,0,0.0341] # center of mass [m]
inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
- mass: 1.9 # mass [kg]
+ mass: 1.952 # mass [kg]
processors:
-
--
GitLab