Skip to content
Snippets Groups Projects
Commit f3126acd authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Use the new CaptureForceTorqueInertial 

This helps handling dynamic sensor params because it creates the dynamic state blocks automatically if required.
parent 5c3c3b7f
No related branches found
No related tags found
3 merge requests!31devel->main,!29Add functionality for UAV,!27Fix test with acc_x = 2 m/s2
Hide whitespace changes
Inline Side-by-side
include/bodydynamics/capture/capture_force_torque_inertial.h
+ 1
− 0
View file @ f3126acd
......@@ -10,6 +10,7 @@ WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial);
class CaptureForceTorqueInertial : public CaptureMotion
{
public:
CaptureForceTorqueInertial(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
......
src/processor/processor_force_torque_inertial_dynamics.cpp
+ 3
− 2
View file @ f3126acd
......@@ -30,6 +30,7 @@
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
#include "bodydynamics/capture/capture_force_torque_inertial.h"
// wolf
#include <core/state_block/state_composite.h>
......@@ -69,8 +70,8 @@ CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const Fram
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr)
{
CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
_frame_own, "CaptureMotion", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
CaptureMotionPtr capture = CaptureBase::emplace<CaptureForceTorqueInertial>(
_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
setCalibration(capture, _calib);
capture->setCalibrationPreint(_calib_preint);
return capture;
......
test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+ 24
− 24
View file @ f3126acd
......@@ -24,6 +24,7 @@
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
#include "bodydynamics/capture/capture_force_torque_inertial.h"
#include <core/sensor/factory_sensor.h>
......@@ -101,14 +102,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
S->getStateBlock('C')->fix();
S->getStateBlock('i')->fix();
S->getStateBlock('m')->unfix();
// S->setStateBlockDynamic('I');
S->setStateBlockDynamic('I');
CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
......@@ -135,10 +136,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
WOLF_INFO("-----------------------------");
CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
{
C->setTimeStamp(t);
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
p->getOrigin()->getFrame()->fix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
......@@ -172,12 +172,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
S->getStateBlock('i')->fix();
S->getStateBlock('m')->fix();
CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>( 0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>( 0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>( 0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>( 0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>( 0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>( 1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
......@@ -204,10 +204,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
{
C->setTimeStamp(t);
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix();
......@@ -240,13 +239,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->fix();
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I');
CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
......@@ -283,9 +283,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment