From 5c3c3b7f124ce03639a7280828416c991684d1d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 8 Aug 2022 16:56:05 +0200
Subject: [PATCH] Use dynamic state blocks from Sensor
---
.../factor/factor_force_torque_inertial_dynamics.h | 12 ++++++------
src/capture/capture_force_torque_inertial.cpp | 2 +-
.../processor_force_torque_inertial_dynamics.cpp | 8 ++++----
3 files changed, 11 insertions(+), 11 deletions(-)
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
index 92fe3ca..abece6e 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -149,9 +149,9 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons
_ftr->getFrame()->getStateBlock('O'), // current frame O
_ftr->getFrame()->getStateBlock('V'), // current frame V
_ftr->getFrame()->getStateBlock('L'), // current frame L
- _capture_origin->getSensor()->getStateBlock('C'), // sensor C
- _capture_origin->getSensor()->getStateBlock('i'), // sensor i
- _capture_origin->getSensor()->getStateBlock('m')), // sensor m
+ _ftr->getCapture()->getStateBlock('C'), // sensor C
+ _ftr->getCapture()->getStateBlock('i'), // sensor i
+ _ftr->getCapture()->getStateBlock('m')), // sensor m
dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
delta_preint_(_ftr->getDeltaPreint()),
calib_preint_(_ftr->getCalibrationPreint()),
@@ -238,9 +238,9 @@ inline VectorXd FactorForceTorqueInertialDynamics::residual() const
Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
Vector3d v1 = getFrame()->getStateBlock('V')->getState();
Vector3d L1 = getFrame()->getStateBlock('L')->getState();
- Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
- Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
- double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
+ Vector3d C = getCapture()->getSensor()->getStateBlockDynamic('C')->getState();
+ Vector3d i = getCapture()->getSensor()->getStateBlockDynamic('i')->getState();
+ double m = getCapture()->getSensor()->getStateBlockDynamic('m')->getState()(0);
residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
return res;
diff --git a/src/capture/capture_force_torque_inertial.cpp b/src/capture/capture_force_torque_inertial.cpp
index 6a15c13..597c093 100644
--- a/src/capture/capture_force_torque_inertial.cpp
+++ b/src/capture/capture_force_torque_inertial.cpp
@@ -18,7 +18,7 @@ CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _i
nullptr, // position is static
nullptr, // orientation is static
(_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias
- ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlock('I')->getState(), false)
+ ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlockDynamic('I')->getState(), false)
: nullptr))
{
//
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 90746a9..77bc249 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -469,10 +469,10 @@ VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseC
else
{
VectorXd calibration(13);
- const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState();
- const Vector3d& C_calib = getSensor()->getStateBlock('C')->getState();
- const Vector3d& i_calib = getSensor()->getStateBlock('i')->getState();
- const Vector1d& m_calib = getSensor()->getStateBlock('m')->getState();
+ const Vector6d& I_calib = getSensor()->getStateBlockDynamic('I')->getState();
+ const Vector3d& C_calib = getSensor()->getStateBlockDynamic('C')->getState();
+ const Vector3d& i_calib = getSensor()->getStateBlockDynamic('i')->getState();
+ const Vector1d& m_calib = getSensor()->getStateBlockDynamic('m')->getState();
calibration << I_calib, C_calib, i_calib, m_calib;
return calibration;
}
--
GitLab