diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp index 6e46c370d6804d350580d24c982f8e5eace448e5..940adf9489f1ddacb280954d657081b0d3f5ef6a 100644 --- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp +++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp @@ -558,7 +558,7 @@ 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.01)); + S->getStateBlock('i')->setState(Vector3d(0.01, 0.01, 0.02)); Vector3d inertia_guess = S->getStateBlock('i')->getState(); // Data @@ -583,13 +583,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis 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('O')->fix(); - Vector3d dL = data.segment<3>(9); - Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState(); - ang_mom = ang_mom + dL; - 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) @@ -599,29 +596,40 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 kg."); WOLF_INFO("-----------------------------"); + 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; - for (TimeStamp t = 0.0; t <= 50.0; t += 1.0) + for (TimeStamp t = 1.0; t <= 50.0; t += 1.0) { - // w = i_inv*L we actualize the data //Això segur que es pot millorar - ang_vel(0) = ang_mom(0) / - (p->getSensor()->getStateBlock('i')->getState()(0)); - ang_vel(1) = ang_mom(1) / - (p->getSensor()->getStateBlock('i')->getState()(1)); - ang_vel(2) = ang_mom(2) / - (p->getSensor()->getStateBlock('i')->getState()(2)); + // 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; + CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr); C->setTimeStamp(t); C->process(); - // increment de L - //dL és tota l'estona la mateixa (?) diria que si dL = torque*dt - ang_mom = ang_mom + dL; - p->getOrigin()->getFrame()->getStateBlock('P')->fix(); - p->getOrigin()->getFrame()->getStateBlock('O')->fix(); + + p->getOrigin()->getFrame()->unfix(); + p->getOrigin()->getStateBlock('P')->fix(); + p->getOrigin()->getStateBlock('O')->fix(); + report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_INFO("Estimated inertia: ", S->getStateBlock('m')->getState(), " m^2 Kg."); + 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; + } auto inertia_estimated = S->getStateBlock('i')->getState();