diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000000000000000000000000000000000000..8f284dd854c7257bc8e4af2bb9e9a056acf81d6c
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,115 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+IndentAccessModifiers: false
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+AlignEscapedNewlines: Right
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+ AfterClass: true
+ AfterControlStatement: Always
+ AfterEnum: true
+ AfterFunction: true
+ AfterNamespace: true
+ AfterObjCDeclaration: true
+ AfterStruct: true
+ AfterUnion: true
+ AfterExternBlock: false
+ BeforeCatch: true
+ BeforeElse: true
+ IndentBraces: false
+ SplitEmptyFunction: true
+ SplitEmptyRecord: true
+ SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 119
+# CommentPragmas: "^ IWYU pragma: ^\\.+"
+CommentPragmas: '^\\.+'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+ - foreach
+ - Q_FOREACH
+ - BOOST_FOREACH
+IncludeBlocks: Preserve
+# IncludeCategories:
+# - Regex: '^<pinocchio/fwd\.hpp>'
+# Priority: 1
+# - Regex: '^<ext/.*\.h>'
+# Priority: 3
+# - Regex: '^<.*\.h>'
+# Priority: 2
+# - Regex: "^<.*"
+# Priority: 3
+# - Regex: ".*"
+# Priority: 4
+IncludeIsMainRegex: "([-_](test|unittest))?$"
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index ff1e2eb36d460125006290be0be82e37296c54e9..76d222e0021420ee8c94f4ca631e8a3bbcab7526 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -112,7 +112,7 @@ stages:
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
- make -j$(nproc)
- - ctest -j$(nproc)
+ - ctest --output-on-failure -j$(nproc)
- make install
############ LICENSE HEADERS ############
diff --git a/CMakeLists.txt b/CMakeLists.txt
index abe435bf640eab5cb095bda3aed702b14e617c03..c2538cb479c738ed6becd5a484ed21ecd3c88e2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -100,55 +100,64 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
# ============ HEADERS ============
SET(HDRS_MATH
include/${PROJECT_NAME}/math/force_torque_delta_tools.h
+include/${PROJECT_NAME}/math/force_torque_inertial_delta_tools.h
)
SET(HDRS_CAPTURE
-include/${PROJECT_NAME}/capture/capture_force_torque_preint.h
+include/${PROJECT_NAME}/capture/capture_force_torque.h
+include/${PROJECT_NAME}/capture/capture_force_torque_inertial.h
include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h
include/${PROJECT_NAME}/capture/capture_leg_odom.h
include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h
)
SET(HDRS_FACTOR
+include/${PROJECT_NAME}/factor/factor_angular_momentum.h
+include/${PROJECT_NAME}/factor/factor_force_torque_inertial.h
+include/${PROJECT_NAME}/factor/factor_force_torque_inertial_dynamics.h
include/${PROJECT_NAME}/factor/factor_force_torque.h
-include/${PROJECT_NAME}/factor/factor_force_torque_preint.h
include/${PROJECT_NAME}/factor/factor_inertial_kinematics.h
include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h
include/${PROJECT_NAME}/factor/factor_point_feet_altitude.h
)
SET(HDRS_FEATURE
include/${PROJECT_NAME}/feature/feature_force_torque.h
-include/${PROJECT_NAME}/feature/feature_force_torque_preint.h
include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
)
SET(HDRS_PROCESSOR
-include/${PROJECT_NAME}/processor/processor_force_torque_preint.h
+include/${PROJECT_NAME}/processor/processor_force_torque_inertial.h
+include/${PROJECT_NAME}/processor/processor_force_torque_inertial_dynamics.h
+include/${PROJECT_NAME}/processor/processor_force_torque.h
include/${PROJECT_NAME}/processor/processor_inertial_kinematics.h
include/${PROJECT_NAME}/processor/processor_point_feet_nomove.h
)
SET(HDRS_SENSOR
include/${PROJECT_NAME}/sensor/sensor_force_torque.h
+include/${PROJECT_NAME}/sensor/sensor_force_torque_inertial.h
include/${PROJECT_NAME}/sensor/sensor_inertial_kinematics.h
include/${PROJECT_NAME}/sensor/sensor_point_feet_nomove.h
)
# ============ SOURCES ============
SET(SRCS_CAPTURE
-src/capture/capture_force_torque_preint.cpp
+src/capture/capture_force_torque.cpp
+src/capture/capture_force_torque_inertial.cpp
src/capture/capture_inertial_kinematics.cpp
src/capture/capture_leg_odom.cpp
src/capture/capture_point_feet_nomove.cpp
)
SET(SRCS_FEATURE
src/feature/feature_force_torque.cpp
-src/feature/feature_force_torque_preint.cpp
src/feature/feature_inertial_kinematics.cpp
- )
+)
SET(SRCS_PROCESSOR
-src/processor/processor_force_torque_preint.cpp
+src/processor/processor_force_torque_inertial.cpp
+src/processor/processor_force_torque_inertial_dynamics.cpp
+src/processor/processor_force_torque.cpp
src/processor/processor_inertial_kinematics.cpp
src/processor/processor_point_feet_nomove.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_force_torque.cpp
+src/sensor/sensor_force_torque_inertial.cpp
src/sensor/sensor_inertial_kinematics.cpp
src/sensor/sensor_point_feet_nomove.cpp
)
diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml
index d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8..e9d8c3b37000e40a58ac253eaec04a96f0ae4232 100644
--- a/demos/processor_ft_preint.yaml
+++ b/demos/processor_ft_preint.yaml
@@ -1,5 +1,5 @@
-type: "ProcessorForceTorquePreint" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "PFTPreint" # This is ignored. The name provided to the SensorFactory prevails
+type: "ProcessorForceTorque" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PFT" # This is ignored. The name provided to the SensorFactory prevails
time_tolerance: 0.0005 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0000000
sensor_ikin_name: "SenIK"
diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp
index a9f52035066348852f9a296504e414520669656e..02a07f6c138bab362d7a2357aa23dc63d2b6826e 100644
--- a/demos/solo_imu_kine.cpp
+++ b/demos/solo_imu_kine.cpp
@@ -68,12 +68,12 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
#include "bodydynamics/capture/capture_leg_odom.h"
#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index 82d2df1c42da66a66e6894fe49e037b4a04259cd..8ae580418ec8691b984c8a5c3a11e1c3ea81aab5 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -68,12 +68,12 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
#include "bodydynamics/capture/capture_leg_odom.h"
#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
#include "bodydynamics/capture/capture_point_feet_nomove.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 9953db41444d0d1b1da732f5460c3bd398cd3a63..181fc3d44d83f1d89755f34abb5a24f4a806e4f3 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -58,12 +58,12 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
#include "bodydynamics/capture/capture_leg_odom.h"
#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
#include "bodydynamics/capture/capture_point_feet_nomove.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
@@ -284,7 +284,7 @@ int main (int argc, char **argv) {
SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
// SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
- ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+ ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>();
params_sen_ft->sensor_ikin_name = "SenIK";
params_sen_ft->sensor_angvel_name = "SenImu";
params_sen_ft->nbc = nbc;
@@ -296,9 +296,9 @@ int main (int argc, char **argv) {
params_sen_ft->dist_traveled = 20000.0;
params_sen_ft->angle_turned = 1000;
params_sen_ft->voting_active = false;
- ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
- ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+ ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", sen_ft, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
+ ProcessorForceTorquePtr proc_ft = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
@@ -355,7 +355,7 @@ int main (int argc, char **argv) {
VectorXd meas_ikin(9); meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
CIKin0->process();
- proc_ftpreint->setOrigin(KF1);
+ proc_ft->setOrigin(KF1);
////////////////////////////////////////////
// INITIAL BIAS FACTORS
@@ -557,7 +557,7 @@ int main (int argc, char **argv) {
CImu->process();
auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
CIKin->process();
- auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
+ auto CFTpreint = std::make_shared<CaptureForceTorque>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
CFTpreint->process();
diff --git a/doc/.gitkeep b/doc/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/doc/figures/.gitkeep b/doc/figures/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/doc/figures/dyn-model.pdf b/doc/figures/dyn-model.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..e4c8ffd0da398795a7aaf4f54cf7f7967fa738c1
Binary files /dev/null and b/doc/figures/dyn-model.pdf differ
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque.h
similarity index 94%
rename from include/bodydynamics/capture/capture_force_torque_preint.h
rename to include/bodydynamics/capture/capture_force_torque.h
index 81db7bf05f90fc9048a907253b477b73f8c748a5..fab1ca4951a7ec09fef965b2176a5d8a848aee66 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque.h
@@ -32,9 +32,9 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint);
+WOLF_PTR_TYPEDEFS(CaptureForceTorque);
-class CaptureForceTorquePreint : public CaptureMotion
+class CaptureForceTorque : public CaptureMotion
{
public:
/*
@@ -51,7 +51,7 @@ class CaptureForceTorquePreint : public CaptureMotion
qbl1, 4 : orientation of foot 1 in base frame
qbl2, 4 : orientation of foot 2 in base frame
*/
- CaptureForceTorquePreint(
+ CaptureForceTorque(
const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
@@ -60,7 +60,7 @@ class CaptureForceTorquePreint : public CaptureMotion
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
- ~CaptureForceTorquePreint() override;
+ ~CaptureForceTorque() override;
CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;}
CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;}
diff --git a/src/feature/feature_force_torque_preint.cpp b/include/bodydynamics/capture/capture_force_torque_inertial.h
similarity index 56%
rename from src/feature/feature_force_torque_preint.cpp
rename to include/bodydynamics/capture/capture_force_torque_inertial.h
index 1a21081d827843645a62c75508b3ceda10794c71..b6ba6d7752cd7c4427df706e84ffedd07ab9873e 100644
--- a/src/feature/feature_force_torque_preint.cpp
+++ b/include/bodydynamics/capture/capture_force_torque_inertial.h
@@ -19,18 +19,27 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-namespace wolf {
+#ifndef CAPTURE_FORCE_TORQUE_INERTIAL_H
+#define CAPTURE_FORCE_TORQUE_INERTIAL_H
-FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::Vector6d& _biases_preint,
- const Eigen::Matrix<double,12,6>& _J_delta_biases) :
- FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
- pbc_bias_preint_(_biases_preint.head<3>()),
- gyro_bias_preint_(_biases_preint.tail<3>()),
- J_delta_bias_(_J_delta_biases)
+#include <core/capture/capture_motion.h>
+
+namespace wolf
{
-}
-} // namespace wolf
+WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial);
+
+class CaptureForceTorqueInertial : public CaptureMotion
+{
+ public:
+ CaptureForceTorqueInertial(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+ virtual ~CaptureForceTorqueInertial();
+};
+
+} // namespace wolf
+
+#endif // CAPTURE_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c120ae2eb369c9c7e373b66f5d55dbd2996f0ba
--- /dev/null
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -0,0 +1,172 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_ANGULAR_MOMENTUM_H
+#define FACTOR_ANGULAR_MOMENTUM_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
+
+/**
+ * @brief
+ *
+ * This factor evaluates the real angular velocity against the one obtained with the pre-integrated angular momentum
+ * and the moment of inertia.
+ *
+ * State blocks considered:
+ * - Frame
+ * 'L' angular momentum
+ * - Capture
+ * 'I' IMU biases
+ * - Sensor Force Torque Inertial
+ * 'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ *
+ * The residual computed has the following components, in this order
+ * - angular velocity error according to FT preintegration
+ */
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3> // State Blocks: L, I, i
+{
+ public:
+ FactorAngularMomentum(const FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ const ProcessorBasePtr& _processor, // gets access to processor and sensor
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorAngularMomentum() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _L, // ang momentum
+ const T* const _I, // IMU bias (acc and gyro)
+ const T* const _i, // inertia matrix
+ T* _res) const; // residual
+
+ template <typename D1, typename D2, typename D3, typename D4>
+ bool residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
+ MatrixBase<D4>& _res) const;
+
+ Vector3d residual() const;
+
+ private:
+ Matrix<double, 12, 1> data;
+ Matrix<double, 3, 3> sqrt_info_upper_;
+};
+
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+ const ProcessorBasePtr& _processor,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>(
+ "FactorAngularMomentum",
+ TOP_MOTION,
+ _ftr, // parent feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
+ _apply_loss_function,
+ _status,
+ _ftr->getFrame()->getStateBlock('L'), // previous frame L
+ _capture_origin->getStateBlock('I'), // previous frame IMU bias
+ _processor->getSensor()->getStateBlock('i'), // sensor i
+ data(ftr->getMeasurement()),
+ sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3)))
+{
+ //
+}
+
+template <typename D1, typename D2, typename D3, typename D4>
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
+ MatrixBase<D4>& _res) const
+{
+ MatrixSizeCheck<3, 1>::check(_res);
+
+ /* Will do the following:
+ *
+ * Compute the real angular velocity through the measurement and the IMU bias
+ * w_real = w_m - w_b
+ *
+ * Compute the angular velocity obtained by the relation between L pre-integrated and the i
+ * w_est = i^(-1)*L
+ *
+ * Compute error between them
+ * err = w_m - w_b - i^(-1)*L
+ *
+ * Compute residual
+ * res = W * err , with W the sqrt of the info matrix.
+ */
+
+ Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3);
+ double Lx = _L(0);
+ double Ly = _L(1);
+ double Lz = _L(2);
+ double ixx = _i(0);
+ double iyy = _i(1);
+ double izz = _i(2);
+ Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz];
+ Matrix<T, 3, 1> err = w_real - w_est;
+ _res = sqrt_info_upper_ * err;
+
+ return true;
+}
+
+inline Vector3d FactorAngularMomentum::residual() const
+{
+ Vector3d res(3);
+ Vector3d L = getFrame()->getStateBlock('L')->getState();
+ Vector6d I = getFeature()->getCapture()->getSensor()->getState();
+ Vector3d i = getFeature()->getSensor()->getStateBlock('i')->getState();
+
+ residual(L, I, i, res);
+ return res;
+}
+
+template <typename T>
+inline bool FactorAngularMomentum::operator()(const T* const _L,
+ const T* const _I,
+ const T* const _i,
+ T* _res) const
+{
+ Map<const Matrix<T, 3, 1>> L(_L);
+ Map<const Matrix<T, 6, 1>> I(_I);
+ Map<const Matrix<T, 3, 1>> i(_i);
+ Map<Matrix<T, 3, 1>> res(_res);
+
+ residual(L, I, i, res);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_ANGULAR_MOMENTUM_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 958398a325c0e6c77fd5b70a2cabb4497fec746b..b9ff1b4c45b3e4108c0ed1e755c7a326044a3f76 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -19,264 +19,325 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file factor_force_torque.h
- *
- * Created on: Feb 19, 2020
- * \author: mfourmy
- *
- * Works only for 2 limbs
- */
-
-
-
#ifndef FACTOR_FORCE_TORQUE_H_
#define FACTOR_FORCE_TORQUE_H_
-#include <core/math/rotations.h>
-#include <core/math/covariance.h>
-#include <core/factor/factor_autodiff.h>
-#include <core/feature/feature_base.h>
-
+//Wolf includes
+#include "bodydynamics/capture/capture_force_torque.h"
#include "bodydynamics/feature/feature_force_torque.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/math/rotations.h"
-namespace wolf
-{
+//Eigen include
+namespace wolf {
+
WOLF_PTR_TYPEDEFS(FactorForceTorque);
-class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4>
+//class
+class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>
{
public:
- FactorForceTorque(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorForceTorque() override { /* nothing */ }
-
- /*
- _ck : COM position in world frame, time k
- _cdk : COM velocity in world frame, time k
- _Lck : Angular momentum in world frame, time k
- _ckm : COM position in world frame, time k-1
- _cdkm : COM velocity in world frame, time k-1
- _Lckm : Angular momentum in world frame, time k-1
- _bpkm : COM position measurement bias, time k-1
- _qkm : Base orientation in world frame, time k-1
+ FactorForceTorque(const FeatureForceTorquePtr& _ftr_ptr,
+ const CaptureForceTorquePtr& _cap_origin_ptr, // gets to bp1, bw1
+ const ProcessorBasePtr& _processor_ptr,
+ const CaptureBasePtr& _cap_ikin_other,
+ const CaptureBasePtr& _cap_gyro_other,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorque() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
*/
template<typename T>
- bool operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const;
-
- void computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15> & _J_ny_nz) const;
-
- // void recomputeJac(const FeatureForceTorquePtr& _feat,
- // const double& _dt,
- // const Eigen::VectorXd& _bp,
- // Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
-
- void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
-
- StateBlockPtr sb_bp_other_;
- double mass_;
- double dt_;
- Eigen::Matrix<double,15,15> cov_meas_;
- // Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
- // Eigen::Matrix9d errCov_;
+ bool operator ()(const T* const _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ * params :
+ * _c1 : COM position at time t1 in world frame
+ * _cd1: COM velocity at time t1 in world frame
+ * _Lc1: Angular momentum at time t1 in world frame
+ * _q1 : Base orientation at time t1
+ * _bp1: COM position measurement at time t1 in body frame
+ * _bw1: gyroscope bias at time t1 in body frame
+ * _c2 : COM position at time t2 in world frame
+ * _cd2: COM velocity at time t2 in world frame
+ * _Lc2: Angular momentum at time t2 in world frame
+ * _q2 : Base orientation at time t2
+ * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
+ */
+ template<typename D1, typename D2, typename D3, typename D4>
+ bool residual(const MatrixBase<D1> & _c1,
+ const MatrixBase<D1> & _cd1,
+ const MatrixBase<D1> & _Lc1,
+ const QuaternionBase<D2> & _q1,
+ const MatrixBase<D1> & _bp1,
+ const MatrixBase<D1> & _bw1,
+ const MatrixBase<D1> & _c2,
+ const MatrixBase<D1> & _cd2,
+ const MatrixBase<D1> & _Lc2,
+ const QuaternionBase<D3> & _q2,
+ MatrixBase<D4> & _res) const;
+
+ // Matrix<double,12,1> residual() const;
+ // double cost() const;
+
+ private:
+ /// Preintegrated delta
+ Vector3d dc_preint_;
+ Vector3d dcd_preint_;
+ Vector3d dLc_preint_;
+ Quaterniond dq_preint_;
+
+ // Biases used during preintegration
+ Vector3d pbc_bias_preint_;
+ Vector3d gyro_bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Matrix3d J_dLc_pb_;
+ Matrix3d J_dc_wb_;
+ Matrix3d J_dcd_wb_;
+ Matrix3d J_dLc_wb_;
+ Matrix3d J_dq_wb_;
+
+ /// Metrics
+ double dt_; ///< delta-time and delta-time-squared between keyframes
+ double mass_; ///< constant total robot mass
+
};
-} /* namespace wolf */
-
-
-namespace wolf {
-
-FactorForceTorque::FactorForceTorque(
- const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorForceTorque",
- TOP_GEOM,
- _feat,
- _frame_other, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getStateBlock('C'), // COM position, current
- _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
- _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
- _frame_other->getStateBlock('C'), // COM position, previous
- _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
- _frame_other->getStateBlock('L'), // Angular momentum, previous
- _sb_bp_other, // BC relative position bias, previous
- _frame_other->getStateBlock('O') // Base orientation, previous
- ),
- sb_bp_other_(_sb_bp_other)
+///////////////////// IMPLEMENTATION ////////////////////////////
+
+inline FactorForceTorque::FactorForceTorque(
+ const FeatureForceTorquePtr& _ftr_ptr,
+ const CaptureForceTorquePtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ const CaptureBasePtr& _cap_ikin_other,
+ const CaptureBasePtr& _cap_gyro_other,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>(
+ "FactorForceTorque",
+ TOP_MOTION,
+ _ftr_ptr,
+ _cap_origin_ptr->getFrame(),
+ _cap_origin_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _cap_origin_ptr->getFrame()->getStateBlock('C'),
+ _cap_origin_ptr->getFrame()->getStateBlock('D'),
+ _cap_origin_ptr->getFrame()->getStateBlock('L'),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_ikin_other->getSensorIntrinsic(),
+ _cap_gyro_other->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getStateBlock('C'),
+ _ftr_ptr->getFrame()->getStateBlock('D'),
+ _ftr_ptr->getFrame()->getStateBlock('L'),
+ _ftr_ptr->getFrame()->getO()
+ ),
+ dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
+ dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
+ dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
+ dq_preint_(_ftr_ptr->getMeasurement().data()+9),
+ pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getPbcBiasPreint()),
+ gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getGyroBiasPreint()),
+ J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
+ J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
+ J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
+ J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
+ J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
+ mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
{
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
- mass_ = feat->getMass();
- dt_ = feat->getDt();
- retrieveMeasurementCovariance(feat, cov_meas_);
+//
}
-
-void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
+template<typename T>
+inline bool FactorForceTorque::operator ()(const T* const _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const
{
- cov.setZero();
- cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
- cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
- cov.block<3,3>(12,12) = feat->getCovPbcMeas();
-}
+ Map<const Matrix<T,3,1> > c1(_c1);
+ Map<const Matrix<T,3,1> > cd1(_cd1);
+ Map<const Matrix<T,3,1> > Lc1(_Lc1);
+ Map<const Quaternion<T> > q1(_q1);
+ Map<const Matrix<T,3,1> > bp1(_bp1);
+ Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
+ Map<const Matrix<T,3,1> > c2(_c2);
+ Map<const Matrix<T,3,1> > cd2(_cd2);
+ Map<const Matrix<T,3,1> > Lc2(_Lc2);
+ Map<const Quaternion<T> > q2(_q2);
+ Map<Matrix<T,12,1> > res(_res);
+
+ residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
+ return true;
+}
-void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
+template<typename D1, typename D2, typename D3, typename D4>
+inline bool FactorForceTorque::residual(const MatrixBase<D1> & _c1,
+ const MatrixBase<D1> & _cd1,
+ const MatrixBase<D1> & _Lc1,
+ const QuaternionBase<D2> & _q1,
+ const MatrixBase<D1> & _bp1,
+ const MatrixBase<D1> & _bw1,
+ const MatrixBase<D1> & _c2,
+ const MatrixBase<D1> & _cd2,
+ const MatrixBase<D1> & _Lc2,
+ const QuaternionBase<D3> & _q2,
+ MatrixBase<D4> & _res) const
{
- _J_ny_nz.setZero();
-
- // Measurements retrieval
- Eigen::Map<const Eigen::Vector3d> f1 (_feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (_feat->getForcesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> tau1(_feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbl1(_feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
- Eigen::Map<const Eigen::Vector3d> pbc (_feat->getPbcMeas().data());
-
- Eigen::Matrix3d bRl1 = q2R(bql1);
- Eigen::Matrix3d bRl2 = q2R(bql2);
- _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
- _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
- _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
- _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
- _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
- _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
- _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
+ /* Help for the Imu residual function
+ *
+ * Notations:
+ * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
+ * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
+ * b* : a bias
+ * J* : a Jacobian matrix
+ *
+ * We use the following functions from imu_tools.h:
+ * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
+ * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
+ * T = diff(D1,D2) : minus operator, T = D2 (-) D1
+ *
+ * Two methods are possible (select with #define below this note) :
+ *
+ * Computations common to methods 1 and 2:
+ * D_exp = betweenStates(x1,x2,dt) // Predict delta from states
+ * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
+ *
+ * Method 1:
+ * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
+ * T_err = diff(D_exp, D_corr) // compare against expected delta
+ * res = W.sqrt * T_err
+ *
+ * results in:
+ * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
+ *
+ * Method 2:
+ * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
+ * T_err = T_diff - T_step // the difference should match the correction step due to bias change
+ * res = W.sqrt * err
+ *
+ * results in :
+ * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
+ *
+ * NOTE: See optimization report at the end of this file for comparisons of both methods.
+ */
+
+ //needed typedefs
+ typedef typename D1::Scalar T;
+
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
+
+ // 1. Expected delta from states
+ Matrix<T, 3, 1 > dc_exp;
+ Matrix<T, 3, 1 > dcd_exp;
+ Matrix<T, 3, 1 > dLc_exp;
+ Quaternion<T> dq_exp;
+
+ bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
+
+ // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
+
+ // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
+ Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
+
+ // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
+ Matrix<T,3,1> dc_correct;
+ Matrix<T,3,1> dcd_correct;
+ Matrix<T,3,1> dLc_correct;
+ Quaternion<T> dq_correct;
+
+ bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
+ dc_step, dcd_step, dLc_step, do_step,
+ dc_correct, dcd_correct, dLc_correct, dq_correct);
+
+ // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
+ // Note the Dt here is zero because it's the delta-time between the same time stamps!
+ Matrix<T, 12, 1> d_error;
+ Map<Matrix<T, 3, 1> > dc_error (d_error.data() );
+ Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
+ Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
+ Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9);
+
+
+ bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
+ dc_correct, dcd_correct, dLc_correct, dq_correct,
+ dc_error, dcd_error, dLc_error, do_error);
+
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
+
+ return true;
}
-// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat,
-// const double& _dt,
-// const Eigen::VectorXd& _bp,
-// Eigen::Matrix<double, 9, 15>& _J_ny_nz)
+// Matrix<double,12,1> FactorForceTorque::residual() const
// {
-// FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
-
-// // Measurements retrieval
-// // Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
-// // Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
-// // Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
-// // Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
-// Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
-// Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
-// Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
-// Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
-// Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
-
-// Eigen::Matrix3d bRl1 = q2R(bql1);
-// Eigen::Matrix3d bRl2 = q2R(bql2);
-// // _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
-// // _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
-// // _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
-// // _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
-// _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
-// _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
-// // _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
-// }
+// Matrix<double,12,1> res;
-template<typename T>
-bool FactorForceTorque::operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const
-{
- auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
-
- // State variables instanciation
- Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdk(_cdk);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lck(_Lck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > ckm(_ckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdkm(_cdkm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lckm(_Lckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > bpkm(_bpkm);
- Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
- Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
-
- // Retrieve all measurements
- Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
-
- // Recompute the error variable covariance according to the new bias bp estimation
-
- Eigen::Matrix<double, 9, 15> J_ny_nz;
- computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz); // bp
- Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
- errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
-
- // Error variable instanciation
- Eigen::Matrix<T, 9, 1> err;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c (err.data());
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
-
- err_c = qkm.conjugate()*(ck - ckm - cdkm * dt_ - 0.5*wolf::gravity()*pow(dt_, 2))
- - (0.5/mass_) * (bql1 * f1 * pow(dt_,2) + bql2 * f2 * pow(dt_,2));
-
- err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
- - (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
-
- err_Lc = qkm.conjugate()*(Lck - Lckm)
- - ( bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1)
- + bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
-
- res = wolf::computeSqrtUpper(errCov.inverse()) * err;
+// FrameBasePtr frm_prev = getFrameOther();
+// FrameBasePtr frm_curr = getFeature()->getFrame();
- return true;
-}
+// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures...
+// auto cap_ikin_other = cap_lst.front().lock();
+// auto cap_gyro_other = cap_lst.back().lock();
+
+// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
+// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
+// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
+// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
+// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
+// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw]
+// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
+// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
+// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
+// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
+
+// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
+
+// return res;
+// }
+
+// double FactorForceTorque::cost() const
+// {
+// Matrix<double,12,1> toto = residual();
+// }
} // namespace wolf
-#endif /* FACTOR_FORCE_TORQUE_H_ */
+#endif
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial.h b/include/bodydynamics/factor/factor_force_torque_inertial.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfea278e30816f355353fbc833d4647c66cf6726
--- /dev/null
+++ b/include/bodydynamics/factor/factor_force_torque_inertial.h
@@ -0,0 +1,233 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_FORCE_TORQUE_INERTIAL_H
+#define FACTOR_FORCE_TORQUE_INERTIAL_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial);
+
+/**
+ * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
+ *
+ * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
+ *
+ * State blocks considered:
+ * - Frame 1 (origin)
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ * - Capture 1 (origin)
+ * 'I' IMU biases
+ * - Frame 2
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ *
+ * The residual computed has the following components, in this order
+ * - position delta error according to IMU preintegration
+ * - velocity delta error according to IMU preintegration
+ * - position delta error according to FT preintegration
+ * - velocity delta error according to FT preintegration
+ * - angular momentum delta error according to FT preintegration
+ * - orientation delta error according to IMU preintegration
+ */
+class FactorForceTorqueInertial
+ : public FactorAutodiff<FactorForceTorqueInertial, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3> // POVLB - POVL // TODO: add FT
+ // bias?
+{
+ public:
+ FactorForceTorqueInertial(const FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ const CaptureBasePtr& _capture_origin, // gets biases
+ const ProcessorBasePtr& _processor, // gets access to processor and sensor
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorqueInertial() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _p1, // position 1
+ const T* const _q1, // orientation
+ const T* const _v1, // velocity
+ const T* const _L1, // ang momentum
+ const T* const _b1, // IMU bias (acc and gyro)
+ const T* const _p2, // position 2
+ const T* const _q2, // orientation
+ const T* const _v2, // velocity
+ const T* const _L2, // ang momentum
+ T* _res) const; // residual
+
+ template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6>
+ bool residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ MatrixBase<D6>& _res) const;
+
+ private:
+ double dt_;
+ Matrix<double, 19, 1> delta_preint_;
+ Vector6d bias_preint_;
+ Matrix<double, 18, 6> J_delta_bias_;
+ Matrix<double, 18, 18> sqrt_info_upper_;
+};
+
+inline FactorForceTorqueInertial::FactorForceTorqueInertial(const FeatureMotionPtr& _ftr,
+ const CaptureBasePtr& _capture_origin,
+ const ProcessorBasePtr& _processor,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorForceTorqueInertial, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3>(
+ "FactorForceTorqueInertial",
+ TOP_MOTION,
+ _ftr, // parent feature
+ _capture_origin->getFrame(), // frame other
+ _capture_origin, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
+ _apply_loss_function,
+ _status,
+ _capture_origin->getFrame()->getStateBlock('P'), // previous frame P
+ _capture_origin->getFrame()->getStateBlock('O'), // previous frame O
+ _capture_origin->getFrame()->getStateBlock('V'), // previous frame V
+ _capture_origin->getFrame()->getStateBlock('L'), // previous frame L
+ _capture_origin->getSensorIntrinsic(), // previous frame IMU bias
+ _ftr->getFrame()->getStateBlock('P'), // current frame P
+ _ftr->getFrame()->getStateBlock('O'), // current frame O
+ _ftr->getFrame()->getStateBlock('V'), // current frame V
+ _ftr->getFrame()->getStateBlock('L')), // current frame L
+ dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
+ delta_preint_(_ftr->getDeltaPreint()),
+ bias_preint_(_ftr->getCalibrationPreint()),
+ J_delta_bias_(_ftr->getJacobianCalibration()),
+ sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())
+{
+ //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6>
+inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ MatrixBase<D6>& _res) const
+{
+ MatrixSizeCheck<18, 1>::check(_res);
+
+ typedef typename D4::Scalar T;
+ typedef Map<Matrix<T, 3, 1>> Matrixt;
+
+ /* Will do the following:
+ *
+ * Compute estimated delta, using betweenStates()
+ * d_est = x2 (-) x1
+ *
+ * Compute correction step according to bias change, this is a linear operation
+ * d_step = J_delta_bias * (b1 - bias_preint)
+ *
+ * Correct preintegrated delta with new bias, using plus()
+ * d_corr = d_preint (+) d_step
+ *
+ * Compute error, using diff()
+ * d_err = d_corr (-) d_est
+ *
+ * Compute residual
+ * res = W * d_err , with W the sqrt of the info matrix.
+ */
+
+ /* WARNING -- order of state blocks might be confusing!!
+ * Frame blocks are ordered as follows: "POVL" -- pos, ori, vel, ang_momentum
+ * Factor blocks are ordered as follows: "POVL-B-POVL" -- B is IMU bias
+ * Delta blocks are ordered as follows: "PVpvLO" -- where "P,V,O" are from IMU, and "p,v,L" from FT
+ */
+
+ Matrix<double, 3, 1> f;
+ Matrix<T, 19, 1> delta_est; // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt)
+ Matrixt dpi(&delta_est(0)); // 'P'
+ Matrixt dvi(&delta_est(3)); // 'V'
+ Matrixt dpd(&delta_est(6)); // 'p'
+ Matrixt dvd(&delta_est(9)); // 'v'
+ Matrixt dL(&delta_est(12)); // 'L'
+ Map<Quaternion<T>> dq(&delta_est(15)); // 'O'
+ fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
+
+ Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
+ Matrix<T, 18, 1> delta_step =
+ J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
+ Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+ Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
+ _res = sqrt_info_upper_ * delta_err;
+
+ return true;
+}
+
+template <typename T>
+inline bool FactorForceTorqueInertial::operator()(const T* const _p1,
+ const T* const _q1,
+ const T* const _v1,
+ const T* const _L1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _q2,
+ const T* const _v2,
+ const T* const _L2,
+ T* _res) const
+{
+ Map<const Matrix<T, 3, 1>> p1(_p1);
+ Map<const Quaternion<T>> q1(_q1);
+ Map<const Matrix<T, 3, 1>> v1(_v1);
+ Map<const Matrix<T, 3, 1>> L1(_L1);
+ Map<const Matrix<T, 6, 1>> b1(_b1);
+ Map<const Matrix<T, 3, 1>> p2(_p2);
+ Map<const Quaternion<T>> q2(_q2);
+ Map<const Matrix<T, 3, 1>> v2(_v2);
+ Map<const Matrix<T, 3, 1>> L2(_L2);
+ Map<Matrix<T, 18, 1>> res(_res);
+
+ residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, res);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..abece6e0d706e25da604a03d8c13f70dafdb2ed6
--- /dev/null
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -0,0 +1,285 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+#define FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorForceTorqueInertialDynamics);
+
+/**
+ * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
+ *
+ * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
+ *
+ * State blocks considered:
+ * - Frame 1 (origin)
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ * - Capture 1 (origin)
+ * 'I' IMU biases
+ * - Frame 2
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ * - Sensor Force Torque Inertial
+ * 'C' position of the center of mass
+ * 'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ * 'm' mass
+ *
+ * The residual computed has the following components, in this order
+ * - position delta error according to IMU preintegration
+ * - velocity delta error according to IMU preintegration
+ * - position delta error according to FT preintegration
+ * - velocity delta error according to FT preintegration
+ * - angular momentum delta error according to FT preintegration
+ * - orientation delta error according to IMU preintegration
+ */
+class FactorForceTorqueInertialDynamics
+ : public FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1> // POVLB - POVL
+ // - Cim
+ // // TODO: add
+ // FT bias?
+{
+ public:
+ FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ const CaptureBasePtr& _capture_origin, // gets biases
+ const ProcessorBasePtr& _processor, // gets access to processor and sensor
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorqueInertialDynamics() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _p1, // position 1
+ const T* const _q1, // orientation
+ const T* const _v1, // velocity
+ const T* const _L1, // ang momentum
+ const T* const _b1, // IMU bias (acc and gyro)
+ const T* const _p2, // position 2
+ const T* const _q2, // orientation
+ const T* const _v2, // velocity
+ const T* const _L2, // ang momentum
+ const T* const _C, // center of mass
+ const T* const _i, // inertia matrix
+ const T* const _m, // mass
+ T* _res) const; // residual
+
+ template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+ bool residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ const MatrixBase<D6>& _C,
+ const MatrixBase<D6>& _i,
+ const D7& _m,
+ MatrixBase<D8>& _res) const;
+
+ VectorXd residual() const;
+
+ VectorXd getCalibPreint() const
+ {
+ return calib_preint_;
+ }
+
+ private:
+ double dt_;
+ Matrix<double, 19, 1> delta_preint_;
+ Matrix<double, 13, 1> calib_preint_;
+ Matrix<double, 18, 13> J_delta_calib_;
+ Matrix<double, 18, 18> sqrt_info_upper_;
+};
+
+inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr,
+ const CaptureBasePtr& _capture_origin,
+ const ProcessorBasePtr& _processor,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1>(
+ "FactorForceTorqueInertialDynamics",
+ TOP_MOTION,
+ _ftr, // parent feature
+ _capture_origin->getFrame(), // frame other
+ _capture_origin, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
+ _apply_loss_function,
+ _status,
+ _capture_origin->getFrame()->getStateBlock('P'), // previous frame P
+ _capture_origin->getFrame()->getStateBlock('O'), // previous frame O
+ _capture_origin->getFrame()->getStateBlock('V'), // previous frame V
+ _capture_origin->getFrame()->getStateBlock('L'), // previous frame L
+ _capture_origin->getStateBlock('I'), // previous frame IMU bias
+ _ftr->getFrame()->getStateBlock('P'), // current frame P
+ _ftr->getFrame()->getStateBlock('O'), // current frame O
+ _ftr->getFrame()->getStateBlock('V'), // current frame V
+ _ftr->getFrame()->getStateBlock('L'), // current frame L
+ _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()),
+ J_delta_calib_(_ftr->getJacobianCalibration()),
+ sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())
+{
+ //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ const MatrixBase<D6>& _C,
+ const MatrixBase<D6>& _i,
+ const D7& _m,
+ MatrixBase<D8>& _res) const
+{
+ MatrixSizeCheck<18, 1>::check(_res);
+
+ typedef typename D4::Scalar T;
+ typedef Map<Matrix<T, 3, 1>> Matrixt;
+
+ /* Will do the following:
+ *
+ * Compute estimated delta, using betweenStates()
+ * d_est = x2 (-) x1
+ *
+ * Compute correction step according to bias change, this is a linear operation
+ * d_step = J_delta_calib * (calib - bias_preint)
+ *
+ * Correct preintegrated delta with new bias, using plus()
+ * d_corr = d_preint (+) d_step
+ *
+ * Compute error, using diff()
+ * d_err = d_corr (-) d_est
+ *
+ * Compute residual
+ * res = W * d_err , with W the sqrt of the info matrix.
+ */
+
+ /* WARNING -- order of state blocks might be confusing!!
+ * Frame blocks are ordered as follows: "POVL" -- pos, ori, vel, ang_momentum
+ * Factor blocks are ordered as follows: "POVL-B-POVL" -- B is IMU bias
+ * Delta blocks are ordered as follows: "PVpvLO" -- where "P,V,O" are from IMU, and "p,v,L" from FT
+ */
+
+ Matrix<double, 3, 1> f;
+ Matrix<T, 19, 1> delta_est; // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt)
+ Matrixt dpi(&delta_est(0)); // 'P'
+ Matrixt dvi(&delta_est(3)); // 'V'
+ Matrixt dpd(&delta_est(6)); // 'p'
+ Matrixt dvd(&delta_est(9)); // 'v'
+ Matrixt dL(&delta_est(12)); // 'L'
+ Map<Quaternion<T>> dq(&delta_est(15)); // 'O'
+ fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
+
+ Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
+ Matrix<T, 13, 1> calib;
+ calib << _b1, _C, _i, _m;
+ Matrix<T, 18, 1> delta_step = J_delta_calib_ * (calib - calib_preint_);
+ Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+ Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
+ _res = sqrt_info_upper_ * delta_err;
+
+ return true;
+}
+
+inline VectorXd FactorForceTorqueInertialDynamics::residual() const
+{
+ VectorXd res(18);
+ Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+ Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
+ Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
+ Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
+ Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = getFrame()->getStateBlock('L')->getState();
+ 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;
+}
+
+template <typename T>
+inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1,
+ const T* const _q1,
+ const T* const _v1,
+ const T* const _L1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _q2,
+ const T* const _v2,
+ const T* const _L2,
+ const T* const _C,
+ const T* const _i,
+ const T* const _m,
+ T* _res) const
+{
+ Map<const Matrix<T, 3, 1>> p1(_p1);
+ Map<const Quaternion<T>> q1(_q1);
+ Map<const Matrix<T, 3, 1>> v1(_v1);
+ Map<const Matrix<T, 3, 1>> L1(_L1);
+ Map<const Matrix<T, 6, 1>> b1(_b1);
+ Map<const Matrix<T, 3, 1>> p2(_p2);
+ Map<const Quaternion<T>> q2(_q2);
+ Map<const Matrix<T, 3, 1>> v2(_v2);
+ Map<const Matrix<T, 3, 1>> L2(_L2);
+ Map<const Matrix<T, 3, 1>> C(_C);
+ Map<const Matrix<T, 3, 1>> i(_i);
+ const T& m = *_m;
+ Map<Matrix<T, 18, 1>> res(_res);
+
+ residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, C, i, m, res);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
deleted file mode 100644
index 841bb63d6f94ffa9dc4ec115e5f4a3c3dd06d20a..0000000000000000000000000000000000000000
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ /dev/null
@@ -1,343 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-
-//Wolf includes
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/math/rotations.h"
-
-//Eigen include
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorForceTorquePreint);
-
-//class
-class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>
-{
- public:
- FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
- const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1
- const ProcessorBasePtr& _processor_ptr,
- const CaptureBasePtr& _cap_ikin_other,
- const CaptureBasePtr& _cap_gyro_other,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorForceTorquePreint() override = default;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _c1,
- const T* const _cd1,
- const T* const _Lc1,
- const T* const _q1,
- const T* const _bp1,
- const T* const _bw1,
- const T* const _c2,
- const T* const _cd2,
- const T* const _Lc2,
- const T* const _q2,
- T* _res) const;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- * params :
- * _c1 : COM position at time t1 in world frame
- * _cd1: COM velocity at time t1 in world frame
- * _Lc1: Angular momentum at time t1 in world frame
- * _q1 : Base orientation at time t1
- * _bp1: COM position measurement at time t1 in body frame
- * _bw1: gyroscope bias at time t1 in body frame
- * _c2 : COM position at time t2 in world frame
- * _cd2: COM velocity at time t2 in world frame
- * _Lc2: Angular momentum at time t2 in world frame
- * _q2 : Base orientation at time t2
- * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
- */
- template<typename D1, typename D2, typename D3, typename D4>
- bool residual(const MatrixBase<D1> & _c1,
- const MatrixBase<D1> & _cd1,
- const MatrixBase<D1> & _Lc1,
- const QuaternionBase<D2> & _q1,
- const MatrixBase<D1> & _bp1,
- const MatrixBase<D1> & _bw1,
- const MatrixBase<D1> & _c2,
- const MatrixBase<D1> & _cd2,
- const MatrixBase<D1> & _Lc2,
- const QuaternionBase<D3> & _q2,
- MatrixBase<D4> & _res) const;
-
- // Matrix<double,12,1> residual() const;
- // double cost() const;
-
- private:
- /// Preintegrated delta
- Vector3d dc_preint_;
- Vector3d dcd_preint_;
- Vector3d dLc_preint_;
- Quaterniond dq_preint_;
-
- // Biases used during preintegration
- Vector3d pbc_bias_preint_;
- Vector3d gyro_bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Matrix3d J_dLc_pb_;
- Matrix3d J_dc_wb_;
- Matrix3d J_dcd_wb_;
- Matrix3d J_dLc_wb_;
- Matrix3d J_dq_wb_;
-
- /// Metrics
- double dt_; ///< delta-time and delta-time-squared between keyframes
- double mass_; ///< constant total robot mass
-
-};
-
-///////////////////// IMPLEMENTATION ////////////////////////////
-
-inline FactorForceTorquePreint::FactorForceTorquePreint(
- const FeatureForceTorquePreintPtr& _ftr_ptr,
- const CaptureForceTorquePreintPtr& _cap_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- const CaptureBasePtr& _cap_ikin_other,
- const CaptureBasePtr& _cap_gyro_other,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
- "FactorForceTorquePreint",
- TOP_MOTION,
- _ftr_ptr,
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getStateBlock('C'),
- _cap_origin_ptr->getFrame()->getStateBlock('D'),
- _cap_origin_ptr->getFrame()->getStateBlock('L'),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_ikin_other->getSensorIntrinsic(),
- _cap_gyro_other->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getStateBlock('C'),
- _ftr_ptr->getFrame()->getStateBlock('D'),
- _ftr_ptr->getFrame()->getStateBlock('L'),
- _ftr_ptr->getFrame()->getO()
- ),
- dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
- dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
- dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
- dq_preint_(_ftr_ptr->getMeasurement().data()+9),
- pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
- gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
- J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
- J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
- J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
- J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
- J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
- mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
-{
-//
-}
-
-template<typename T>
-inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
- const T* const _cd1,
- const T* const _Lc1,
- const T* const _q1,
- const T* const _bp1,
- const T* const _bw1,
- const T* const _c2,
- const T* const _cd2,
- const T* const _Lc2,
- const T* const _q2,
- T* _res) const
-{
- Map<const Matrix<T,3,1> > c1(_c1);
- Map<const Matrix<T,3,1> > cd1(_cd1);
- Map<const Matrix<T,3,1> > Lc1(_Lc1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T,3,1> > bp1(_bp1);
- Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
- Map<const Matrix<T,3,1> > c2(_c2);
- Map<const Matrix<T,3,1> > cd2(_cd2);
- Map<const Matrix<T,3,1> > Lc2(_Lc2);
- Map<const Quaternion<T> > q2(_q2);
- Map<Matrix<T,12,1> > res(_res);
-
- residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
-
- return true;
-}
-
-template<typename D1, typename D2, typename D3, typename D4>
-inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
- const MatrixBase<D1> & _cd1,
- const MatrixBase<D1> & _Lc1,
- const QuaternionBase<D2> & _q1,
- const MatrixBase<D1> & _bp1,
- const MatrixBase<D1> & _bw1,
- const MatrixBase<D1> & _c2,
- const MatrixBase<D1> & _cd2,
- const MatrixBase<D1> & _Lc2,
- const QuaternionBase<D3> & _q2,
- MatrixBase<D4> & _res) const
-{
- /* Help for the Imu residual function
- *
- * Notations:
- * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
- * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
- * b* : a bias
- * J* : a Jacobian matrix
- *
- * We use the following functions from imu_tools.h:
- * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
- * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
- * T = diff(D1,D2) : minus operator, T = D2 (-) D1
- *
- * Two methods are possible (select with #define below this note) :
- *
- * Computations common to methods 1 and 2:
- * D_exp = betweenStates(x1,x2,dt) // Predict delta from states
- * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
- *
- * Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
- * T_err = diff(D_exp, D_corr) // compare against expected delta
- * res = W.sqrt * T_err
- *
- * results in:
- * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
- *
- * Method 2:
- * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
- * T_err = T_diff - T_step // the difference should match the correction step due to bias change
- * res = W.sqrt * err
- *
- * results in :
- * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
- *
- * NOTE: See optimization report at the end of this file for comparisons of both methods.
- */
-
- //needed typedefs
- typedef typename D1::Scalar T;
-
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
-
- // 1. Expected delta from states
- Matrix<T, 3, 1 > dc_exp;
- Matrix<T, 3, 1 > dcd_exp;
- Matrix<T, 3, 1 > dLc_exp;
- Quaternion<T> dq_exp;
-
- bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
-
- // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
-
- // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
- Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
- Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
- Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
- Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
-
- // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
- Matrix<T,3,1> dc_correct;
- Matrix<T,3,1> dcd_correct;
- Matrix<T,3,1> dLc_correct;
- Quaternion<T> dq_correct;
-
- bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
- dc_step, dcd_step, dLc_step, do_step,
- dc_correct, dcd_correct, dLc_correct, dq_correct);
-
- // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
- // Note the Dt here is zero because it's the delta-time between the same time stamps!
- Matrix<T, 12, 1> d_error;
- Map<Matrix<T, 3, 1> > dc_error (d_error.data() );
- Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
- Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
- Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9);
-
-
- bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
- dc_correct, dcd_correct, dLc_correct, dq_correct,
- dc_error, dcd_error, dLc_error, do_error);
-
- _res = getMeasurementSquareRootInformationUpper() * d_error;
-
- return true;
-}
-
-// Matrix<double,12,1> FactorForceTorquePreint::residual() const
-// {
-// Matrix<double,12,1> res;
-
-
-// FrameBasePtr frm_prev = getFrameOther();
-// FrameBasePtr frm_curr = getFeature()->getFrame();
-
-// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures...
-// auto cap_ikin_other = cap_lst.front().lock();
-// auto cap_gyro_other = cap_lst.back().lock();
-
-// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
-// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
-// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
-// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
-// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
-// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw]
-// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
-// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
-// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
-// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
-
-// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
-
-// return res;
-// }
-
-// double FactorForceTorquePreint::cost() const
-// {
-// Matrix<double,12,1> toto = residual();
-// }
-
-} // namespace wolf
-
-#endif
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index 14594821f3171f7628199e4b3b5f76026f4f7f63..c9fdd98d49b1619db1bf1d424cddfb59ae84a3b9 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -23,70 +23,73 @@
#define FEATURE_FORCE_TORQUE_H_
//Wolf includes
-#include "core/feature/feature_base.h"
+#include <core/feature/feature_base.h>
+#include <core/common/wolf.h>
//std includes
+
namespace wolf {
-
+
+//WOLF_PTR_TYPEDEFS(CaptureImu);
WOLF_PTR_TYPEDEFS(FeatureForceTorque);
-//class FeatureApriltag
class FeatureForceTorque : public FeatureBase
{
public:
- FeatureForceTorque(
- const double& _dt,
- const double& _mass,
- const Eigen::Vector6d& _forces_meas,
- const Eigen::Vector6d& _torques_meas,
- const Eigen::Vector3d& _pbc_meas,
- const Eigen::Matrix<double,14,1>& _kin_meas,
- const Eigen::Matrix6d& _cov_forces_meas,
- const Eigen::Matrix6d& _cov_torques_meas,
- const Eigen::Matrix3d& _cov_pbc_meas,
- const Eigen::Matrix<double,14,14>& _cov_kin_meas = Eigen::Matrix<double,14,14>::Zero()
- );
-
- ~FeatureForceTorque() override;
-
- const double & getDt() const {return dt_;}
- const double & getMass() const {return mass_;}
- void setDt(const double & _dt){dt_ = _dt;}
- void setMass(const double & _mass){mass_ = _mass;}
-
- const Eigen::Vector6d& getForcesMeas() const {return forces_meas_;}
- const Eigen::Vector6d& getTorquesMeas() const {return torques_meas_;}
- const Eigen::Vector3d& getPbcMeas() const {return pbc_meas_;}
- const Eigen::Matrix<double,14,1>& getKinMeas() const {return kin_meas_;}
- const Eigen::Matrix6d& getCovForcesMeas() const {return cov_forces_meas_;}
- const Eigen::Matrix6d& getCovTorquesMeas() const {return cov_torques_meas_;}
- const Eigen::Matrix3d& getCovPbcMeas() const {return cov_pbc_meas_;}
- const Eigen::Matrix<double,14,14>& getCovKinMeas() const {return cov_kin_meas_;}
-
- void setForcesMeas(const Eigen::Vector6d& _forces_meas){forces_meas_ = _forces_meas;}
- void setTorquesMeas(const Eigen::Vector6d& _torques_meas){torques_meas_ = _torques_meas;}
- void setKinMeas(const Eigen::Matrix<double,14,1>& _kin_meas){kin_meas_ = _kin_meas;}
- void setPbcMeas(const Eigen::Vector3d& _pbc_meas){pbc_meas_ = _pbc_meas;}
- void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas){cov_forces_meas_ = _cov_forces_meas;}
- void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas){cov_torques_meas_ = _cov_torques_meas;}
- void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas){cov_pbc_meas_ = _cov_pbc_meas;}
- void setCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;}
-
+ /** \brief Constructor from and measures
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
+ * \param _pbc_bias COM position relative to bias bias of origin frame time
+ * \param _gyro_bias gyroscope bias of origin frame time
+ * \param _cap_ft_ptr pointer to parent CaptureMotion (CaptureForceTorque)
+ */
+ FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double,12,6>& _J_delta_biases);
+
+// /** \brief Constructor from capture pointer
+// *
+// * \param _cap_imu_ptr pointer to parent CaptureMotion
+// */
+// FeatureForceTorque(CaptureMotionPtr _cap_imu_ptr);
+
+ ~FeatureForceTorque() override = default;
+
+ const Eigen::Vector3d& getPbcBiasPreint() const;
+ const Eigen::Vector3d& getGyroBiasPreint() const;
+ const Eigen::Matrix<double, 12, 6>& getJacobianBias() const;
+
private:
- double dt_;
- double mass_;
- Eigen::Vector6d forces_meas_;
- Eigen::Vector6d torques_meas_;
- Eigen::Vector3d pbc_meas_;
- Eigen::Matrix<double,14,1> kin_meas_;
- Eigen::Matrix6d cov_forces_meas_;
- Eigen::Matrix6d cov_torques_meas_;
- Eigen::Matrix3d cov_pbc_meas_;
- Eigen::Matrix<double,14,14> cov_kin_meas_;
+ // Used biases
+ Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration
+ Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
+
+ Eigen::Matrix<double, 12, 6> J_delta_bias_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
+inline const Eigen::Vector3d& FeatureForceTorque::getPbcBiasPreint() const
+{
+ return pbc_bias_preint_;
+}
+
+inline const Eigen::Vector3d& FeatureForceTorque::getGyroBiasPreint() const
+{
+ return gyro_bias_preint_;
+}
+
+inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorque::getJacobianBias() const
+{
+ return J_delta_bias_;
+}
+
} // namespace wolf
-#endif
+#endif // FEATURE_FORCE_TORQUE_H_
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h
deleted file mode 100644
index 9a03d2a73e668123b29dd948c7988795ea45f9cf..0000000000000000000000000000000000000000
--- a/include/bodydynamics/feature/feature_force_torque_preint.h
+++ /dev/null
@@ -1,95 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef FEATURE_FORCE_TORQUE_PREINT_H_
-#define FEATURE_FORCE_TORQUE_PREINT_H_
-
-//Wolf includes
-#include <core/feature/feature_base.h>
-#include <core/common/wolf.h>
-
-//std includes
-
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(CaptureImu);
-WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint);
-
-class FeatureForceTorquePreint : public FeatureBase
-{
- public:
-
- /** \brief Constructor from and measures
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
- * \param _pbc_bias COM position relative to bias bias of origin frame time
- * \param _gyro_bias gyroscope bias of origin frame time
- * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint)
- */
- FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::Vector6d& _biases_preint,
- const Eigen::Matrix<double,12,6>& _J_delta_biases);
-
-// /** \brief Constructor from capture pointer
-// *
-// * \param _cap_imu_ptr pointer to parent CaptureMotion
-// */
-// FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
-
- ~FeatureForceTorquePreint() override = default;
-
- const Eigen::Vector3d& getPbcBiasPreint() const;
- const Eigen::Vector3d& getGyroBiasPreint() const;
- const Eigen::Matrix<double, 12, 6>& getJacobianBias() const;
-
- private:
-
- // Used biases
- Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration
- Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
-
- Eigen::Matrix<double, 12, 6> J_delta_bias_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-};
-
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const
-{
- return pbc_bias_preint_;
-}
-
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getGyroBiasPreint() const
-{
- return gyro_bias_preint_;
-}
-
-inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobianBias() const
-{
- return J_delta_bias_;
-}
-
-} // namespace wolf
-
-#endif // FEATURE_FORCE_TORQUE_PREINT_H_
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..42081eba2c3aa4e5e1bc68cf349fa6a57de6cc20
--- /dev/null
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -0,0 +1,1498 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * force_torque_inertial_delta_tools.h
+ *
+ * Created on: Aug 8, 2021
+ * Author: jsola
+ */
+
+#ifndef MATH_FORCE_TORQUE_INERTIAL_DELTA_TOOLS_H_
+#define MATH_FORCE_TORQUE_INERTIAL_DELTA_TOOLS_H_
+
+#include <core/common/wolf.h>
+#include <core/math/rotations.h>
+#include <core/state_block/state_composite.h>
+
+/*
+ * Most functions in this file are explained in the document:
+ *
+ * https://www.overleaf.com/project/6107c87f581beb50f732ce5f
+ *
+ * The functions relate manipulations of Delta motion magnitudes used for Force torque and inertial sensors
+ * pre-integration. See the following figure for the flow of calculations of the deltas
+ *
+ * https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics/-/blob/12-add-functionality-for-aerial-vehicles/doc/figures/dyn-model.pdf
+ *
+ * The Delta, of type VectorXd(19) and size 19, is defined as
+ * Delta = [Dpi, Dvi, Dpd, Dvd, DL, Dq]
+ * with
+ * Dpi : position delta as integrated by IMU
+ * Dvi : velocity delta as integrated by IMU
+ * Dpd : position delta as integrated by force-torque
+ * Dvd : velocity delta as integrated by force-torque
+ * DL : CoM Angular momentum delta as integrated by force-torque
+ * Dq : quaternion delta as integrated by IMU
+ *
+ * In VectorComposite classes representing deltas, these blocks are identified with the following keys
+ * 'P': position delta as integrated by IMU
+ * 'V': velocity delta as integrated by IMU
+ * 'p': position delta as integrated by FT
+ * 'v': velocity delta as integrated by FT
+ * 'L': angular momentum delta as integrated by FT
+ * 'O': orientation delta as integrated by IMU
+ *
+ * When Deltas are represented in vector form VectorXd, the order of the blocks is "PVpvLO".
+ *
+ * In Frame classes, blocks are identified with the following keys
+ * 'P': position
+ * 'V': velocity
+ * 'L': angular momentum
+ * 'O': orientation
+ *
+ * When states are represented in vector form VectorXd, the order of the blocks is "PVLO".
+ * Such Frame states are therefore of size 13, VectorXd(13).
+ *
+ * Functions involving group-like operations on deltas are listed below:
+ *
+ * - identity: I = Delta at the origin,
+ * with Dpi = [0,0,0], Dvi = [0,0,0], Dpd = [0,0,0], Dvd = [0,0,0], DL = [0,0,0], Dq = [0,0,0,1]
+ * - inverse: so that D * D.inv = I
+ * - compose: Dc = D1 * D2
+ * - between: Db = D1.inv * D2, so that D2 = D1 * Db
+ * - composeOverState: x2 = x1 [*] D'
+ * where D' is a Delta with just the IMU or the FT parts for the "PV" blocks,
+ * that is "PVLO" or "pvLO",
+ * as decided by the user when providing the delta.
+ * - composeOverDelta: x2 = x1 [*] D
+ * where x is a state "PVLO" and D is a delta "PVpvLO".
+ * A second parameter `fti::Method` is provided
+ * to select between IMU "PV" and FT "pv" delta blocks to create the state "PVLO".
+ * - betweenStates: D = x2 (-) x1, so that x2 = x1 [*] D
+ * where D is a full delta "PVpvLO", but the "PV" and "pv" blocks are equal.
+ * - log_fti: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp_fti: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 * exp_fti(d)
+ * - diff: d = log_fti( D2 (-) D1 )
+ *
+ * For creating the deltas from real data, the following vectors are defined:
+ * - data[6+np] : [acc[3], gyro[3], prop_speeds[np]], with acc, gyro from IMU, prop_speeds of any size `np`
+ * - bias[12] : [acc_b[3], gyro_b[3], force_b[3], torque_b[3]] each a 3d vector
+ * - model[7] : [com[3], inertia[3], mass[1]], with
+ * - com[3] : position of CoM wrt. Base frame
+ * - inertia[3]: a 3-vector with [Ixx, Iyy, Izz], so that the inertia matrix is inertia.asDiagonal()
+ * - mass[1] : a scalar, in Kg
+ * - tangent[12]: [acc, angvel, force, torque], a vector of intermediate magnitudes, each piece a 3-vector
+ *
+ * The design of the math in this file assumes the IMU frame matches the Base frame exactly.
+ *
+ * Also a structure Propeller holds important data for a propeller:
+ * - position : position wrt Base frame
+ * - rotation : rotation matrix wrt. Base frame
+ * - c_T : thrust param, thrust = c_T * n^2, with `n` propeller speed in rad/s
+ * - c_M : torque param, torque = - dir * c_M * n^2, with `n` propeller speed in rad/s, and `dir` the blade
+ * direction below
+ * - dir_ccw : direction of blades: clockwise = -1, counter-clockwise = -1.
+ *
+ * The following functions are defined, with all Jacobians:
+ * - data2tangent : obtain tangent from data
+ * - forces2acc : obtain acceleration from forces
+ * - tangent2delta : obtain delta from tangent
+ * - data2delta : obtain deltas from data (uses the three above)
+ */
+
+namespace wolf
+{
+namespace bodydynamics
+{
+namespace fti
+{
+using namespace Eigen;
+
+/** \brief method to compose deltas with states
+ */
+enum Method
+{
+ IMU, ///< Will take IMU parts of the Delta for P and V
+ FT ///< Will take force-torque parts of the delta for P and V
+};
+
+struct Propeller
+{
+ Propeller(const Vector3d& _position,
+ const Matrix3d& _rotation,
+ const double& _direction_ccw, /// 1: Counter clock wise (CCW) seen from above; -1: CW
+ double _c_T,
+ double _c_M)
+ : position(_position), rotation(_rotation), direction_ccw(_direction_ccw), c_T(_c_T), c_M(_c_M){};
+
+ Vector3d position;
+ Matrix3d rotation;
+ double direction_ccw;
+ double c_T; // thrust
+ double c_M; // torque
+};
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6>
+inline void identity(MatrixBase<D1>& dpi,
+ MatrixBase<D2>& dvi,
+ MatrixBase<D3>& dpd,
+ MatrixBase<D4>& dvd,
+ MatrixBase<D5>& dL,
+ QuaternionBase<D6>& dq)
+{
+ dpi = MatrixBase<D1>::Zero(3, 1);
+ dvi = MatrixBase<D2>::Zero(3, 1);
+ dpd = MatrixBase<D3>::Zero(3, 1);
+ dvd = MatrixBase<D4>::Zero(3, 1);
+ dL = MatrixBase<D5>::Zero(3, 1);
+ dq = QuaternionBase<D6>::Identity();
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6>
+inline void identity(MatrixBase<D1>& dpi,
+ MatrixBase<D2>& dvi,
+ MatrixBase<D3>& dpd,
+ MatrixBase<D4>& dvd,
+ MatrixBase<D5>& dL,
+ MatrixBase<D6>& dq)
+{
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ typedef typename D3::Scalar T3;
+ typedef typename D4::Scalar T4;
+ typedef typename D5::Scalar T5;
+ typedef typename D6::Scalar T6;
+ dpi << T1(0), T1(0), T1(0);
+ dvi << T2(0), T2(0), T2(0);
+ dpd << T3(0), T3(0), T3(0);
+ dvd << T4(0), T4(0), T4(0);
+ dL << T5(0), T5(0), T5(0);
+ dq << T6(0), T6(0), T6(0), T6(1);
+}
+
+template <typename T = double>
+inline Matrix<T, 19, 1> identity()
+{
+ Matrix<T, 19, 1> ret;
+ ret << T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0),
+ T(1);
+ return ret;
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T>
+inline void inverse(MatrixBase<D1>& dpi,
+ MatrixBase<D2>& dvi,
+ MatrixBase<D3>& dpd,
+ MatrixBase<D4>& dvd,
+ MatrixBase<D5>& dL,
+ QuaternionBase<D6>& dq,
+ const T dt,
+ MatrixBase<D7>& idpi,
+ MatrixBase<D8>& idvi,
+ MatrixBase<D9>& idpd,
+ MatrixBase<D10>& idvd,
+ MatrixBase<D11>& idL,
+ QuaternionBase<D12>& idq)
+{
+ MatrixSizeCheck<3, 1>::check(dpi);
+ MatrixSizeCheck<3, 1>::check(dvi);
+ MatrixSizeCheck<3, 1>::check(dpd);
+ MatrixSizeCheck<3, 1>::check(dvd);
+ MatrixSizeCheck<3, 1>::check(dL);
+ MatrixSizeCheck<3, 1>::check(idpi);
+ MatrixSizeCheck<3, 1>::check(idvi);
+ MatrixSizeCheck<3, 1>::check(idpd);
+ MatrixSizeCheck<3, 1>::check(idvd);
+ MatrixSizeCheck<3, 1>::check(idL);
+
+ idpi = -(dq.conjugate() * (dpi - dvi * typename D3::Scalar(dt)));
+ idvi = -(dq.conjugate() * dvi);
+ idpd = -(dq.conjugate() * (dpd - dvd * typename D3::Scalar(dt)));
+ idvd = -(dq.conjugate() * dvd);
+ idL = -(dq.conjugate() * dL);
+ idq = (dq.conjugate());
+}
+
+template <typename D1, typename D2, class T>
+inline void inverse(const MatrixBase<D1>& d, T dt, MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<19, 1>::check(d);
+ MatrixSizeCheck<19, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpi(&d(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvi(&d(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpd(&d(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvd(&d(9));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dL(&d(12));
+ Map<const Quaternion<typename D1::Scalar>> dq(&d(15));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idpi(&id(0));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idvi(&id(3));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idpd(&id(6));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idvd(&id(9));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idL(&id(12));
+ Map<Quaternion<typename D1::Scalar>> idq(&id(15));
+
+ inverse(dpi, dvi, dpd, dvd, dL, dq, dt, idpi, idvi, idpd, idvd, idL, idq);
+}
+
+template <typename D, class T>
+inline Matrix<typename D::Scalar, 19, 1> inverse(const MatrixBase<D>& d, T dt)
+{
+ Matrix<typename D::Scalar, 19, 1> id;
+ inverse(d, dt, id);
+ return id;
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ typename D19,
+ typename D14,
+ typename D15,
+ typename D16,
+ typename D17,
+ typename D18,
+ class T>
+inline void compose(const MatrixBase<D1>& dpi1,
+ const MatrixBase<D2>& dvi1,
+ const MatrixBase<D3>& dpd1,
+ const MatrixBase<D4>& dvd1,
+ const MatrixBase<D5>& dL1,
+ const QuaternionBase<D6>& dq1,
+ const MatrixBase<D7>& dpi2,
+ const MatrixBase<D8>& dvi2,
+ const MatrixBase<D9>& dpd2,
+ const MatrixBase<D10>& dvd2,
+ const MatrixBase<D11>& dL2,
+ const QuaternionBase<D12>& dq2,
+ const T dt,
+ MatrixBase<D19>& sum_pi,
+ MatrixBase<D14>& sum_vi,
+ MatrixBase<D15>& sum_pd,
+ MatrixBase<D16>& sum_vd,
+ MatrixBase<D17>& sum_L,
+ QuaternionBase<D18>& sum_q)
+{
+ MatrixSizeCheck<3, 1>::check(dpi1);
+ MatrixSizeCheck<3, 1>::check(dvi1);
+ MatrixSizeCheck<3, 1>::check(dpd1);
+ MatrixSizeCheck<3, 1>::check(dvd1);
+ MatrixSizeCheck<3, 1>::check(dL1);
+ MatrixSizeCheck<3, 1>::check(dpi2);
+ MatrixSizeCheck<3, 1>::check(dvi2);
+ MatrixSizeCheck<3, 1>::check(dpd2);
+ MatrixSizeCheck<3, 1>::check(dvd2);
+ MatrixSizeCheck<3, 1>::check(dL2);
+ MatrixSizeCheck<3, 1>::check(sum_pi);
+ MatrixSizeCheck<3, 1>::check(sum_vi);
+ MatrixSizeCheck<3, 1>::check(sum_pd);
+ MatrixSizeCheck<3, 1>::check(sum_vd);
+ MatrixSizeCheck<3, 1>::check(sum_L);
+
+ sum_pi = dpi1 + dvi1 * dt + dq1 * dpi2;
+ sum_vi = dvi1 + dq1 * dvi2;
+ sum_pd = dpd1 + dvd1 * dt + dq1 * dpd2;
+ sum_vd = dvd1 + dq1 * dvd2;
+ sum_L = dL1 + dq1 * dL2;
+ sum_q = dq1 * dq2;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, MatrixBase<D3>& sum)
+{
+ MatrixSizeCheck<19, 1>::check(d1);
+ MatrixSizeCheck<19, 1>::check(d2);
+ MatrixSizeCheck<19, 1>::check(sum);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpi1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvi1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpd1(&d1(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvd1(&d1(9));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dL1(&d1(12));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(15));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpi2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvi2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpd2(&d2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvd2(&d2(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dL2(&d2(12));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(15));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_pi(&sum(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_vi(&sum(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_pd(&sum(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_vd(&sum(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_L(&sum(12));
+ Map<Quaternion<typename D3::Scalar>> sum_q(&sum(15));
+
+ compose(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, dt, sum_pi, sum_vi, sum_pd, sum_vd,
+ sum_L, sum_q);
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 19, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt)
+{
+ Matrix<typename D1::Scalar, 19, 1> ret;
+ compose(d1, d2, dt, ret);
+ return ret;
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, class T>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
+{
+ MatrixSizeCheck<19, 1>::check(d1);
+ MatrixSizeCheck<19, 1>::check(d2);
+ MatrixSizeCheck<19, 1>::check(sum);
+ MatrixSizeCheck<18, 18>::check(J_sum_d1);
+ MatrixSizeCheck<18, 18>::check(J_sum_d2);
+
+ // Some useful temporaries
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(15));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpi2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvi2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpd2(&d2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvd2(&d2(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dL2(&d2(12));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(15));
+
+ Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix();
+ Matrix<typename D2::Scalar, 3, 3> dR2 = dq2.matrix();
+
+ // JACOBIANS OF COMPOSE() JAC WRT D1 JAC WRT D2
+ // dpi1 dvi1 dpd1 dvd1 dL1 dR1 dpi2 dvi2 dpd2 dvd2 dL2 dR2
+ // ------------------------------------------ --------------------------
+ // sum_pi = dpi1 + dvi1*dt + dq1*dpi2; // I, I*dt, 0, 0 , 0, -dR1 * dpi2_x // dR1, 0, 0, 0, 0, 0
+ // sum_vi = dvi1 + dq1*dvi2; // 0, I , 0, 0 , 0, -dR1 * dvi2_x // 0, dR1, 0, 0, 0, 0
+ // sum_pd = dpd1 + dvd1*dt + dq1*dpd2; // 0, 0 , I, I*dt, 0, -dR1 * dpd2_x // 0, 0, dR1, 0, 0, 0
+ // sum_vd = dvd1 + dq1*dvd2; // 0, 0 , 0, I , 0, -dR1 * dvd2_x // 0, 0, 0, dR1, 0, 0
+ // sum_L = dL1 + dq1*dL2; // 0, 0 , 0, 0 , I, -dR1 * dL2_x // 0, 0, 0, 0, dR1, 0
+ // sum_q = dq1*dq2; // 0, 0 , 0, 0 , 0, dR2.tr // 0, 0, 0, 0, 0, I
+
+ // // Jac wrt first delta
+ J_sum_d1.setIdentity();
+ J_sum_d1.template block<3, 3>(0, 3) = Matrix3d::Identity() * dt;
+ J_sum_d1.template block<3, 3>(0, 15) = -dR1 * skew(dpi2);
+ J_sum_d1.template block<3, 3>(3, 15) = -dR1 * skew(dvi2);
+ J_sum_d1.template block<3, 3>(6, 9) = Matrix3d::Identity() * dt;
+ J_sum_d1.template block<3, 3>(6, 15) = -dR1 * skew(dpd2);
+ J_sum_d1.template block<3, 3>(9, 15) = -dR1 * skew(dvd2);
+ J_sum_d1.template block<3, 3>(12, 15) = -dR1 * skew(dL2);
+ J_sum_d1.template block<3, 3>(15, 15) = dR2.transpose();
+
+ // // // Jac wrt second delta
+ J_sum_d2.setIdentity();
+ J_sum_d2.template block<3, 3>(0, 0) = dR1;
+ J_sum_d2.template block<3, 3>(3, 3) = dR1;
+ J_sum_d2.template block<3, 3>(6, 6) = dR1;
+ J_sum_d2.template block<3, 3>(9, 9) = dR1;
+ J_sum_d2.template block<3, 3>(12, 12) = dR1;
+
+ // compose deltas -- done here to avoid aliasing when calling with input
+ // `d1` and result `sum` referencing the same variable
+ compose(d1, d2, dt, sum);
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ typename D13,
+ typename D14,
+ typename D15,
+ typename D16,
+ typename D17,
+ typename D18,
+ class T>
+inline void between(const MatrixBase<D1>& dpi1,
+ const MatrixBase<D2>& dvi1,
+ const MatrixBase<D3>& dpd1,
+ const MatrixBase<D4>& dvd1,
+ const MatrixBase<D5>& dL1,
+ const QuaternionBase<D6>& dq1,
+ const MatrixBase<D7>& dpi2,
+ const MatrixBase<D8>& dvi2,
+ const MatrixBase<D9>& dpd2,
+ const MatrixBase<D10>& dvd2,
+ const MatrixBase<D11>& dL2,
+ const QuaternionBase<D12>& dq2,
+ const T dt,
+ MatrixBase<D13>& bet_pi,
+ MatrixBase<D14>& bet_vi,
+ MatrixBase<D15>& bet_pd,
+ MatrixBase<D16>& bet_vd,
+ MatrixBase<D17>& bet_L,
+ QuaternionBase<D18>& bet_q)
+{
+ MatrixSizeCheck<3, 1>::check(dpi1);
+ MatrixSizeCheck<3, 1>::check(dvi1);
+ MatrixSizeCheck<3, 1>::check(dpd1);
+ MatrixSizeCheck<3, 1>::check(dvd1);
+ MatrixSizeCheck<3, 1>::check(dL1);
+ MatrixSizeCheck<3, 1>::check(dpi2);
+ MatrixSizeCheck<3, 1>::check(dvi2);
+ MatrixSizeCheck<3, 1>::check(dpd2);
+ MatrixSizeCheck<3, 1>::check(dvd2);
+ MatrixSizeCheck<3, 1>::check(dL2);
+ MatrixSizeCheck<3, 1>::check(bet_pi);
+ MatrixSizeCheck<3, 1>::check(bet_vi);
+ MatrixSizeCheck<3, 1>::check(bet_pd);
+ MatrixSizeCheck<3, 1>::check(bet_vd);
+ MatrixSizeCheck<3, 1>::check(bet_L);
+
+ bet_pi = dq1.conjugate() * (dpi2 - dpi1 - dvi1 * dt);
+ bet_vi = dq1.conjugate() * (dvi2 - dvi1);
+ bet_pd = dq1.conjugate() * (dpd2 - dpd1 - dvd1 * dt);
+ bet_vd = dq1.conjugate() * (dvd2 - dvd1);
+ bet_L = dq1.conjugate() * (dL2 - dL1);
+ bet_q = dq1.conjugate() * dq2;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, MatrixBase<D3>& d2_minus_d1)
+{
+ MatrixSizeCheck<19, 1>::check(d1);
+ MatrixSizeCheck<19, 1>::check(d2);
+ MatrixSizeCheck<19, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpi1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvi1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpd1(&d1(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvd1(&d1(9));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dL1(&d1(12));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(15));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpi2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvi2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpd2(&d2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvd2(&d2(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dL2(&d2(12));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(15));
+ Map<Matrix<typename D3::Scalar, 3, 1>> bet_pi(&d2_minus_d1(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> bet_vi(&d2_minus_d1(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> bet_pd(&d2_minus_d1(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> bet_vd(&d2_minus_d1(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> bet_L(&d2_minus_d1(12));
+ Map<Quaternion<typename D3::Scalar>> bet_q(&d2_minus_d1(15));
+
+ between(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, dt, bet_pi, bet_vi, bet_pd, bet_vd,
+ bet_L, bet_q);
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 19, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt)
+{
+ Matrix<typename D1::Scalar, 19, 1> diff;
+ between(d1, d2, dt, diff);
+ return diff;
+}
+
+inline VectorComposite composeOverState(const VectorComposite& x,
+ const VectorComposite& d,
+ double dt,
+ Method method = IMU)
+{
+ // Maps to have nice names
+ Map<const Vector3d> p(x.at('P').data()); // state POVL
+ Map<const Vector3d> v(x.at('V').data());
+ Map<const Quaterniond> q(x.at('O').data());
+ Map<const Vector3d> L(x.at('L').data());
+ Map<const Vector3d> dpi(d.at('P').data()); // delta PVO from IMU
+ Map<const Vector3d> dvi(d.at('V').data());
+ Map<const Quaterniond> dq(d.at('O').data());
+ Map<const Vector3d> dpd(d.at('p').data()); // delta pvL from FT
+ Map<const Vector3d> dvd(d.at('v').data());
+ Map<const Vector3d> dL(d.at('L').data());
+
+ const Vector3d& g_dt = gravity() * dt;
+
+ VectorComposite x_plus_d;
+ switch (method)
+ {
+ case IMU:
+ default:
+ x_plus_d['P'] = p + v * dt + 0.5 * g_dt * dt + q * dpi;
+ x_plus_d['V'] = v + g_dt + q * dvi;
+ break;
+ case FT:
+ x_plus_d['P'] = p + v * dt + 0.5 * g_dt * dt + q * dpd;
+ x_plus_d['V'] = v + g_dt + q * dvd;
+ break;
+ }
+ x_plus_d['L'] = L + q * dL;
+ x_plus_d['O'] = (q * dq).coeffs();
+
+ return x_plus_d;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt,
+ MatrixBase<D3>& x_plus_d,
+ Method method = IMU)
+{
+ MatrixSizeCheck<13, 1>::check(x);
+ MatrixSizeCheck<19, 1>::check(d);
+ MatrixSizeCheck<13, 1>::check(x_plus_d);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p(&x(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> v(&x(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> L(&x(6));
+ Map<const Quaternion<typename D1::Scalar>> q(&x(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpi(&d(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvi(&d(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpd(&d(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvd(&d(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dL(&d(12));
+ Map<const Quaternion<typename D2::Scalar>> dq(&d(15));
+ Map<Matrix<typename D3::Scalar, 3, 1>> p_plus_d(&x_plus_d(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> v_plus_d(&x_plus_d(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> L_plus_d(&x_plus_d(6));
+ Map<Quaternion<typename D3::Scalar>> q_plus_d(&x_plus_d(9));
+
+ const Vector3d& g_dt = gravity() * dt;
+
+ switch (method)
+ {
+ case IMU:
+ default:
+ p_plus_d = p + v * dt + 0.5 * g_dt * dt + q * dpi;
+ v_plus_d = v + g_dt + q * dvi;
+ break;
+ case FT:
+ p_plus_d = p + v * dt + 0.5 * g_dt * dt + q * dpd;
+ v_plus_d = v + g_dt + q * dvd;
+ break;
+ }
+ L_plus_d = L + q * dL;
+ q_plus_d = q * dq;
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt,
+ Method method = IMU)
+{
+ Matrix<typename D1::Scalar, 13, 1> ret;
+ composeOverState(x, d, dt, ret, method);
+ return ret;
+}
+
+template <typename D1,
+ typename D2,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D11,
+ typename D12,
+ typename D13,
+ typename D14,
+ typename D15,
+ typename D16,
+ typename D17,
+ typename D18,
+ class T>
+inline void betweenStates(const MatrixBase<D1>& p1,
+ const MatrixBase<D2>& v1,
+ const MatrixBase<D5>& L1,
+ const QuaternionBase<D6>& q1,
+ const MatrixBase<D7>& p2,
+ const MatrixBase<D8>& v2,
+ const MatrixBase<D11>& L2,
+ const QuaternionBase<D12>& q2,
+ const T dt,
+ MatrixBase<D13>& dpi,
+ MatrixBase<D14>& dvi,
+ MatrixBase<D15>& dpd,
+ MatrixBase<D16>& dvd,
+ MatrixBase<D17>& dL,
+ QuaternionBase<D18>& dq)
+{
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(v1);
+ MatrixSizeCheck<3, 1>::check(L1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(v2);
+ MatrixSizeCheck<3, 1>::check(L2);
+ MatrixSizeCheck<3, 1>::check(dpi);
+ MatrixSizeCheck<3, 1>::check(dvi);
+ MatrixSizeCheck<3, 1>::check(dpd);
+ MatrixSizeCheck<3, 1>::check(dvd);
+ MatrixSizeCheck<3, 1>::check(dL);
+
+ dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * gravity() * dt * dt);
+ dvi = q1.conjugate() * (v2 - v1 - gravity() * dt);
+ dpd = dpi;
+ dvd = dvi;
+ dL = q1.conjugate() * (L2 - L1);
+ dq = q1.conjugate() * q2;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
+{
+ MatrixSizeCheck<13, 1>::check(x1);
+ MatrixSizeCheck<13, 1>::check(x2);
+ MatrixSizeCheck<19, 1>::check(x2_minus_x1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&x1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> v1(&x1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> L1(&x1(6));
+ Map<const Quaternion<typename D1::Scalar>> q1(&x1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&x2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> v2(&x2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> L2(&x2(6));
+ Map<const Quaternion<typename D2::Scalar>> q2(&x2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dpi(&x2_minus_x1(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dvi(&x2_minus_x1(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dpd(&x2_minus_x1(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dvd(&x2_minus_x1(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dL(&x2_minus_x1(12));
+ Map<Quaternion<typename D3::Scalar>> dq(&x2_minus_x1(15));
+
+ betweenStates(p1, v1, L1, q1, p2, v2, L2, q2, dt, dpi, dvi, dpd, dvd, dL, dq);
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
+{
+ Matrix<typename D1::Scalar, 19, 1> ret;
+ betweenStates(x1, x2, dt, ret);
+ return ret;
+}
+
+inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret)
+{
+ Map<const Quaterniond> q1(x1.at('O').data());
+ Map<const Quaterniond> q2(x2.at('O').data());
+
+ ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * gravity() * dt * dt);
+ ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - gravity() * dt);
+ ret['p'] = ret.at('P');
+ ret['v'] = ret.at('V');
+ ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L'));
+ ret['O'] = (q1.conjugate() * q2).coeffs();
+}
+
+inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt)
+{
+ VectorComposite ret;
+ betweenStates(x1, x2, dt, ret);
+ return ret;
+}
+
+template <typename Derived>
+Matrix<typename Derived::Scalar, 18, 1> log_fti(const MatrixBase<Derived>& delta_in)
+{
+ MatrixSizeCheck<19, 1>::check(delta_in);
+
+ Matrix<typename Derived::Scalar, 18, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dpi_in(&delta_in(0));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dvi_in(&delta_in(3));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dpd_in(&delta_in(6));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dvd_in(&delta_in(9));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dL_in(&delta_in(12));
+ Map<const Quaternion<typename Derived::Scalar>> dq_in(&delta_in(15));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dpi_ret(&ret(0));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dvi_ret(&ret(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dpd_ret(&ret(6));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dvd_ret(&ret(9));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dL_ret(&ret(12));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> do_ret(&ret(15));
+
+ dpi_ret = dpi_in;
+ dvi_ret = dvi_in;
+ dpd_ret = dpd_in;
+ dvd_ret = dvd_in;
+ dL_ret = dL_in;
+ do_ret = log_q(dq_in);
+
+ return ret;
+}
+
+template <typename Derived>
+Matrix<typename Derived::Scalar, 19, 1> exp_fti(const MatrixBase<Derived>& d_alg_in)
+{
+ MatrixSizeCheck<18, 1>::check(d_alg_in);
+
+ Matrix<typename Derived::Scalar, 19, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dpi_alg_in(&d_alg_in(0));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dvi_alg_in(&d_alg_in(3));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dpd_alg_in(&d_alg_in(6));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dvd_alg_in(&d_alg_in(9));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dL_alg_in(&d_alg_in(12));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> do_alg_in(&d_alg_in(15));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dpi_ret(&ret(0));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dvi_ret(&ret(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dpd_ret(&ret(6));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dvd_ret(&ret(9));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dL_ret(&ret(12));
+ Map<Quaternion<typename Derived::Scalar>> dq_ret(&ret(15));
+
+ dpi_ret = dpi_alg_in;
+ dvi_ret = dvi_alg_in;
+ dpd_ret = dpd_alg_in;
+ dvd_ret = dvd_alg_in;
+ dL_ret = dL_alg_in;
+ dq_ret = exp_q(do_alg_in);
+
+ return ret;
+}
+
+template <typename D1, typename D2, typename D3>
+void exp_fti(const MatrixBase<D1>& _tangent, MatrixBase<D2>& _delta, MatrixBase<D3>& _jacobian_delta_tangent)
+{
+ MatrixSizeCheck<18, 1>::check(_tangent);
+ MatrixSizeCheck<19, 1>::check(_delta);
+ MatrixSizeCheck<18, 18>::check(_jacobian_delta_tangent);
+
+ _delta = fti::exp_fti(_tangent);
+ _jacobian_delta_tangent.setIdentity();
+ _jacobian_delta_tangent.template block<3, 3>(15, 15) = jac_SO3_right(_tangent.template segment<3>(15));
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ typename D13,
+ typename D14,
+ typename D15,
+ typename D16,
+ typename D17,
+ typename D18>
+inline void plus(const MatrixBase<D1>& dpi1,
+ const MatrixBase<D2>& dvi1,
+ const MatrixBase<D3>& dpd1,
+ const MatrixBase<D4>& dvd1,
+ const MatrixBase<D5>& dL1,
+ const QuaternionBase<D6>& dq1,
+ const MatrixBase<D7>& diff_pi2,
+ const MatrixBase<D8>& diff_vi2,
+ const MatrixBase<D9>& diff_pd2,
+ const MatrixBase<D10>& diff_vd2,
+ const MatrixBase<D11>& diff_L2,
+ const MatrixBase<D12>& diff_o2,
+ MatrixBase<D13>& plus_pi,
+ MatrixBase<D14>& plus_vi,
+ MatrixBase<D15>& plus_pd,
+ MatrixBase<D16>& plus_vd,
+ MatrixBase<D17>& plus_L,
+ QuaternionBase<D18>& plus_q)
+{
+ plus_pi = dpi1 + diff_pi2;
+ plus_vi = dvi1 + diff_vi2;
+ plus_pd = dpd1 + diff_pd2;
+ plus_vd = dvd1 + diff_vd2;
+ plus_L = dL1 + diff_L2;
+ plus_q = dq1 * exp_q(diff_o2);
+}
+
+// Composite plus
+template <typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& diff2, MatrixBase<D3>& d)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpi1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvi1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpd1(&d1(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvd1(&d1(9));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dL1(&d1(12));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(15));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dpi2(&diff2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dvi2(&diff2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dpd2(&diff2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dvd2(&diff2(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dL2(&diff2(12));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_do2(&diff2(15));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_pi(&d(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_vi(&d(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_pd(&d(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_vd(&d(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_L(&d(12));
+ Map<Quaternion<typename D3::Scalar>> d_q(&d(15));
+
+ plus(dpi1, dvi1, dpd1, dvd1, dL1, dq1, diff_dpi2, diff_dvi2, diff_dpd2, diff_dvd2, diff_dL2, diff_do2, d_pi, d_vi,
+ d_pd, d_vd, d_L, d_q);
+}
+
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 19, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 19, 1> ret;
+ plus(d1, d2, ret);
+ return ret;
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ typename D13,
+ typename D14,
+ typename D15,
+ typename D16,
+ typename D17,
+ typename D18>
+inline void diff(const MatrixBase<D1>& dpi1,
+ const MatrixBase<D2>& dvi1,
+ const MatrixBase<D3>& dpd1,
+ const MatrixBase<D4>& dvd1,
+ const MatrixBase<D5>& dL1,
+ const QuaternionBase<D6>& dq1,
+ const MatrixBase<D7>& dpi2,
+ const MatrixBase<D8>& dvi2,
+ const MatrixBase<D9>& dpd2,
+ const MatrixBase<D10>& dvd2,
+ const MatrixBase<D11>& dL2,
+ const QuaternionBase<D12>& dq2,
+ MatrixBase<D13>& diff_pi,
+ MatrixBase<D14>& diff_vi,
+ MatrixBase<D15>& diff_pd,
+ MatrixBase<D16>& diff_vd,
+ MatrixBase<D17>& diff_L,
+ MatrixBase<D18>& diff_o)
+{
+ diff_pi = dpi2 - dpi1;
+ diff_vi = dvi2 - dvi1;
+ diff_pd = dpd2 - dpd1;
+ diff_vd = dvd2 - dvd1;
+ diff_L = dL2 - dL1;
+ diff_o = log_q(dq1.conjugate() * dq2);
+}
+
+template <typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& diff_d1_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpi1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvi1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dpd1(&d1(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dvd1(&d1(9));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dL1(&d1(12));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(15));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpi2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvi2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dpd2(&d2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dvd2(&d2(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dL2(&d2(12));
+ Map<const Quaternion<typename D1::Scalar>> dq2(&d2(15));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_pi(&diff_d1_d2(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_vi(&diff_d1_d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_pd(&diff_d1_d2(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_vd(&diff_d1_d2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_L(&diff_d1_d2(12));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_o(&diff_d1_d2(15));
+
+ diff(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, diff_pi, diff_vi, diff_pd, diff_vd,
+ diff_L, diff_o);
+}
+
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 18, 1> diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 18, 1> ret;
+ diff(d1, d2, ret);
+ return ret;
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12>
+void data2tangent(const MatrixBase<D1>& _data_acc,
+ const MatrixBase<D2>& _data_angvel,
+ const MatrixBase<D3>& _data_motor_speeds,
+ const MatrixBase<D4>& _bias_acc,
+ const MatrixBase<D5>& _bias_angvel,
+ const MatrixBase<D6>& _bias_force,
+ const MatrixBase<D7>& _bias_torque,
+ const MatrixBase<D8>& _com,
+ const std::list<Propeller>& _propellers,
+ MatrixBase<D9>& _tangent_acc,
+ MatrixBase<D10>& _tangent_angvel,
+ MatrixBase<D11>& _tangent_force,
+ MatrixBase<D12>& _tangent_torque)
+{
+ // check sizes
+ // TODO
+
+ // helpers
+ const auto& r_bc = _com;
+
+ _tangent_force.setZero();
+ _tangent_torque.setZero();
+
+ unsigned int i = 0;
+ for (const Propeller& propeller : _propellers)
+ {
+ const Vector3d& r_bi = propeller.position;
+ const Matrix3d& R_bi = propeller.rotation;
+ double direction = propeller.direction_ccw;
+ const double& c_T = propeller.c_T;
+ const double& c_M = propeller.c_M;
+ const double c_M_by_c_T = c_M / c_T;
+
+ const auto& n_i = _data_motor_speeds(i);
+
+ Vector3d f_bi = R_bi.rightCols(1) * c_T * n_i * n_i;
+
+ _tangent_force += f_bi;
+ _tangent_torque += (r_bi - r_bc).cross(f_bi) - direction * c_M_by_c_T * f_bi;
+
+ i++;
+ }
+
+ _tangent_acc = _data_acc - _bias_acc;
+ _tangent_angvel = _data_angvel - _bias_angvel;
+ _tangent_force += _bias_force;
+ _tangent_torque += _bias_torque;
+}
+
+template <typename D1, typename D2, typename D3, typename D4>
+void data2tangent(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ const MatrixBase<D3>& _com,
+ const std::list<Propeller>& _propellers,
+ MatrixBase<D4>& _tangent)
+{
+ const int Np = _data.size() - 6;
+ Map<const Matrix<typename D1::Scalar, 3, 1>> am(&_data(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> wm(&_data(3));
+ Map<const Matrix<typename D1::Scalar, Dynamic, 1>> n(&_data(6), Np);
+
+ Map<const Matrix<typename D2::Scalar, 3, 1>> ab(&_bias(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> wb(&_bias(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> fb(&_bias(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> mb(&_bias(9));
+
+ Map<Matrix<typename D4::Scalar, 3, 1>> a(&_tangent(0));
+ Map<Matrix<typename D4::Scalar, 3, 1>> w(&_tangent(3));
+ Map<Matrix<typename D4::Scalar, 3, 1>> f(&_tangent(6));
+ Map<Matrix<typename D4::Scalar, 3, 1>> m(&_tangent(9));
+
+ data2tangent(am, wm, n, ab, wb, fb, mb, _com, _propellers, a, w, f, m);
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+void data2tangent(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ const MatrixBase<D3>& _com,
+ const std::list<Propeller>& _propellers,
+ MatrixBase<D4>& _tangent,
+ MatrixBase<D5>& _J_tan_data,
+ MatrixBase<D6>& _J_tan_bias,
+ MatrixBase<D7>& _J_tan_com)
+{
+ const unsigned int np = _propellers.size();
+ Map<const Matrix<typename D1::Scalar, 3, 1>> am(&_data(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> wm(&_data(3));
+ Map<const Matrix<typename D1::Scalar, Dynamic, 1>> n(&_data(6), np);
+ Map<const Matrix<typename D2::Scalar, 3, 1>> ab(&_bias(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> wb(&_bias(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> fb(&_bias(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> mb(&_bias(9));
+
+ Map<Matrix<typename D4::Scalar, 3, 1>> a(&_tangent(0));
+ Map<Matrix<typename D4::Scalar, 3, 1>> w(&_tangent(3));
+ Map<Matrix<typename D4::Scalar, 3, 1>> f(&_tangent(6));
+ Map<Matrix<typename D4::Scalar, 3, 1>> m(&_tangent(9));
+
+ data2tangent(_data, _bias, _com, _propellers, _tangent);
+
+ // blocks
+ const unsigned int A = 0; // acc
+ const unsigned int W = 3; // gyro
+ const unsigned int F = 6; // force
+ const unsigned int M = 9; // torque
+ const unsigned int N = 6; // motor speeds
+ const unsigned int C = 0; // CoM
+
+ const Matrix3d I_33 = Matrix3d::Identity();
+
+ // jac wrt data
+ _J_tan_data.setIdentity();
+ // _J_tan_data.block<3,3>(A,A) = I_33;
+ // _J_tan_data.block<3,3>(W,W) = I_33;
+
+ unsigned int i = 0;
+ for (const Propeller& propeller : _propellers)
+ {
+ const auto& ri = propeller.position;
+ const auto& Ri = propeller.rotation;
+ const auto& dir = propeller.direction_ccw;
+ const auto& cTi = propeller.c_T;
+ const auto& cMi = propeller.c_M;
+ const auto& ni = n(i);
+
+ Vector3d J_fi_ni = 2 * cTi * ni * Ri.rightCols(1);
+
+ Vector3d J_mi_ni = skew(ri - _com) * J_fi_ni - 2 * dir * cMi * ni * Ri.rightCols(1);
+
+ _J_tan_data.block(F, N + i, 3, 1) = J_fi_ni;
+ _J_tan_data.block(M, N + i, 3, 1) = J_mi_ni;
+
+ i++;
+ }
+
+ // jac wrt bias
+ _J_tan_bias.setIdentity();
+ _J_tan_bias.template block<3, 3>(A, A) = -I_33;
+ _J_tan_bias.template block<3, 3>(W, W) = -I_33;
+ // _J_tan_bias.block<3,3>(F,F) = I_33;
+ // _J_tan_bias.block<3,3>(M,M) = I_33;
+
+ // jac wrt com
+ _J_tan_com.setZero();
+ // __J_tan_com.block<3,3>(A,C) = 0;
+ // __J_tan_com.block<3,3>(W,C) = 0;
+ // __J_tan_com.block<3,3>(F,C) = 0;
+
+ // The total torque is
+ // m = sum_i m_i
+ // with
+ // m_i = (r_i - r_com) x f_bi + (terms not related to r_com).
+ // Then,
+ // J_mi_com = [f_bi]x
+ // and thus
+ // J_m_com = sum_i [f_bi]x .
+ // But since for any set of a_i
+ // sum [a_i]x = [sum a_i]x
+ // we have that
+ // J_m_com = sum [f_bi]x = [sum fbi]x = [ft]x
+ // where
+ // ft = sum_i f_bi
+ // is the total force. To get it, remove the bias which was added in the function:
+ // ft = f - fb
+
+ _J_tan_com.template block<3, 3>(M, C) = skew(f - fb);
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T>
+void forces2acc(const MatrixBase<D1>& _force,
+ const MatrixBase<D2>& _torque,
+ const MatrixBase<D3>& _angvel,
+ const MatrixBase<D4>& _com,
+ const MatrixBase<D5>& _inertia,
+ const T _mass,
+ MatrixBase<D6>& _acc)
+{
+ typedef typename D6::Scalar TT; // take scalar from return type
+ typedef Matrix<TT, 3, 3> Matrix3t;
+ typedef Matrix<TT, 3, 1> Vector3t;
+
+ const Matrix3t& I = _inertia.asDiagonal();
+ const Matrix3t& I_inv = ((typename D5::Scalar)(1.0) / _inertia.array()).matrix().asDiagonal();
+
+ const Vector3t& angacc = I_inv * (_torque - _angvel.cross(I * _angvel));
+
+ _acc = _force / _mass - angacc.cross(_com) - _angvel.cross(_angvel.cross(_com));
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T>
+void forces2acc(const MatrixBase<D1>& _force,
+ const MatrixBase<D2>& _torque,
+ const MatrixBase<D3>& _angvel,
+ const MatrixBase<D4>& _com,
+ const MatrixBase<D5>& _inertia,
+ const T _mass,
+ MatrixBase<D6>& _acc,
+ MatrixBase<D7>& _J_acc_force,
+ MatrixBase<D8>& _J_acc_torque,
+ MatrixBase<D9>& _J_acc_angvel,
+ MatrixBase<D10>& _J_acc_com,
+ MatrixBase<D11>& _J_acc_inertia,
+ MatrixBase<D12>& _J_acc_mass)
+{
+ typedef typename D6::Scalar S; // take scalar from return type
+ typedef Matrix<S, 3, 3> Matrix3t;
+ typedef Matrix<S, 3, 1> Vector3t;
+
+ // constants
+ const Matrix3t& I = _inertia.asDiagonal();
+ const Matrix3t& I_inv = ((typename D5::Scalar)(1.0) / _inertia.array()).matrix().asDiagonal();
+
+ // variables
+ const Vector3t& angacc = I_inv * (_torque - _angvel.cross(I * _angvel)); // (36)
+ const Matrix3t& angvel_x = skew(_angvel);
+ const Matrix3t& com_x = skew(_com);
+ const Matrix3t& angacc_x = skew(angacc);
+
+ _acc = _force / _mass - angacc.cross(_com) - _angvel.cross(_angvel.cross(_com)); // (35)
+
+ // Jacobian blocks
+ const Matrix3t& tmp = (_angvel.cross(I * _angvel) - _torque).asDiagonal();
+ const Matrix3t& J_Iw_i = _angvel.asDiagonal(); // (58)
+ const Matrix3t& J_angacc_inertia = I_inv * (I_inv * tmp - angvel_x * J_Iw_i); // (57)
+ const Matrix3t& J_acc_angacc = com_x; // (56)
+ const Matrix3t& J_angacc_torque = I_inv; // (53)
+ const Matrix3t& J_angvelcom_angvel = -com_x; // (39)
+ const Matrix3t& J_angacc_angvel = I_inv * (skew(I * _angvel) - angvel_x * I); // (38)
+
+ // Output Jacobians
+ _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
+ _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
+ _J_acc_angvel =
+ J_acc_angacc * J_angacc_angvel - angvel_x * J_angvelcom_angvel + skew(_angvel.cross(_com)); // (37)
+ _J_acc_com = -angacc_x - angvel_x * angvel_x; // (51)
+ _J_acc_inertia = J_acc_angacc * J_angacc_inertia; // (55)
+ _J_acc_mass = -_force / (_mass * _mass); // (50)
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T,
+ class DT>
+void tangent2delta(const MatrixBase<D1>& _tangent_acc_imu,
+ const MatrixBase<D2>& _tangent_angvel_imu,
+ const MatrixBase<D3>& _tangent_force,
+ const MatrixBase<D4>& _tangent_torque,
+ const MatrixBase<D5>& _com,
+ const MatrixBase<D6>& _inertia,
+ const T _mass,
+ const DT _dt,
+ MatrixBase<D7>& _delta_p_imu,
+ MatrixBase<D8>& _delta_v_imu,
+ MatrixBase<D9>& _delta_p_dyn,
+ MatrixBase<D10>& _delta_v_dyn,
+ MatrixBase<D11>& _delta_L,
+ QuaternionBase<D12>& _delta_q)
+{
+ DT dt2 = _dt * _dt;
+
+ Matrix<typename D3::Scalar, 3, 1> acc_d;
+
+ forces2acc(_tangent_force, _tangent_torque, _tangent_angvel_imu, _com, _inertia, _mass, acc_d);
+
+ _delta_p_imu = 0.5 * _tangent_acc_imu * dt2;
+ _delta_v_imu = _tangent_acc_imu * _dt;
+ _delta_p_dyn = 0.5 * acc_d * dt2;
+ _delta_v_dyn = acc_d * _dt;
+ _delta_L = _tangent_torque * _dt;
+ _delta_q = exp_q(_tangent_angvel_imu * _dt);
+}
+
+template <typename D1, typename D2, typename D3, typename D4, class T, class DT>
+void tangent2delta(const MatrixBase<D1>& _tangent,
+ const MatrixBase<D2>& _com,
+ const MatrixBase<D3>& _inertia,
+ const T _mass,
+ const DT _dt,
+ MatrixBase<D4>& _delta)
+{
+ // use shorter names
+ Map<const Matrix<typename D1::Scalar, 3, 1>> acc_i(&_tangent(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> angvel(&_tangent(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> force(&_tangent(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> torque(&_tangent(9));
+ Map<Matrix<typename D4::Scalar, 3, 1>> _delta_p_imu(&_delta(0));
+ Map<Matrix<typename D4::Scalar, 3, 1>> _delta_v_imu(&_delta(3));
+ Map<Matrix<typename D4::Scalar, 3, 1>> _delta_p_dyn(&_delta(6));
+ Map<Matrix<typename D4::Scalar, 3, 1>> _delta_v_dyn(&_delta(9));
+ Map<Matrix<typename D4::Scalar, 3, 1>> _delta_L(&_delta(12));
+ Map<Quaternion<typename D4::Scalar>> _delta_q(&_delta(15));
+
+ DT dt2 = _dt * _dt;
+
+ Matrix<typename D1::Scalar, 3, 1> acc_d;
+
+ forces2acc(force, torque, angvel, _com, _inertia, _mass, acc_d);
+
+ _delta_p_imu = 0.5 * acc_i * dt2;
+ _delta_v_imu = acc_i * _dt;
+ _delta_p_dyn = 0.5 * acc_d * dt2;
+ _delta_v_dyn = acc_d * _dt;
+ _delta_L = torque * _dt;
+ _delta_q = exp_q(angvel * _dt);
+}
+
+template <typename D1, typename D2, typename D3, class T>
+void tangent2delta(const MatrixBase<D1>& _tangent, const MatrixBase<D2>& _model, const T _dt, MatrixBase<D3>& _delta)
+{
+ // use shorter names
+ Map<const Matrix<typename D1::Scalar, 3, 1>> acc_i(&_tangent(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> angvel(&_tangent(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> force(&_tangent(6));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> torque(&_tangent(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> com(&_model(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> inertia(&_model(3));
+ const typename D2::Scalar& mass(_model(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_p_imu(&_delta(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_v_imu(&_delta(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_p_dyn(&_delta(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_v_dyn(&_delta(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_L(&_delta(12));
+ Map<Quaternion<typename D3::Scalar>> _delta_q(&_delta(15));
+
+ Matrix<typename D1::Scalar, 3, 1> acc_d;
+
+ forces2acc(force, torque, angvel, com, inertia, mass, acc_d);
+
+ T dt2 = _dt * _dt;
+
+ // delta
+ _delta_p_imu = 0.5 * acc_i * dt2;
+ _delta_v_imu = acc_i * _dt;
+ _delta_p_dyn = 0.5 * acc_d * dt2;
+ _delta_v_dyn = acc_d * _dt;
+ _delta_L = torque * _dt;
+ _delta_q = exp_q(angvel * _dt);
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, class T>
+void tangent2delta(const MatrixBase<D1>& _tangent,
+ const MatrixBase<D2>& _model,
+ const T _dt,
+ MatrixBase<D3>& _delta,
+ MatrixBase<D4>& _J_delta_tangent,
+ MatrixBase<D5>& _J_delta_model)
+{
+ // blocks
+ const unsigned int A = 0; // acc
+ const unsigned int W = 3; // gyro
+ const unsigned int F = 6; // force
+ const unsigned int TQ = 9; // torque
+ const unsigned int C = 0; // CoM
+ const unsigned int I = 3; // inertia
+ const unsigned int M = 6; // mass
+ const unsigned int PI = 0; // position IMU
+ const unsigned int VI = 3; // velocity IMU
+ const unsigned int PD = 6; // position DYN
+ const unsigned int VD = 9; // velocity DYN
+ const unsigned int L = 12; // ang momentum
+ const unsigned int Q = 15; // orientation
+
+ // use shorter names
+ Map<const Matrix<typename D1::Scalar, 3, 1>> acc_i(&_tangent(A));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> angvel(&_tangent(W));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> force(&_tangent(F));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> torque(&_tangent(TQ));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> com(&_model(C));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> inertia(&_model(I));
+ const typename D2::Scalar& mass(_model(M));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_p_imu(&_delta(PI));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_v_imu(&_delta(VI));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_p_dyn(&_delta(PD));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_v_dyn(&_delta(VD));
+ Map<Matrix<typename D3::Scalar, 3, 1>> _delta_L(&_delta(L));
+ Map<Quaternion<typename D3::Scalar>> _delta_q(&_delta(Q));
+
+ Matrix<typename D1::Scalar, 3, 1> acc_d;
+
+ Matrix<typename D1::Scalar, 3, 3> J_accd_force;
+ Matrix<typename D1::Scalar, 3, 3> J_accd_torque;
+ Matrix<typename D1::Scalar, 3, 3> J_accd_angvel;
+ Matrix<typename D1::Scalar, 3, 3> J_accd_com;
+ Matrix<typename D1::Scalar, 3, 3> J_accd_inertia;
+ Matrix<typename D1::Scalar, 3, 1> J_accd_mass;
+
+ forces2acc(force, torque, angvel, com, inertia, mass,
+ acc_d,
+ J_accd_force, J_accd_torque, J_accd_angvel,
+ J_accd_com, J_accd_inertia, J_accd_mass);
+
+ T dt2 = _dt * _dt;
+
+ // delta
+ _delta_p_imu = 0.5 * acc_i * dt2;
+ _delta_v_imu = acc_i * _dt;
+ _delta_p_dyn = 0.5 * acc_d * dt2;
+ _delta_v_dyn = acc_d * _dt;
+ _delta_q = exp_q(angvel * _dt);
+ _delta_L = torque * _dt;
+
+ // temporaries
+ const Matrix<typename D1::Scalar, 3, 3>& I_dt = Matrix<typename D1::Scalar, 3, 3>::Identity() * _dt;
+ const Matrix<typename D1::Scalar, 3, 3>& I_dt2_2 = 0.5 * Matrix<typename D1::Scalar, 3, 3>::Identity() * dt2;
+
+ // jacobians
+ Matrix<typename D1::Scalar, 3, 3> J_dpi_acci = I_dt2_2;
+ Matrix<typename D1::Scalar, 3, 3> J_dvi_acci = I_dt;
+ const auto& J_dpd_accd = I_dt2_2;
+ const auto& J_dvd_accd = I_dt;
+ Matrix<typename D1::Scalar, 3, 3> J_dq_angvel = jac_SO3_right(angvel * _dt) * _dt;
+ Matrix<typename D1::Scalar, 3, 3> J_dl_torque = I_dt;
+
+ _J_delta_tangent.setZero();
+
+ _J_delta_tangent.template block<3, 3>(PI, A) = J_dpi_acci;
+ _J_delta_tangent.template block<3, 3>(VI, A) = J_dvi_acci;
+
+ _J_delta_tangent.template block<3, 3>(PD, W) = J_dpd_accd * J_accd_angvel;
+ _J_delta_tangent.template block<3, 3>(PD, F) = J_dpd_accd * J_accd_force;
+ _J_delta_tangent.template block<3, 3>(PD, TQ) = J_dpd_accd * J_accd_torque;
+
+ _J_delta_tangent.template block<3, 3>(VD, W) = J_dvd_accd * J_accd_angvel;
+ _J_delta_tangent.template block<3, 3>(VD, F) = J_dvd_accd * J_accd_force;
+ _J_delta_tangent.template block<3, 3>(VD, TQ) = J_dvd_accd * J_accd_torque;
+
+ _J_delta_tangent.template block<3, 3>(Q, W) = J_dq_angvel;
+ _J_delta_tangent.template block<3, 3>(L, TQ) = J_dl_torque;
+
+ _J_delta_model.setZero();
+
+ _J_delta_model.template block<3, 3>(PD, C) = J_dpd_accd * J_accd_com;
+ _J_delta_model.template block<3, 3>(PD, I) = J_dpd_accd * J_accd_inertia;
+ _J_delta_model.template block<3, 1>(PD, M) = J_dpd_accd * J_accd_mass;
+
+ _J_delta_model.template block<3, 3>(VD, C) = J_dvd_accd * J_accd_com;
+ _J_delta_model.template block<3, 3>(VD, I) = J_dvd_accd * J_accd_inertia;
+ _J_delta_model.template block<3, 1>(VD, M) = J_dvd_accd * J_accd_mass;
+}
+
+template <typename D1, typename D2, typename D3, typename D4, class T>
+void data2delta(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ const MatrixBase<D3>& _model,
+ const std::list<Propeller>& _propellers,
+ const T _dt,
+ MatrixBase<D4>& _delta)
+{
+ Map<const Matrix<typename D2::Scalar, 3, 1>> com(&_model(0));
+
+ Matrix<typename D4::Scalar, 12, 1> tangent;
+
+ data2tangent(_data, _bias, com, _propellers, tangent);
+
+ tangent2delta(tangent, _model, _dt, _delta);
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, class T>
+void data2delta(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ const MatrixBase<D3>& _model,
+ const std::list<Propeller>& _propellers,
+ const T _dt,
+ MatrixBase<D4>& _delta,
+ MatrixBase<D5>& _J_delta_data,
+ MatrixBase<D6>& _J_delta_bias,
+ MatrixBase<D7>& _J_delta_model)
+{
+ const unsigned int np = _propellers.size();
+ Map<const Matrix<typename D2::Scalar, 3, 1>> com(&_model(0));
+
+ Matrix<typename D4::Scalar, 12, 1> tangent;
+
+ Matrix<typename D4::Scalar, 12, Dynamic> J_tangent_data(12, 6 + np);
+ Matrix<typename D4::Scalar, 12, 12> J_tangent_bias;
+ Matrix<typename D4::Scalar, 12, 7> J_tangent_model;
+ Matrix<typename D4::Scalar, 18, 12> J_delta_tangent;
+
+ J_tangent_model.setZero(12, 7);
+
+ Matrix<typename D4::Scalar, 12, 3> J_tangent_com;
+ data2tangent(_data, _bias, com, _propellers, tangent, J_tangent_data, J_tangent_bias, J_tangent_com);
+ J_tangent_model.template block<12, 3>(0, 0) = J_tangent_com;
+
+ tangent2delta(tangent, _model, _dt, _delta, J_delta_tangent, _J_delta_model);
+
+ _J_delta_data = J_delta_tangent * J_tangent_data;
+ _J_delta_bias = J_delta_tangent * J_tangent_bias;
+ _J_delta_model = _J_delta_model + J_delta_tangent * J_tangent_model;
+}
+
+
+} // namespace fti
+} // namespace bodydynamics
+} // namespace wolf
+
+#endif /* MATH_FORCE_TORQUE_INERTIAL_DELTA_TOOLS_H_ */
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque.h
similarity index 79%
rename from include/bodydynamics/processor/processor_force_torque_preint.h
rename to include/bodydynamics/processor/processor_force_torque.h
index 6cd089107cfebd55fad2a51ff1b6cf5eddb0b036..1b8197fc27350412ec330feb802d0a5751bcb916 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque.h
@@ -19,24 +19,24 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef PROCESSOR_FORCE_TORQUE_PREINT_H
-#define PROCESSOR_FORCE_TORQUE_PREINT_H
+#ifndef PROCESSOR_FORCE_TORQUE_H
+#define PROCESSOR_FORCE_TORQUE_H
// Wolf core
#include <core/processor/processor_motion.h>
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorquePreint);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorque);
-struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
+struct ParamsProcessorForceTorque : public ParamsProcessorMotion
{
std::string sensor_ikin_name;
std::string sensor_angvel_name;
int nbc; // Number of contacts
int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench
- ParamsProcessorForceTorquePreint() = default;
- ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorForceTorque() = default;
+ ParamsProcessorForceTorque(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorMotion(_unique_name, _server)
{
sensor_ikin_name = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name");
@@ -44,7 +44,7 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
nbc = _server.getParam<int>(_unique_name + "/nbc_");
dimc = _server.getParam<int>(_unique_name + "/dimc_");
}
- ~ParamsProcessorForceTorquePreint() override = default;
+ ~ParamsProcessorForceTorque() override = default;
std::string print() const override
{
return ParamsProcessorMotion::print() + "\n"
@@ -56,16 +56,16 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
}
};
-WOLF_PTR_TYPEDEFS(ProcessorForceTorquePreint);
+WOLF_PTR_TYPEDEFS(ProcessorForceTorque);
//class
-class ProcessorForceTorquePreint : public ProcessorMotion{
+class ProcessorForceTorque : public ProcessorMotion{
public:
- ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint);
- ~ProcessorForceTorquePreint() override;
+ ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque);
+ ~ProcessorForceTorque() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorque, ParamsProcessorForceTorque);
protected:
void computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -104,12 +104,11 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
private:
- ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
+ ParamsProcessorForceTorquePtr params_motion_force_torque_;
SensorBasePtr sensor_ikin_;
SensorBasePtr sensor_angvel_;
int nbc_;
@@ -124,17 +123,17 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
namespace wolf{
-inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor)
+inline void ProcessorForceTorque::configure(SensorBasePtr _sensor)
{
- sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_ikin_name);
- sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_angvel_name);
+ sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_ikin_name);
+ sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_angvel_name);
};
-inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const
+inline Eigen::VectorXd ProcessorForceTorque::deltaZero() const
{
return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation
}
} // namespace wolf
-#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
+#endif // PROCESSOR_FORCE_TORQUE_H
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial.h b/include/bodydynamics/processor/processor_force_torque_inertial.h
new file mode 100644
index 0000000000000000000000000000000000000000..c51dff8a183d77f33856abd55fc665e44b703024
--- /dev/null
+++ b/include/bodydynamics/processor/processor_force_torque_inertial.h
@@ -0,0 +1,245 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_force_torque_inertial.h
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+
+#include <core/processor/processor_motion.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertial);
+
+struct ParamsProcessorForceTorqueInertial : public ParamsProcessorMotion
+{
+ ParamsProcessorForceTorqueInertial() = default;
+ ParamsProcessorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorMotion(_unique_name, _server)
+ {
+ n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
+ }
+
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n" //
+ + "n_propellers: " + std::to_string(n_propellers); //
+ }
+
+ int n_propellers;
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertial);
+class ProcessorForceTorqueInertial : public ProcessorMotion
+{
+ public:
+ ProcessorForceTorqueInertial(
+ ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial);
+ ~ProcessorForceTorqueInertial() override;
+ void configure(SensorBasePtr _sensor) override;
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertial, ParamsProcessorForceTorqueInertial);
+
+ protected:
+ ParamsProcessorForceTorqueInertialPtr params_force_torque_inertial_;
+ SensorForceTorqueInertialPtr sensor_fti_;
+ Matrix6d imu_drift_cov_;
+
+ public:
+ /** \brief convert raw CaptureMotion data to the delta-state format.
+ *
+ * This function accepts raw data and time step dt,
+ * and computes the value of the delta-state and its covariance. Note that these values are
+ * held by the members delta_ and delta_cov_.
+ *
+ * @param _data measured motion data
+ * @param _data_cov covariance
+ * @param _dt time step
+ * @param _delta computed delta
+ * @param _delta_cov covariance
+ * @param _calib current state of the calibrated parameters
+ * @param _jacobian_calib Jacobian of the delta wrt calib
+ *
+ * Rationale:
+ *
+ * The delta-state format must be compatible for integration using
+ * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
+ * See the class documentation for some Eigen::VectorXd suggestions.
+ *
+ * The data format is generally not the same as the delta format,
+ * because it is the format of the raw data provided by the Capture,
+ * which is unaware of the needs of this processor.
+ *
+ * Additionally, sometimes the data format is in the form of a
+ * velocity, or a higher derivative, while the delta is in the form of an increment.
+ * In such cases, converting from data to delta-state needs integrating
+ * the data over the period dt.
+ *
+ * Two trivial implementations would establish:
+ * - If `_data` is an increment:
+ *
+ * delta_ = _data;
+ * delta_cov_ = _data_cov
+ * - If `_data` is a velocity:
+ *
+ * delta_ = _data * _dt
+ * delta_cov_ = _data_cov * _dt.
+ *
+ * However, other more complicated relations are possible.
+ * In general, we'll have a nonlinear
+ * function relating `delta_` to `_data` and `_dt`, as follows:
+ *
+ * delta_ = f ( _data, _dt)
+ *
+ * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
+ * that is, through the Jacobians of `f()`,
+ *
+ * delta_cov_ = F_data * _data_cov * F_data.transpose
+ *
+ * where `F_data = d_f/d_data` is the Jacobian of `f()`.
+ */
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ /** \brief composes a delta-state on top of another delta-state
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+
+ /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
+ * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
+ * jacobians.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+
+ /** \brief composes a delta-state on top of a state
+ * \param _x the initial state
+ * \param _delta the delta-state
+ * \param _x_plus_delta the updated state. It has the same format as the initial state.
+ * \param _dt the time interval spanned by the Delta
+ *
+ * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
+ */
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override;
+
+ /** \brief Delta zero
+ * \return a delta state equivalent to the null motion.
+ *
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+ * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+ * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
+ */
+ Eigen::VectorXd deltaZero() const override;
+
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const override;
+
+ protected:
+ /** \brief emplace a CaptureMotion
+ * \param _frame_own frame owning the Capture to create
+ * \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
+ * \param _data a vector of motion data
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
+ */
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ public:
+ virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+};
+
+inline Eigen::VectorXd ProcessorForceTorqueInertial::deltaZero() const
+{
+ return bodydynamics::fti::identity();
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ */
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd9eef9ee86e6fd6daab00cdd142fc6713217d59
--- /dev/null
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -0,0 +1,267 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_force_torque_inertial_dynamics.h
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+
+#include <core/processor/processor_motion.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialDynamics);
+
+struct ParamsProcessorForceTorqueInertialDynamics : public ParamsProcessorMotion
+{
+ ParamsProcessorForceTorqueInertialDynamics() = default;
+ ParamsProcessorForceTorqueInertialDynamics(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorMotion(_unique_name, _server)
+ {
+ // n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
+ }
+
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n"; //
+ // + "n_propellers: " + std::to_string(n_propellers); //
+ }
+
+ // int n_propellers;
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialDynamics);
+class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
+{
+ public:
+ ProcessorForceTorqueInertialDynamics(
+ ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics_);
+ ~ProcessorForceTorqueInertialDynamics() override;
+ void configure(SensorBasePtr _sensor) override;
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics,
+ ParamsProcessorForceTorqueInertialDynamics);
+
+ protected:
+ ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
+ SensorForceTorqueInertialPtr sensor_fti_;
+ Matrix6d imu_drift_cov_;
+ Matrix3d gyro_noise_cov_;
+
+ public:
+ /** \brief convert raw CaptureMotion data to the delta-state format.
+ *
+ * This function accepts raw data and time step dt,
+ * and computes the value of the delta-state and its covariance. Note that these values are
+ * held by the members delta_ and delta_cov_.
+ *
+ * @param _data measured motion data
+ * @param _data_cov covariance
+ * @param _dt time step
+ * @param _delta computed delta
+ * @param _delta_cov covariance
+ * @param _calib current state of the calibrated parameters
+ * @param _jacobian_calib Jacobian of the delta wrt calib
+ *
+ * Rationale:
+ *
+ * The delta-state format must be compatible for integration using
+ * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
+ * See the class documentation for some Eigen::VectorXd suggestions.
+ *
+ * The data format is generally not the same as the delta format,
+ * because it is the format of the raw data provided by the Capture,
+ * which is unaware of the needs of this processor.
+ *
+ * Additionally, sometimes the data format is in the form of a
+ * velocity, or a higher derivative, while the delta is in the form of an increment.
+ * In such cases, converting from data to delta-state needs integrating
+ * the data over the period dt.
+ *
+ * Two trivial implementations would establish:
+ * - If `_data` is an increment:
+ *
+ * delta_ = _data;
+ * delta_cov_ = _data_cov
+ * - If `_data` is a velocity:
+ *
+ * delta_ = _data * _dt
+ * delta_cov_ = _data_cov * _dt.
+ *
+ * However, other more complicated relations are possible.
+ * In general, we'll have a nonlinear
+ * function relating `delta_` to `_data` and `_dt`, as follows:
+ *
+ * delta_ = f ( _data, _dt)
+ *
+ * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
+ * that is, through the Jacobians of `f()`,
+ *
+ * delta_cov_ = F_data * _data_cov * F_data.transpose
+ *
+ * where `F_data = d_f/d_data` is the Jacobian of `f()`.
+ */
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ /** \brief composes a delta-state on top of another delta-state
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+
+ /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
+ * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
+ * jacobians.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+
+ /** \brief composes a delta-state on top of a state
+ * \param _x the initial state
+ * \param _delta the delta-state
+ * \param _x_plus_delta the updated state. It has the same format as the initial state.
+ * \param _dt the time interval spanned by the Delta
+ *
+ * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
+ */
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override;
+
+ /** \brief Delta zero
+ * \return a delta state equivalent to the null motion.
+ *
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+ * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+ * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
+ */
+ Eigen::VectorXd deltaZero() const override;
+
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const override;
+
+ public:
+ virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+
+ protected:
+ /** \brief emplace a CaptureMotion
+ * \param _frame_own frame owning the Capture to create
+ * \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
+ * \param _data a vector of motion data
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
+ */
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+
+ /** \brief Emplace features and factors
+ *
+ * This processor emplaces tree possible factors (with its features):
+ * - A Force-torque-inertial pre-integrated factor -- the motion factor
+ * - An Angular-momentum factor -- links angular momentum with angular velocity and inertia
+ * - An IMU bias drift factor -- this one only if the IMU biases are dynamic
+ *
+ * \param _capture_origin: origin of the integrated delta
+ * \param _capture_own: final of the integrated delta
+ */
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ bool voteForKeyFrame() const override;
+
+ private:
+ void data2tangent(const Eigen::VectorXd& _data, ///< [a, w, f, t] raw measures
+ const Eigen::VectorXd& _bias, ///< [ab, wb]
+ const Eigen::VectorXd& _model, ///< [com, inertia, mass]
+ Eigen::VectorXd& _tangent, ///< [a, w, f, t] calibrated
+ Eigen::MatrixXd& _J_tangent_data,
+ Eigen::MatrixXd& _J_tangent_bias,
+ Eigen::MatrixXd& _J_tangent_model) const;
+};
+
+inline Eigen::VectorXd ProcessorForceTorqueInertialDynamics::deltaZero() const
+{
+ return bodydynamics::fti::identity();
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ */
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d509f80415ce98f27d84ee11440086f0fc83595
--- /dev/null
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -0,0 +1,142 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef SENSOR_FORCE_TORQUE_INERTIAL_H
+#define SENSOR_FORCE_TORQUE_INERTIAL_H
+
+// wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorqueInertial);
+
+struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
+{
+ // noise std dev
+ double acc_noise_std;
+ double gyro_noise_std;
+ double acc_drift_std;
+ double gyro_drift_std;
+ double force_noise_std;
+ double torque_noise_std;
+ Vector3d com;
+ Vector3d inertia;
+ double mass;
+
+ ParamsSensorForceTorqueInertial() = default;
+ ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
+ {
+ acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
+ gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
+ acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
+ gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
+ force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
+ torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
+ com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+ inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+ mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ }
+ ~ParamsSensorForceTorqueInertial() override = default;
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n" //
+ + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
+ + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
+ + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
+ + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ + "com: print not implemented." + "\n" //
+ + "inertia: print not implemented. \n" //
+ + "mass: " + std::to_string(mass) + "\n"; //
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorForceTorqueInertial);
+
+class SensorForceTorqueInertial : public SensorBase
+{
+ protected:
+ ParamsSensorForceTorqueInertialPtr params_fti_;
+
+ public:
+ SensorForceTorqueInertial(const VectorComposite& _states, ParamsSensorForceTorqueInertialPtr _params);
+ SensorForceTorqueInertial(const VectorXd& _extrinsics, ParamsSensorForceTorqueInertialPtr _params);
+ ~SensorForceTorqueInertial() override = default;
+
+ WOLF_SENSOR_CREATE(SensorForceTorqueInertial, ParamsSensorForceTorqueInertial, 7);
+
+ // getters return by copy
+ Vector6d getImuBias() const; // Imu bias [acc, gyro]
+ Vector3d getAccBias() const; // Acc bias
+ Vector3d getGyroBias() const; // Gyro bias
+ double getMass() const; // Total mass
+ Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
+ Vector3d getCom() const; // centre of mass
+ Vector7d getModel() const; // dynamic model [com, inertia, mass]
+ ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;}
+};
+
+inline double SensorForceTorqueInertial::getMass() const
+{
+ return getStateBlock('m')->getState()(0);
+}
+
+inline Vector6d SensorForceTorqueInertial::getImuBias() const
+{
+ return getStateBlock('I')->getState();
+}
+
+inline Vector3d SensorForceTorqueInertial::getAccBias() const
+{
+ return getStateBlock('I')->getState().head<3>();
+}
+
+inline Vector3d SensorForceTorqueInertial::getGyroBias() const
+{
+ return getStateBlock('I')->getState().tail<3>();
+}
+
+inline Vector3d SensorForceTorqueInertial::getInertia() const
+{
+ return getStateBlock('i')->getState();
+}
+
+inline Vector3d SensorForceTorqueInertial::getCom() const
+{
+ return getStateBlock('C')->getState();
+}
+
+inline Vector7d SensorForceTorqueInertial::getModel() const
+{
+ Vector7d model;
+ model << getCom(), getInertia(), getMass();
+ return model;
+}
+
+} // namespace wolf
+
+#endif // SENSOR_FORCE_TORQUE_H
diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque.cpp
similarity index 87%
rename from src/capture/capture_force_torque_preint.cpp
rename to src/capture/capture_force_torque.cpp
index 83e145e4b85a4895a81dce2251fcddd10681460d..3b09bdda9e821497e4e7cc77300a8e06cb656e71 100644
--- a/src/capture/capture_force_torque_preint.cpp
+++ b/src/capture/capture_force_torque.cpp
@@ -20,13 +20,13 @@
//
//--------LICENSE_END--------
#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "core/state_block/state_quaternion.h"
namespace wolf {
-CaptureForceTorquePreint::CaptureForceTorquePreint(
+CaptureForceTorque::CaptureForceTorque(
const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
@@ -34,14 +34,14 @@ CaptureForceTorquePreint::CaptureForceTorquePreint(
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics
CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov,
+ CaptureMotion("CaptureForceTorque", _init_ts, _sensor_ptr, _data, _data_cov,
_capture_origin_ptr, nullptr, nullptr, nullptr),
cap_ikin_other_(_capture_IK_ptr),
cap_gyro_other_(_capture_motion_ptr)
{
}
-CaptureForceTorquePreint::~CaptureForceTorquePreint()
+CaptureForceTorque::~CaptureForceTorque()
{
}
diff --git a/src/capture/capture_force_torque_inertial.cpp b/src/capture/capture_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85a7f3603e24aaa04de9bfaafa5b33f32aa58da4
--- /dev/null
+++ b/src/capture/capture_force_torque_inertial.cpp
@@ -0,0 +1,50 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
+
+#include <core/state_block/state_block_derived.h>
+
+namespace wolf
+{
+CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureForceTorqueInertial",
+ _init_ts,
+ _sensor_ptr,
+ _data,
+ _data_cov,
+ _capture_origin_ptr,
+ nullptr, // position is static
+ nullptr, // orientation is static
+ (_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias
+ ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlockDynamic('I')->getState(), false)
+ : nullptr))
+{
+ //
+}
+
+CaptureForceTorqueInertial::~CaptureForceTorqueInertial() {}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 83c12548fd923e7621b56465ecedbcc12b673500..04eacfa7e8c4c02944bb2fd6a151f05551030c47 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -20,35 +20,18 @@
//
//--------LICENSE_END--------
#include "bodydynamics/feature/feature_force_torque.h"
+namespace wolf
+{
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureForceTorque);
+FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double, 12, 6>& _J_delta_biases)
+ : FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance),
+ pbc_bias_preint_(_biases_preint.head<3>()),
+ gyro_bias_preint_(_biases_preint.tail<3>()),
+ J_delta_bias_(_J_delta_biases)
+{
+}
-FeatureForceTorque::FeatureForceTorque(
- const double& _dt,
- const double& _mass,
- const Eigen::Vector6d& _forces_meas,
- const Eigen::Vector6d& _torques_meas,
- const Eigen::Vector3d& _pbc_meas,
- const Eigen::Matrix<double,14,1>& _kin_meas,
- const Eigen::Matrix6d& _cov_forces_meas,
- const Eigen::Matrix6d& _cov_torques_meas,
- const Eigen::Matrix3d& _cov_pbc_meas,
- const Eigen::Matrix<double,14,14>& _cov_kin_meas) :
- FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> we are bypassing the way meas and cov is usually stored because too many of them == vector is too big -> error prone
- dt_(_dt),
- mass_(_mass),
- forces_meas_(_forces_meas),
- torques_meas_(_torques_meas),
- pbc_meas_(_pbc_meas),
- kin_meas_(_kin_meas),
- cov_forces_meas_(_cov_forces_meas),
- cov_torques_meas_(_cov_torques_meas),
- cov_pbc_meas_(_cov_pbc_meas),
- cov_kin_meas_(_cov_kin_meas)
-{}
-
-FeatureForceTorque::~FeatureForceTorque(){}
-
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd7f5806463b14bbe56ee458d542048675879b7a
--- /dev/null
+++ b/src/processor/processor_force_torque.cpp
@@ -0,0 +1,300 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+// wolf
+#include "bodydynamics/math/force_torque_delta_tools.h"
+#include "bodydynamics/capture/capture_force_torque.h"
+#include "bodydynamics/feature/feature_force_torque.h"
+#include "bodydynamics/processor/processor_force_torque.h"
+#include "bodydynamics/factor/factor_force_torque.h"
+
+namespace wolf
+{
+
+int compute_data_size(int nbc, int dimc)
+{
+ // compute the size of the data/body vectors used by processorMotion API depending
+ // on the number of contacts (nbc) and contact dimension (dimc)
+ // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
+ return nbc * dimc + 3 + 3 + nbc * 3 + nbc * 4;
+}
+
+using namespace Eigen;
+
+ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque)
+ : ProcessorMotion("ProcessorForceTorque",
+ "CDLO",
+ 3,
+ 13,
+ 13,
+ 12,
+ compute_data_size(_params_motion_force_torque->nbc, _params_motion_force_torque->dimc),
+ 6,
+ _params_motion_force_torque),
+ params_motion_force_torque_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque)),
+ nbc_(_params_motion_force_torque->nbc),
+ dimc_(_params_motion_force_torque->dimc)
+
+{
+ //
+}
+
+ProcessorForceTorque::~ProcessorForceTorque()
+{
+ //
+}
+
+bool ProcessorForceTorque::voteForKeyFrame() const
+{
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_->max_time_span)
+ {
+ WOLF_DEBUG("PM: vote: time span");
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_motion_force_torque_->max_buff_length)
+ {
+ WOLF_DEBUG("PM: vote: buffer length");
+ return true;
+ }
+
+ // Some other measure of movement?
+
+ // WOLF_DEBUG( "PM: do not vote" );
+ return false;
+}
+
+CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
+{
+ // Here we have to retrieve the origin capture no
+ // !! PROBLEM:
+ // when doing setOrigin, emplaceCapture is called 2 times
+ // - first on the first KF (usually created by setPrior), this one contains the biases
+ // - secondly on the inner frame (last) which does not contains other captures
+ auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
+ auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
+ if ((capture_ikin == nullptr) || (capture_angvel == nullptr))
+ {
+ // We have to retrieve the bias from a former Keyframe: origin
+ capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
+ capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
+ }
+ auto cap =
+ CaptureBase::emplace<CaptureForceTorque>(_frame_own,
+ _ts,
+ _sensor,
+ std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
+ std::static_pointer_cast<CaptureMotion>(capture_angvel),
+ _data,
+ _data_cov,
+ _capture_origin);
+
+ // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
+ // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
+
+ auto cap_ft = std::static_pointer_cast<CaptureForceTorque>(cap);
+ Vector6d calib = getCalibration(cap_ft);
+ setCalibration(cap_ft, calib);
+ cap_ft->setCalibrationPreint(calib);
+
+ return cap;
+}
+
+// FeatureBasePtr ProcessorForceTorque::emplaceFeature(CaptureMotionPtr _capture_motion)
+// {
+// // Retrieving the origin capture and getting the bias from here should be done here?
+// auto feature = FeatureBase::emplace<FeatureForceTorque>(
+// _capture_motion,
+// _capture_motion->getBuffer().back().delta_integr_,
+// _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+// _capture_motion->getCalibrationPreint(),
+// _capture_motion->getBuffer().back().jacobian_calib_);
+// return feature;
+// }
+
+Eigen::VectorXd ProcessorForceTorque::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ return bodydynamics::plus(delta_preint, delta_step);
+}
+
+VectorXd ProcessorForceTorque::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+ VectorXd bias_vec(6);
+
+ if (_capture) // access from capture is quicker
+ {
+ auto cap_ft(std::static_pointer_cast<const CaptureForceTorque>(_capture));
+
+ // get calib part from Ikin capture
+ bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+
+ // get calib part from IMU capture
+ bias_vec.segment<3>(3) =
+ cap_ft->getGyroCaptureOther()
+ ->getSensorIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ }
+ else // access from sensor is slower
+ {
+ // get calib part from Ikin capture
+ bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
+
+ // get calib part from IMU capture
+ bias_vec.segment<3>(3) =
+ sensor_angvel_->getIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ }
+ return bias_vec;
+}
+
+void ProcessorForceTorque::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ CaptureForceTorquePtr cap_ft(std::static_pointer_cast<CaptureForceTorque>(_capture));
+
+ // set calib part in Ikin capture
+ cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
+
+ // set calib part in IMU capture
+ Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
+ calib_imu.tail<3>() = _calibration.tail<3>();
+
+ cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
+}
+
+void ProcessorForceTorque::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
+{
+ // Retrieving the origin capture and getting the bias from here should be done here?
+ auto feature = FeatureBase::emplace<FeatureForceTorque>(
+ _capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
+
+ CaptureForceTorquePtr cap_ft_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
+ FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(feature);
+ CaptureForceTorquePtr cap_ft_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ft->getCapture());
+
+ FactorBase::emplace<FactorForceTorque>(ftr_ft,
+ ftr_ft,
+ cap_ft_origin,
+ shared_from_this(),
+ cap_ft_origin->getIkinCaptureOther(), // to retrieve sb_bp1
+ cap_ft_origin->getGyroCaptureOther(), // to retrieve sb_w1
+ false);
+}
+
+void ProcessorForceTorque::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jac_delta_calib) const
+{
+ assert(_data.size() == data_size_ && "Wrong data size!");
+
+ // create delta
+ MatrixXd jac_body_bias(data_size_ - nbc_, 6);
+ VectorXd body(data_size_);
+ bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
+
+ MatrixXd jac_delta_body(12, data_size_ - nbc_);
+ bodydynamics::body2delta(body,
+ _dt,
+ std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
+ nbc_,
+ dimc_,
+ _delta,
+ jac_delta_body); // Jacobians tested in bodydynamics_tools
+
+ // [f], [tau], pbc, wb
+ MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_ * dimc_ + 6);
+
+ // compute delta_cov (using uncertainty on f,tau,pbc)
+ _delta_cov = jac_delta_body_reduced * _data_cov *
+ jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
+ // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity
+
+ // compute jacobian_calib
+ _jac_delta_calib = jac_delta_body * jac_body_bias;
+}
+
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
+{
+ _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
+}
+
+void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+{
+ assert(_delta.size() == 13 && "Wrong _delta vector size");
+ assert(_dt >= 0 && "Time interval _dt is negative!");
+
+ // verbose way : create Eigen vectors, then compute, then convert back to Composite
+
+ auto x = _x.vector("CDLO");
+
+ VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
+
+ _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3, 3, 3, 4});
+}
+
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
+{
+ bodydynamics::compose(_delta_preint,
+ _delta,
+ _dt,
+ _delta_preint_plus_delta,
+ _jacobian_delta_preint,
+ _jacobian_delta); // jacobians tested in bodydynamics_tools
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorque)
+}
diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc3256c2674eb1205a59c98ece711c9fb2f734c8
--- /dev/null
+++ b/src/processor/processor_force_torque_inertial.cpp
@@ -0,0 +1,225 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_force_torque_inertial.cpp
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+// bodydynamics
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
+#include "bodydynamics/factor/factor_force_torque_inertial.h"
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+
+// wolf
+#include <core/state_block/state_composite.h>
+#include <core/capture/capture_motion.h>
+#include <core/factor/factor_block_difference.h>
+
+namespace wolf
+{
+
+ProcessorForceTorqueInertial::ProcessorForceTorqueInertial(
+ ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial)
+ : ProcessorMotion("ProcessorForceTorqueInertial",
+ "POVL",
+ 3, // dim
+ 13, // state size [p, q, v, L]
+ 19, // delta size [pi, vi, pd, vd, L, q]
+ 18, // delta cov size
+ 12, // data size [a, w, f, t]
+ 6, // calib size [ab, wb]
+ _params_force_torque_inertial),
+ params_force_torque_inertial_(_params_force_torque_inertial)
+{
+ // TODO Auto-generated constructor stub
+}
+
+ProcessorForceTorqueInertial::~ProcessorForceTorqueInertial()
+{
+ // TODO Auto-generated destructor stub
+}
+
+CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr)
+{
+ CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
+ _frame_own, "CaptureForceTorqueInertial", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+ setCalibration(capture, _calib);
+ capture->setCalibrationPreint(_calib_preint);
+ return capture;
+}
+
+void ProcessorForceTorqueInertial::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
+ CaptureMotionPtr _capture_own)
+{
+ // 1. Feature and factor FTI -- force-torque-inertial
+ //----------------------------------------------------
+ auto motion = _capture_own->getBuffer().back();
+ auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own,
+ "FeatureMotion",
+ motion.delta_integr_,
+ motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(),
+ motion.jacobian_calib_);
+
+ FactorBase::emplace<FactorForceTorqueInertial>(
+ ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
+
+
+ // 2. Feature and factor bias -- IMU bias drift for acc and gyro
+ //---------------------------------------------------------------
+ // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+ // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+ // - factor relates bias(last capture) and bias(origin capture)
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own,
+ "FeatureBase",
+ Vector6d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+ ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ false); // loss function
+ }
+ }
+}
+
+
+void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
+}
+
+void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
+{
+ sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+
+
+ auto acc_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
+ auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
+
+ ArrayXd imu_drift_sigmas(6);
+ imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+}
+
+void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
+{
+ // compute tangent by removing IMU bias
+ Matrix<double, 12, 1> tangent = _data;
+ tangent.head<6>() -= _calib; // J_tangent_data = Identity(12,12)
+ Matrix<double, 12, 6> J_tangent_calib; // J_tangent_calib = [-Identity(6,6) ; Zero(6,6)]
+ J_tangent_calib.topRows<6>() = -Matrix6d::Identity();
+ J_tangent_calib.bottomRows<6>() = Matrix6d::Zero();
+
+ // go from tangent to delta. We need to dynamic model for this
+ const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+ Matrix<double, 18, 12> J_delta_tangent;
+ Matrix<double, 18, 7> J_delta_model;
+ fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
+
+ // Compute cov and compose jacobians
+ const auto& J_delta_data = J_delta_tangent; // * J_tangent_data, which is the Identity;
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+ _jacobian_calib = J_delta_tangent * J_tangent_calib;
+}
+
+void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+}
+
+void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+}
+
+void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+{
+ auto x = _x.vector("PVLO");
+ VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
+ _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
+}
+
+Eigen::VectorXd ProcessorForceTorqueInertial::correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const
+{
+ return fti::plus(_delta_preint, _delta_step);
+}
+
+VectorXd ProcessorForceTorqueInertial::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+ if (_capture)
+ return _capture->getStateBlock('I')->getState(); // IMU bias
+ else
+ return getSensor()->getStateBlock('I')->getState(); // IMU bias
+}
+} /* namespace wolf */
+
+#include <core/processor/factory_processor.h>
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertial);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertial);
+} // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..678074d6f632f70b47712da7ffcf056fa8a64146
--- /dev/null
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,503 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_force_torque_inertial_dynamics.cpp
+ *
+ * Created on: Aug 1, 2022
+ * Author: asanjuan
+ */
+
+// bodydynamics
+#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>
+#include <core/capture/capture_motion.h>
+#include <core/factor/factor_block_difference.h>
+
+namespace wolf
+{
+
+ProcessorForceTorqueInertialDynamics::ProcessorForceTorqueInertialDynamics(
+ ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics)
+ : ProcessorMotion("ProcessorForceTorqueInertialDynamics",
+ "POVL",
+ 3, // dim
+ 13, // state size [p, q, v, L]
+ 19, // delta size [pi, vi, pd, vd, L, q]
+ 18, // delta cov size
+ 12, // data size [a, w, f, t]
+ 13, // calib size [ab, wb, c, i, m]
+ _params_force_torque_inertial_dynamics),
+ params_force_torque_inertial_dynamics_(_params_force_torque_inertial_dynamics)
+{
+ //
+}
+
+ProcessorForceTorqueInertialDynamics::~ProcessorForceTorqueInertialDynamics()
+{
+ //
+}
+
+CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _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;
+}
+
+void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
+ CaptureMotionPtr _capture_own)
+{
+ // 1. Feature and factor FTI -- force-torque-inertial
+ //----------------------------------------------------
+ auto motion = _capture_own->getBuffer().back();
+ auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own,
+ "FeatureMotion",
+ motion.delta_integr_,
+ motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(),
+ motion.jacobian_calib_);
+
+ FactorBase::emplace<FactorForceTorqueInertialDynamics>(
+ ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
+
+ // 2. Feature and factor L -- angular momentum
+ //---------------------------------------------
+ // - feature has the current gyro measurement
+ // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
+
+ // auto measurement_gyro = motion.data_.segment<3>(3);
+ // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
+
+ // 3. Feature and factor bias -- IMU bias drift for acc and gyro
+ //---------------------------------------------------------------
+ // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+ // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+ // - factor relates bias(last capture) and bias(origin capture)
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own,
+ "FeatureBase",
+ Vector6d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+ ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ false); // loss function
+ }
+ }
+}
+
+void ProcessorForceTorqueInertialDynamics::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias
+ _capture->getStateBlock('C')->setState(_calibration.segment<3>(6)); // Set C
+ _capture->getStateBlock('i')->setState(_calibration.segment<3>(9)); // Set i
+ _capture->getStateBlock('m')->setState(_calibration.segment<1>(12)); // Set m
+}
+
+void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
+{
+ sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+
+ auto gyro_noise_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ auto acc_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
+ auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
+
+ ArrayXd imu_drift_sigmas(6);
+ imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+ gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
+}
+
+void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
+ const Eigen::VectorXd& _bias,
+ const Eigen::VectorXd& _model,
+ Eigen::VectorXd& _tangent,
+ Eigen::MatrixXd& _J_tangent_data,
+ Eigen::MatrixXd& _J_tangent_bias,
+ Eigen::MatrixXd& _J_tangent_model) const
+
+{
+ const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector
+ const Vector3d& fd = _data.segment<3>(6);
+ const Vector3d& td = _data.segment<3>(9);
+ const Vector3d& c = _model.segment<3>(0);
+ const Matrix3d& fd_cross = skew(fd);
+ const Matrix3d& c_cross = skew(c);
+
+ // tangent(a,w) = data(a,w) - bias(a,w)
+ // tangent(f) = data(f)
+ // tangent(t) = data(t) - model(c) x data(f)
+ _tangent.segment<6>(0) = awd - _bias;
+ _tangent.segment<3>(6) = fd;
+ _tangent.segment<3>(9) = td - c.cross(fd); // c x fd
+
+ // J_tangent_data
+ _J_tangent_data.setIdentity(12, 12);
+ _J_tangent_data.block<3, 3>(9, 6) = -c_cross; // J_tt_fd = -c_cross
+
+ // J_tangent_bias = [-Identity(6,6) ; Zero(6,6)]
+ _J_tangent_bias.topRows<6>() = -Matrix6d::Identity();
+ _J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
+
+ // J_tangent_model
+ _J_tangent_model.setZero(12, 7);
+ _J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross
+}
+
+void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
+{
+ // compute tangent by removing IMU bias
+
+ /**
+ * Notes on the computation of `tangent` and `delta` from `data` and `calib`, and its Jacobians:
+ * =============================================================================================
+ *
+ * Overview: from sensor data and system calibration parameters, we want to compute a delta.
+ * We proceed in two steps:
+ * - tangent = f ( data, bias, model )
+ * - delta = g (tangent, model, dt )
+ *
+ * Notice that the `model` part appears in both stages of the computation, f() and g().
+ * This means that the chain rule for `J_delta_model` will have a double path.
+ *
+ * 1) On a first stage, we want to obtain `tangent`
+ * ------------------------------------------------
+ * We have the following partitions of our variables:
+ *
+ * tangent = [at,wt,ft,tt] -- tangent to the delta manifold. (a,w) at BASE, (f,t) at COM
+ * data = [ad,wd,fd,td] -- sensor data, all measured at BASE
+ * bias = [ab,wb] -- IMU bias. This we called `I` in previous versions
+ * model = [c,i,m] -- system dynamic model, where c = r_base_com (CoM wrt BASE)
+ * calib = [bias,model] -- system calibration parameters
+ *
+ * we have the function:
+ *
+ * tangent = f (data, bias, model)
+ *
+ * we need to obtain:
+ * - tangent
+ * - J_tangent_data
+ * - J_tangent_bias
+ * - J_tangent_model
+ *
+ * We express each one of the blocks of `tangent` wrt the blocks of `data`, `bias` and `model`:
+ *
+ * at = ad - ab --> J_at_ad = I33 ; J_at_ab = -I33
+ * wt = wd - wb --> J_wt_wd = I33 ; J_wt_wb = -I33
+ * ft = fd --> J_ft_fd = I33
+ * tt = td - c x fd --> J_tt_td = I33 ; J_tt_c = skew(fd) = fd_x ; J_tt_fd = -skew(c) = -c_x
+ *
+ * note: to obtain `tt` (at COM) we need to account for the torque measurement `td` (at BASE)
+ * and add the torque produced by the force `fd` applied at the lever arm of BASE wrt COM, which is `-c`,
+ * obtaining `tt = td - c x fd`
+ *
+ * So we can assemble the tangent vector as
+ *
+ * tangent = [at,wt,ft,tt]
+ *
+ * and all the Jacobian blocks as:
+ *
+ * ad wd fd td
+ * J_tangent_data = [ I33 033 033 033 at
+ * 033 I33 033 033 wt
+ * 033 033 I33 033 ft
+ * 033 033 -c_x I33 ] tt
+ *
+ * ab wb
+ * J_tangent_bias = [ -I33 033 at
+ * 033 -I33 wt
+ * 033 033 ft
+ * 033 033 ] tt
+ *
+ * c i m
+ * J_tangent_model = [ 033 033 031 at
+ * 033 033 031 wt
+ * 033 033 031 ft
+ * fd_x 033 031 ] tt
+ *
+ * and from here on, we just need to fill in the matrices.
+ *
+ * Notations:
+ * I33 : 3x3 Identity matrix
+ * 033 : 3x3 zero matrix
+ * 031 : 3x1 zero vector
+ *
+ *
+ * 2) On a second stage, we need to obtain the `delta` from the previous results.
+ * ------------------------------------------------------------------------------
+ *
+ * We have:
+ *
+ * delta = g (tangent, model, dt)
+ *
+ * we need to obtain:
+ *
+ * delta
+ * J_delta_tangent
+ * J_delta_model
+ *
+ * these are obtained directly by the function g() = tangent2delta().
+ *
+ * Note 1: It is unclear to me (JS, 4-aug-2022) if this function tangent2delta() is completely accurate
+ * with regard to the two different reference frames: BASE and COM. It is possible that
+ * we have to revise the math.
+ *
+ * Note 2: It is even possible that the function tangent2delta() is OK, but then that the function
+ * betweenStates() does not account for the difference in reference frames. So we also need to revise
+ * the math inside betweenStates() with regard to the two reference frames BASE and COM.
+ *
+ * 3) On a third stage, we need to apply the chain rule for the functions f() and g(),
+ * that is delta = g( f( data, bias, model), model) -- yes, it's a mess!!
+ *
+ * J_delta_data = J_delta_tangent * J_tangent_data
+ * J_delta_model = J_delta_model + J_delta_tangent * J_tangent_model <-- remember all Jacobians are PARTIAL
+ * derivatives.
+ * J_delta_bias = J_delta_tangent * J_tangent_bias
+ *
+ * Finally we can assemble the Jacobian `J_delta_calib` as the concatenation of bias and model:
+ *
+ * J_delta_calib = [ J_delta_bias | J_delta_model ]
+ *
+ * 4) On a fourth stage, we compute the covariance of the delta:
+ *
+ * Q_delta = J_delta_data * Q_data * J_delta_data.transpose()
+ *
+ * 5) The function returns the following quantities, which relate to the variables used above:
+ *
+ * - _delta = delta
+ * - _delta_cov = Q_delta
+ * - _jacobian_calib = J_delta_calib
+ *
+ * ---------------------------------------------------------------
+ *
+ * Notes: Jacobians of cross product
+ *
+ * a, b in R^3
+ *
+ * a_x, b_x, skew-symmetric matrices, in R^3x3
+ *
+ * a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
+ *
+ * a x b = a_x b
+ *
+ * a x b = - b x a = - b_x a
+ *
+ * J_(axb)_a = -b_x
+ *
+ * J_(axb)_b = a_x
+ */
+
+ // Matrix<double, 12, 1> tangent = _data;
+ // tangent.head<6>() -= _calib.head<6>(); // J_tangent_data = Identity(12,12)
+ // Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
+ // J_tangent_I.topRows<6>() = -Matrix6d::Identity();
+ // J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
+
+ // // go from tangent to delta. We need to dynamic model for this
+ // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+ // Matrix<double, 18, 12> J_delta_tangent;
+ // Matrix<double, 18, 7> J_delta_model;
+ // fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
+
+ // // Compute cov and compose jacobians
+ // Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
+ // _jacobian_calib << J_delta_I, J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
+ // const auto& J_delta_data = J_delta_tangent; // * J_tangent_data, which is the Identity;
+ // _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+
+ ////////////////////// NOU CODI QUE HAURE DE REVISAR I CANVIAR PEL QUE HI HA ADALT //////////////////////////
+
+ /*
+ * 1. tangent = f(data, bias, model) --> J_tangent_data, J_tangent_bias, J_tangent_model
+ *
+ * data = [ad,wd,fd,td]
+ * bias = [ab,wb]
+ * model = [c,i,m]
+ * tangent = [at,wt,ft,tt]
+ */
+
+ Vector6d bias = _calib.segment<6>(0);
+ Vector7d model = _calib.segment<7>(6);
+ VectorXd tangent(12);
+ MatrixXd J_tangent_data(12, 12);
+ MatrixXd J_tangent_bias(12, 6);
+ MatrixXd J_tangent_model(12, 7);
+
+ data2tangent(_data, bias, model, tangent, J_tangent_data, J_tangent_bias, J_tangent_model);
+
+ // 2. go from tangent to delta. We need again the dynamic model for this
+ Matrix<double, 18, 12> J_delta_tangent;
+ Matrix<double, 18, 7> J_delta_model;
+ fti::tangent2delta(
+ tangent, model, _dt, _delta, J_delta_tangent, J_delta_model); // Aquà ja farà bé ell sol el J_delta_model?
+
+ // 3. Compose jacobians
+ Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
+ J_delta_model += J_delta_tangent * J_tangent_model;
+ _jacobian_calib << J_delta_bias, J_delta_model; // J_delta_calib = [J_delta_bias ; J_delta_model]
+ const auto& J_delta_data = J_delta_tangent * J_tangent_data;
+
+ // 4. compute covariance
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+}
+
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+}
+
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+}
+
+void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+{
+ auto x = _x.vector("PVLO");
+ VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
+ _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
+}
+
+Eigen::VectorXd ProcessorForceTorqueInertialDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const
+{
+ return fti::plus(_delta_preint, _delta_step);
+}
+
+VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+ if (_capture)
+ {
+ VectorXd calibration(13);
+ const Vector6d& I_calib = _capture->getStateBlock('I')->getState();
+ const Vector3d& C_calib = _capture->getStateBlock('C')->getState();
+ const Vector3d& i_calib = _capture->getStateBlock('i')->getState();
+ const Vector1d& m_calib = _capture->getStateBlock('m')->getState();
+ calibration << I_calib, C_calib, i_calib, m_calib;
+ return calibration;
+ }
+ else
+ {
+ VectorXd calibration(13);
+ 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;
+ }
+}
+
+bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
+{
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_dynamics_->max_time_span)
+ {
+ WOLF_DEBUG("PM: vote: time span");
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
+ {
+ WOLF_DEBUG("PM: vote: buffer length");
+ return true;
+ }
+ // dist_traveled
+ VectorComposite X0 = getOrigin()->getFrame()->getState();
+ VectorComposite X1;
+ double dt = getBuffer().back().ts_ - getOrigin()->getFrame()->getTimeStamp();
+ statePlusDelta(X0, getBuffer().back().delta_integr_, dt, X1);
+ double dist = (X1.at('P') - X0.at('P')).norm();
+ if (dist > params_force_torque_inertial_dynamics_->dist_traveled)
+ {
+ WOLF_DEBUG("PM: vote: distance traveled");
+ return true;
+ }
+ // angle turned
+ double angle = 2.0 * acos(getMotion().delta_integr_(15));
+ if (angle > params_force_torque_inertial_dynamics_->angle_turned)
+ {
+ WOLF_DEBUG("PM: vote: angle turned");
+ return true;
+ }
+
+ return false;
+}
+
+} /* namespace wolf */
+
+#include <core/processor/factory_processor.h>
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialDynamics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialDynamics);
+} // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
deleted file mode 100644
index 1b84f786077a0f65ee3cc944bf8ebe4f54511499..0000000000000000000000000000000000000000
--- a/src/processor/processor_force_torque_preint.cpp
+++ /dev/null
@@ -1,272 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-// wolf
-#include "bodydynamics/math/force_torque_delta_tools.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-namespace wolf {
-
-int compute_data_size(int nbc, int dimc){
- // compute the size of the data/body vectors used by processorMotion API depending
- // on the number of contacts (nbc) and contact dimension (dimc)
- // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
- return nbc*dimc + 3 + 3 + nbc*3 + nbc*4;
-}
-
-using namespace Eigen;
-
-ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) :
- ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12,
- compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc),
- 6, _params_motion_force_torque_preint),
- params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)),
- nbc_(_params_motion_force_torque_preint->nbc),
- dimc_(_params_motion_force_torque_preint->dimc)
-
-{
- //
-}
-
-ProcessorForceTorquePreint::~ProcessorForceTorquePreint()
-{
- //
-}
-
-bool ProcessorForceTorquePreint::voteForKeyFrame() const
-{
- // time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span)
- {
- WOLF_DEBUG( "PM: vote: time span" );
- return true;
- }
- // buffer length
- if (getBuffer().size() > params_motion_force_torque_preint_->max_buff_length)
- {
- WOLF_DEBUG( "PM: vote: buffer length" );
- return true;
- }
-
- // Some other measure of movement?
-
- //WOLF_DEBUG( "PM: do not vote" );
- return false;
-}
-
-
-CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin)
-{
-
- // Here we have to retrieve the origin capture no
- // !! PROBLEM:
- // when doing setOrigin, emplaceCapture is called 2 times
- // - first on the first KF (usually created by setPrior), this one contains the biases
- // - secondly on the inner frame (last) which does not contains other captures
- auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
- auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
- if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){
- // We have to retrieve the bias from a former Keyframe: origin
- capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
- capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
- }
- auto cap = CaptureBase::emplace<CaptureForceTorquePreint>(
- _frame_own,
- _ts,
- _sensor,
- std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
- std::static_pointer_cast<CaptureMotion>(capture_angvel),
- _data,
- _data_cov,
- _capture_origin);
-
- // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
- // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
-
- auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
- Vector6d calib = getCalibration(cap_ftpreint);
- setCalibration(cap_ftpreint, calib);
- cap_ftpreint->setCalibrationPreint(calib);
-
- return cap;
-}
-
-FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- // Retrieving the origin capture and getting the bias from here should be done here?
- auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
- return feature;
-}
-
-Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
-{
- return bodydynamics::plus(delta_preint, delta_step);
-}
-
-VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const
-{
-
- VectorXd bias_vec(6);
-
- if (_capture) // access from capture is quicker
- {
- auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture));
-
- // get calib part from Ikin capture
- bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
-
- // get calib part from IMU capture
- bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
- }
- else // access from sensor is slower
- {
- // get calib part from Ikin capture
- bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
-
- // get calib part from IMU capture
- bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
- }
- return bias_vec;
-}
-
-void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
-{
- CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
-
- // set calib part in Ikin capture
- cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
-
- // set calib part in IMU capture
- Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
- calib_imu.tail<3>() = _calibration.tail<3>();
-
- cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
-}
-
-FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
-{
- CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
- FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion);
- CaptureForceTorquePreintPtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture());
-
- auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>(
- ftr_ftpreint,
- ftr_ftpreint,
- cap_ftpreint_origin,
- shared_from_this(),
- cap_ftpreint_origin->getIkinCaptureOther(), // to retrieve sb_bp1
- cap_ftpreint_origin->getGyroCaptureOther(), // to retrieve sb_w1
- false);
-
- return fac_ftpreint;
-}
-
-void ProcessorForceTorquePreint::computeCurrentDelta(
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jac_delta_calib) const
-{
- assert(_data.size() == data_size_ && "Wrong data size!");
-
- // create delta
- MatrixXd jac_body_bias(data_size_-nbc_,6);
- VectorXd body(data_size_);
- bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
-
- MatrixXd jac_delta_body(12,data_size_-nbc_);
- bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
- nbc_, dimc_,
- _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
-
- // [f], [tau], pbc, wb
- MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6);
-
- // compute delta_cov (using uncertainty on f,tau,pbc)
- _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
- // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity
-
- // compute jacobian_calib
- _jac_delta_calib = jac_delta_body * jac_body_bias;
-}
-
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const
-{
- _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
-}
-
-void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
-{
- assert(_delta.size() == 13 && "Wrong _delta vector size");
- assert(_dt >= 0 && "Time interval _dt is negative!");
-
- // verbose way : create Eigen vectors, then compute, then convert back to Composite
-
- auto x = _x.vector("CDLO");
-
- VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
-
- _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
-}
-
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const
-{
- bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools
-}
-
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/processor/factory_processor.h"
-
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorquePreint)
-}
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa38e8ab62d4bd0bdc0fee48bd15c227ea52fc8d
--- /dev/null
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -0,0 +1,82 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+
+#include <core/state_block/factory_state_block.h>
+#include <core/state_block/state_block_derived.h>
+
+namespace wolf
+{
+
+// TODO: remove this constructor after merging MR !448
+SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd& _extrinsics,
+ ParamsSensorForceTorqueInertialPtr _params)
+ : SensorBase("SensorForceTorqueInertial",
+ FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), true),
+ FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), true),
+ FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false), // IMU bias
+ 12,
+ false,
+ false,
+ false),
+ params_fti_(_params)
+{
+ addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass
+ addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true)); // inertial vector
+ addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true)); // total mass
+ setStateBlockDynamic('I', false);
+ setStateBlockDynamic('C', false);
+ setStateBlockDynamic('i', false);
+ setStateBlockDynamic('m', false);
+}
+
+// TODO: Adapt this API to that of MR !448
+SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite& _states,
+ ParamsSensorForceTorqueInertialPtr _params)
+ : SensorBase("SensorForceTorqueInertial",
+ FactoryStateBlock::create("StatePoint3d", _states.at('P'), false),
+ FactoryStateBlock::create("StateQuaternion", _states.at('O'), false),
+ FactoryStateBlock::create("StateParams6", _states.at('I'), false), // IMU bias
+ 12,
+ false,
+ false,
+ false),
+ params_fti_(_params)
+{
+ auto sbc = emplaceStateBlock<StateParams3>('C', getProblem(), _states.at('C'), true); // centre of mass
+ auto sbi = emplaceStateBlock<StateParams3>('i', getProblem(), _states.at('i'), true); // inertial vector
+ auto sbm = emplaceStateBlock<StateParams1>('m', getProblem(), _states.at('m'), true); // total mass
+ setStateBlockDynamic('I', false);
+ setStateBlockDynamic('C', false);
+ setStateBlockDynamic('i', false);
+ setStateBlockDynamic('m', false);
+}
+
+} // namespace wolf
+
+#include <core/sensor/factory_sensor.h>
+namespace wolf
+{
+WOLF_REGISTER_SENSOR(SensorForceTorqueInertial);
+WOLF_REGISTER_SENSOR_AUTO(SensorForceTorqueInertial);
+} // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5850e2db6763d74e6385f16417ed48102091ba5b..52e1fd976f3619fc9b303b7c9ef601c740d10677 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,14 +16,20 @@ wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinemati
wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
+wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_inertial.cpp)
+
+wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp)
wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
+wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inertial_delta_tools.cpp)
+
# TODO: revive those
-# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
+# wolf_add_gtest(gtest_feature_force_torque gtest_feature_force_torque.cpp)
# wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
-# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
+# wolf_add_gtest(gtest_processor_force_torque gtest_processor_force_torque.cpp)
+
+wolf_add_gtest(gtest_processor_force_torque_inertial_dynamics gtest_processor_force_torque_inertial_dynamics.cpp)
wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
@@ -31,3 +37,5 @@ wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
+wolf_add_gtest(gtest_solve_problem_force_torque_inertial_dynamics gtest_solve_problem_force_torque_inertial_dynamics.cpp)
+
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
deleted file mode 100644
index 0ccf5135e3d1bbe45a2c07cdbd68cf530dc9c571..0000000000000000000000000000000000000000
--- a/test/gtest_factor_force_torque.cpp
+++ /dev/null
@@ -1,865 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-/**
- * \file gtest_factor_inertial_kinematics.cpp
- *
- * Created on: Janv 29, 2020
- * \author: Mederic Fourmy
- */
-
-/*
-
-
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
-Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
-
-Tests list:
-
-FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
-FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
-*/
-
-
-// debug
-#include <iostream>
-#include <fstream>
-
-#include <core/utils/utils_gtest.h>
-#include <core/utils/logging.h>
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/state_block/state_block_derived.h>
-
-// Imu
-// #include "imu/internal/config.h"
-// #include "imu/capture/capture_imu.h"
-// #include "imu/processor/processor_imu.h"
-// #include "imu/sensor/sensor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_force_torque.h"
-#include "bodydynamics/factor/factor_force_torque.h"
-
-// ODOM3d
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-#include "core/factor/factor_relative_pose_3d.h"
-
-#include <Eigen/Eigenvalues>
-
-using namespace wolf;
-using namespace Eigen;
-using namespace std;
-
-
-// SOME CONSTANTS
-const double Acc1 = 1;
-const double Acc2 = 3;
-const Vector3d zero3 = Vector3d::Zero();
-const Vector6d zero6 = Vector6d::Zero();
-
-
-
-Matrix9d computeKinJac(const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d J; J.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- J.topLeftCorner<3,3>() = Matrix3d::Identity();
- // J.block<3,3>(0,3) = Matrix3d::Zero();
- // J.topRightCorner<3,3>() = Matrix3d::Zero();
- J.block<3,3>(3,0) = -wx;
- J.block<3,3>(3,3) = -Matrix3d::Identity();
- J.block<3,3>(3,6) = px;
- // J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- // J.block<3,3>(6,3) = Matrix3d::Zero();
- J.bottomRightCorner<3,3>() = -Iq;
-
- return J;
-}
-
-Matrix9d computeKinCov(const Matrix3d& Qp,
- const Matrix3d& Qv,
- const Matrix3d& Qw,
- const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d cov; cov.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- cov.topLeftCorner<3,3>() = Qp;
- cov.block<3,3>(0,3) = Qp * wx;
- // cov.topRightCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
- cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
- cov.block<3,3>(3,6) = -px*Qw*Iq;
- // cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(6,3) = Iq*Qw*px;
- cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
-
- return cov;
-}
-
-
-void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
- if(!KF->getStateBlock(sb_name)->isFixed())
- {
- if (sb_name == 'O')
- {
- double max_rad_pert = M_PI / 3;
- Vector3d do_pert = max_rad_pert*Vector3d::Random();
- Quaterniond curr_state_q(KF->getO()->getState().data());
- KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
- }
- else
- {
- VectorXd pert;
- pert.resize(KF->getStateBlock(sb_name)->getSize());
- pert.setRandom(); pert *= 0.5;
- KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
- }
- }
-}
-
-void perturbateAllIfUnFixed(const FrameBasePtr& KF)
-{
- for (auto it: KF->getStateBlockMap()){
- perturbateIfUnFixed(KF, it.first);
- }
-}
-
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
- const TimeStamp& t,
- VectorComposite x_origin,
- Vector3d c,
- Vector3d cd,
- Vector3d Lc,
- Vector6d bias_imu)
-{
- FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
- StateBlockPtr sbc = make_shared<StatePoint3d>(c);
- KF->addStateBlock('C', sbc, problem);
- StateBlockPtr sbd = make_shared<StateVector3d>(cd);
- KF->addStateBlock('D', sbd, problem);
- StateBlockPtr sbL = make_shared<StateVector3d>(Lc);
- KF->addStateBlock('L', sbL, problem);
- StateBlockPtr sbbimu = make_shared<StateParams6>(bias_imu);
- KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
- return KF;
-}
-
-class FactorInertialKinematics_2KF : public testing::Test
-{
- public:
- double mass_;
- wolf::TimeStamp tA_;
- wolf::TimeStamp tB_;
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- VectorComposite x_origin_; // initial state
- VectorComposite s_origin_; // initial sigmas for prior
- SensorInertialKinematicsPtr SIK_;
- CaptureInertialKinematicsPtr CIKA_, CIKB_;
- FrameBasePtr KFA_;
- FrameBasePtr KFB_;
- Matrix3d Qp_, Qv_, Qw_;
- Vector3d bias_p_;
- Vector6d bias_imu_;
- FeatureInertialKinematicsPtr FIKA_;
- FeatureInertialKinematicsPtr FIKB_;
- FeatureForceTorquePtr FFTAB_;
- // SensorImuPtr sen_imu_;
- // ProcessorImuPtr processor_imu_;
-
- protected:
- void SetUp() override
- {
-
- std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- mass_ = 10.0; // Small 10 kg robot
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- problem_ = Problem::create("POV", 3);
-
- VectorXd extr_ik(0);
- ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = 0.1;
- intr_ik->std_vbc = 0.1;
- auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
- SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
-
- // CERES WRAPPER
- solver_ = std::make_shared<SolverCeres>(problem_);
- solver_->getSolverOptions().max_num_iterations = 1e3;
- // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
-
- // INSTALL Imu SENSOR
- // Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
- // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
- // sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
- // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
- // Vector6d data = zero6;
- // wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
- // // sen_imu_->process(imu_ptr);
-
- // ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
- tA_.set(0);
- x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
- s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
- KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_);
-
-
- // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- bias_p_ = zero3;
- bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StatePoint3d>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateParams6>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
-
- // Fix the one we cannot estimate
- // KFA_->getP()->fix();
- // KFA_->getO()->fix();
- // KFA_->getV()->fix();
- KFA_->getStateBlock('I')->fix(); // Imu
-
- // INERTIAL KINEMATICS FACTOR
- // Measurements
- Vector3d pBC_measA = zero3;
- Vector3d vBC_measA = zero3;
- Vector3d w_measA = zero3;
- Vector9d meas_ikinA; meas_ikinA << pBC_measA, vBC_measA, w_measA;
- // momentum parameters
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
-
- Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
- Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
-
- CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
- CIKA_->getStateBlock('I')->fix(); // IK bias
- FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
-
-
- // =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
- tB_.set(1);
-
- KFB_ = createKFWithCDLI(problem_, tB_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
- // Fix the one we cannot estimate
- // KFB_->getP()->fix();
- KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
- // KFB_->getV()->fix();
- KFB_->getStateBlock('I')->fix(); // Imu
-
-
- // // ================ PROCESS Imu DATA
- // Vector6d data_imu;
- // data_imu << -wolf::gravity(), 0,0,0;
- // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on Imu (measure only gravity here)
- // // process data in capture
- // // sen_imu_->process(cap_imu);
-
- // ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measB = zero3;
- Vector3d vBC_measB = zero3;
- Vector3d w_measB = zero3;
- Vector9d meas_ikinB; meas_ikinB << pBC_measB, vBC_measB, w_measB;
- CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
- CIKB_->getSensorIntrinsic()->fix(); // IK bias
- FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
-
-
- // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
- // aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
- CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr);
- FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
- tB_ - tA_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
-
- FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
-
- }
-};
-
-
-class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- cout << "\n\n\nFactorInertialKinematics_2KF_foot1turned" << endl;
- FactorInertialKinematics_2KF::SetUp();
- problem_->print(3,false,true,true);
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
- Vector3d f2 = -mass_*wolf::gravity()/2;
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- // problem_->print(3,false,true,true);
- }
-};
-
-class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- Vector3d f2 = -mass_*wolf::gravity()/2;
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity();
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
- Vector3d f2 = zero3;
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot1turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
- Vector3d f1 = zero3;
- Vector3d f2 = -mass_*wolf::gravity();
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-
-
-
-class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
-{
- public:
- FrameBasePtr KFC_;
- CaptureInertialKinematicsPtr CIKC_;
- TimeStamp tC_;
-
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF_ForceX::SetUp();
- tC_.set(2);
-
- // KFB_->getStateBlock("O")->unfix();
-
- KFC_ = createKFWithCDLI(problem_, tC_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
- // Fix the one we cannot estimate
- // KFB_->getP()->fix();
- // KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
- // KFB_->getV()->fix();
- KFC_->getStateBlock('I')->fix();
-
- // ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measC = zero3;
- Vector3d vBC_measC = zero3;
- Vector3d w_measC = zero3;
- Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
- Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
-
- CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
- CIKC_->getStateBlock('I')->fix(); // IK bias
- auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
-
-
- // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
- // aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
- CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
- auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
- tC_ - tB_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
- FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinematics_2KF_ForceX
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF_ForceX::SetUp();
-
- Vector7d pose_A_B; pose_A_B << (tB_-tA_)*Acc1/2,0,0, 0,0,0,1;
- Matrix6d rel_pose_cov = Matrix6d::Identity();
- rel_pose_cov.topLeftCorner(3, 3) *= pow(1e-3, 2);
- rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD*1e-3, 2);
-
- CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
- FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
- FactorBase::emplace<FactorRelativePose3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false, TOP_MOTION);
-
- // With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
- CIKB_->getStateBlock('I')->unfix();
- // However this test is not sufficient to observe the bias at KFA
- // CIKA_->getStateBlock('I')->unfix(); // this is not observable
- }
-};
-
-
-
-
-
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-// =============== TEST FONCTIONS ======================
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-
-
-TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
-{
- // problem_->print(4,true,true,true);
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
-// {
-// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-// perturbateAllIfUnFixed(KFA_);
-// perturbateAllIfUnFixed(KFB_);
-// perturbateAllIfUnFixed(KFC_);
-
-// // problem_->print(4,true,true,true);
-// report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-// // std::cout << report << std::endl;
-// // problem_->print(4,true,true,true);
-
-// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
-// }
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
-
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..72e5f59675df274e4387876ae644c081a61798ab
--- /dev/null
+++ b/test/gtest_factor_force_torque_inertial.cpp
@@ -0,0 +1,136 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial.h"
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+#include <Eigen/Dense>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+// Register StateBlock of type "L"
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialPreint : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorForceTorqueInertialPtr sensor;
+ ProcessorForceTorqueInertialPtr processor;
+ FrameBasePtr frame_origin, frame_last, frame;
+ CaptureMotionPtr capture_origin, capture_last, capture;
+ FeatureMotionPtr feature;
+ FactorForceTorqueInertialPtr factor;
+
+ VectorXd data;
+ MatrixXd data_cov;
+ VectorXd delta;
+ MatrixXd delta_cov;
+ Vector6d bias;
+ MatrixXd J_delta_bias;
+
+ protected:
+ void SetUp() override
+ {
+ data = VectorXd::Random(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ problem = Problem::autoSetup(server);
+
+ sensor = std::static_pointer_cast<SensorForceTorqueInertial>(problem->getHardware()->getSensorList().front());
+ processor = std::static_pointer_cast<ProcessorForceTorqueInertial>(sensor->getProcessorList().front());
+
+ problem->print(4, 1, 1, 1);
+
+ capture = std::make_shared<CaptureMotion>("CaptureForceTorqueInertial", 0.0, sensor, data, data_cov, nullptr);
+ capture->process();
+
+ problem->print(4, 1, 1, 1);
+
+ capture_origin = std::static_pointer_cast<CaptureMotion>(processor->getOrigin());
+ frame_origin = capture_origin->getFrame();
+
+ ASSERT_EQ(capture_origin->getTimeStamp(), frame_origin->getTimeStamp());
+ ASSERT_EQ(capture_origin->getTimeStamp(), frame_origin->getCaptureOf(sensor)->getTimeStamp());
+
+ capture = std::make_shared<CaptureMotion>("CaptureForceTorqueInertial", 0.1, sensor, data, data_cov, nullptr);
+ capture->process();
+
+ capture_last = std::static_pointer_cast<CaptureMotion>(processor->getLast());
+ frame_last = capture_last->getFrame();
+ }
+};
+
+TEST(Dummy, force_load_libwolfbodydynamics_so_at_launch_time)
+{
+ ParamsSensorForceTorqueInertialPtr p = std::make_shared<ParamsSensorForceTorqueInertial>();
+ p->acc_noise_std = 1;
+ p->force_noise_std = 1;
+ p->gyro_noise_std = 1;
+ p->torque_noise_std = 1;
+ SensorForceTorqueInertial s((Vector7d() << 1, 2, 3, 0, 0, 0, 1).finished(), p);
+}
+
+TEST_F(Test_FactorForceTorqueInertialPreint, constructor)
+{
+ feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19),
+ MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
+ FactorForceTorqueInertial f(feature, capture_origin, processor, false);
+
+ WOLF_INFO("f id: ", f.id());
+
+ ASSERT_EQ(f.getCaptureOther(), capture_origin);
+}
+
+TEST_F(Test_FactorForceTorqueInertialPreint, emplace)
+{
+ feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19),
+ MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
+ factor = FactorBase::emplace<FactorForceTorqueInertial>(feature, feature, capture_origin, processor, false);
+ // this ^^^ will create a warning "ADDING FACTOR id TO FEATURE id THAT IS NOT CONNECTED WITH PROBLEM."
+
+ problem->print(4, 1, 1, 1); // feature and factor not in the tree since they belong to last_ptr with no KF.
+
+ ASSERT_EQ(factor->getCaptureOther(), capture_origin);
+ ASSERT_EQ(factor->getCapture(), capture_last);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..846807390641df274b1b0d9cf8542183e6948067
--- /dev/null
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,357 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ FrameBasePtr F0, F1, F;
+ CaptureMotionPtr C0, C1, C;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
+
+ VectorXd data;
+ MatrixXd data_cov;
+ VectorXd delta;
+ MatrixXd delta_cov;
+ Matrix<double, 13, 1> calib;
+ MatrixXd J_delta_calib;
+
+ protected:
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+
+ data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+
+ C->process();
+
+ C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ F0 = C0->getFrame();
+
+ ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
+ ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
+
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C->process();
+
+ C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
+ F1 = C1->getFrame();
+
+ F1->link(P); // by the face
+
+ VectorXd delta_preint(VectorXd::Zero(19));
+ delta_preint.head<3>() = -0.5*gravity();
+ delta_preint.segment<3>(3) = -gravity();
+ delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint.segment<3>(9) = -gravity();
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+ jacobian_calib);
+
+ c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+
+ F1->getStateBlock('P')->setState(Vector3d(0,0,0));
+ F1->getStateBlock('V')->setState(Vector3d(0,0,0));
+ F1->getStateBlock('O')->setState(Vector4d(0,0,0,1));
+ F1->getStateBlock('L')->setState(Vector3d(0,0,0));
+
+ }
+
+};
+
+
+class Test_FactorForceTorqueInertialDynamics : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
+
+ VectorXd data;
+ MatrixXd data_cov;
+ VectorXd delta;
+ MatrixXd delta_cov;
+ Matrix<double, 13, 1> calib;
+ MatrixXd J_delta_calib;
+
+ protected:
+ void SetUp() override
+ {
+ data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+
+ // crear un problem
+ P = Problem::create("POVL", 3);
+
+ // crear un sensor
+ auto params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
+ Vector7d extrinsics;
+ extrinsics << 0, 0, 0, 0, 0, 0, 1;
+ S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
+ S->setStateBlockDynamic('I', true);
+
+ // crear processador
+ auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>();
+ p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
+
+ // crear dos frame
+ VectorXd state(13);
+ state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
+ VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
+ F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
+ F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
+
+ // crear dues capture
+ VectorXd data(VectorXd::Zero(12));
+ MatrixXd data_cov(MatrixXd::Identity(12, 12));
+ Vector6d bias(Vector6d::Zero());
+ auto sb_bias = std::make_shared<StateParams6>(bias);
+ C0 = CaptureBase::emplace<CaptureMotion>(F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr,
+ nullptr, sb_bias);
+ C1 = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr,
+ sb_bias);
+
+ // crear un feature
+ VectorXd delta_preint(VectorXd::Zero(19));
+ delta_preint.head<3>() = -0.5*gravity();
+ delta_preint.segment<3>(3) = -gravity();
+ delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint.segment<3>(9) = -gravity();
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+ jacobian_calib);
+
+ // crear un factor
+ c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+ }
+};
+
+// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion)
+// {
+// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+// }
+
+//Test to see if the constructor works (not yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
+{
+ ASSERT_EQ(c1->getCapture(), C1);
+ ASSERT_EQ(c1->getCalibPreint().size(), 13);
+}
+
+//Test to see if the constructor works (yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
+{
+ ASSERT_EQ(c1->getCapture(), C1);
+ ASSERT_EQ(c1->getCalibPreint().size(), 13);
+}
+
+//Test en el que no hi ha moviment (not yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
+{
+ VectorXd res_exp = VectorXd::Zero(18);
+ Matrix<double, 18, 1> res;
+ VectorXd bias = VectorXd::Zero(6);
+
+ P->print(4,1,1,1);
+
+ c1->residual(F0->getStateBlock('P')->getState(), // p0
+ Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+ F0->getStateBlock('V')->getState(), // v0
+ F0->getStateBlock('L')->getState(), // L0
+ bias, // I
+ F1->getStateBlock('P')->getState(), // p1
+ Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+ F1->getStateBlock('V')->getState(), // v1
+ F1->getStateBlock('L')->getState(), // L1
+ S->getCom(), // C
+ S->getInertia(), // i
+ S->getMass(), // m
+ res);
+
+ ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+//Test en el que no hi ha moviment (yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
+{
+ VectorXd res_exp = VectorXd::Zero(18);
+ Matrix<double, 18, 1> res;
+ VectorXd bias = VectorXd::Zero(6);
+
+ c1->residual(F0->getStateBlock('P')->getState(), // p0
+ Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+ F0->getStateBlock('V')->getState(), // v0
+ F0->getStateBlock('L')->getState(), // L0
+ bias, // I
+ F1->getStateBlock('P')->getState(), // p1
+ Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+ F1->getStateBlock('V')->getState(), // v1
+ F1->getStateBlock('L')->getState(), // L1
+ S->getCom(), // C
+ S->getInertia(), // i
+ S->getMass(), // m
+ res);
+
+ ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+
+//Test en el que s'avança 1m en direcció x (not yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
+{
+ VectorXd res_exp = VectorXd::Zero(18);
+ Matrix<double, 18, 1> res;
+ VectorXd bias = VectorXd::Zero(6);
+
+ //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+ F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+
+ //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+ VectorXd delta_preint(VectorXd::Zero(19));
+ delta_preint.head<3>() = -0.5*gravity();
+ delta_preint(0) = 1;
+ delta_preint.segment<3>(3) = -gravity();
+ delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint(6) = 1;
+ delta_preint.segment<3>(9) = -gravity();
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
+
+ FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+
+ c2->residual(F0->getStateBlock('P')->getState(), // p0
+ Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+ F0->getStateBlock('V')->getState(), // v0
+ F0->getStateBlock('L')->getState(), // L0
+ bias, // I
+ F1->getStateBlock('P')->getState(), // p1
+ Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+ F1->getStateBlock('V')->getState(), // v1
+ F1->getStateBlock('L')->getState(), // L1
+ S->getCom(), // C
+ S->getInertia(), // i
+ S->getMass(), // m
+ res);
+
+ ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+//Test en el que s'avança 1m en direcció x (fitxers yaml)
+
+TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
+{
+ VectorXd res_exp = VectorXd::Zero(18);
+ Matrix<double, 18, 1> res;
+ VectorXd bias = VectorXd::Zero(6);
+
+ //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+ F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+
+ //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+ VectorXd delta_preint(VectorXd::Zero(19));
+ delta_preint.head<3>() = -0.5*gravity();
+ delta_preint(0) = 1;
+ delta_preint.segment<3>(3) = -gravity();
+ delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint(6) = 1;
+ delta_preint.segment<3>(9) = -gravity();
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
+
+ FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+
+ c2->residual(F0->getStateBlock('P')->getState(), // p0
+ Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+ F0->getStateBlock('V')->getState(), // v0
+ F0->getStateBlock('L')->getState(), // L0
+ bias, // I
+ F1->getStateBlock('P')->getState(), // p1
+ Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+ F1->getStateBlock('V')->getState(), // v1
+ F1->getStateBlock('L')->getState(), // L1
+ S->getCom(), // C
+ S->getInertia(), // i
+ S->getMass(), // m
+ res);
+
+ ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_force_torque_inertial_delta_tools.cpp b/test/gtest_force_torque_inertial_delta_tools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58152170e01188454a1793a0958d087878680dde
--- /dev/null
+++ b/test/gtest_force_torque_inertial_delta_tools.cpp
@@ -0,0 +1,691 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * gtest_force_torque_inertial_tools.cpp
+ *
+ * Created on: Aug 8, 2021
+ * Author: jsola
+ */
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/utils/utils_gtest.h>
+
+#include <iostream>
+#include <iomanip>
+
+using namespace Eigen;
+using namespace wolf;
+using namespace bodydynamics;
+using namespace fti;
+
+TEST(FTI_tools, identity)
+{
+ VectorXd id_true;
+ id_true.setZero(19);
+ id_true(18) = 1.0;
+
+ VectorXd id = identity<double>();
+ ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+TEST(FTI_tools, identity_split)
+{
+ VectorXd pi(3), vi(3), pd(3), vd(3), L(3), qv(4);
+ Quaterniond q;
+
+ identity(pi, vi, pd, vd, L, q);
+ ASSERT_MATRIX_APPROX(pi, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(vi, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(vd, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(L, Vector3d::Zero(), 1e-10);
+ ASSERT_QUATERNION_APPROX(q, Quaterniond::Identity(), 1e-10);
+
+ identity(pi, vi, pd, vd, L, qv);
+ ASSERT_MATRIX_APPROX(pi, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(vi, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(vd, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(L, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(qv, (Vector4d() << 0, 0, 0, 1).finished(), 1e-10);
+}
+
+TEST(FTI_tools, inverse)
+{
+ VectorXd d(19), id(19), iid(19), iiid(19), I(19);
+ Vector4d qv;
+ double dt = 0.1;
+
+ qv = (Vector4d() << 9, 10, 11, 12).finished().normalized();
+ d << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, qv;
+
+ id = inverse(d, dt);
+
+ compose(id, d, dt, I);
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+ compose(d, id, -dt, I); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+
+ inverse(id, -dt, iid); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(d, iid, 1e-10);
+ iiid = inverse(iid, dt);
+ ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
+}
+
+TEST(FTI_tools, compose_between)
+{
+ VectorXd dx1(19), dx2(19), dx3(19);
+ Matrix<double, 19, 1> d1, d2, d3;
+ double dt = 0.1;
+
+ dx1 << VectorXd::Random(15), Vector4d::Random().normalized();
+ d1 = dx1;
+ dx2 << VectorXd::Random(15), Vector4d::Random().normalized();
+ d2 = dx2;
+
+ // combinations of templates for sum()
+ compose(dx1, dx2, dt, dx3);
+ compose(d1, d2, dt, d3);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ compose(d1, dx2, dt, dx3);
+ d3 = compose(dx1, d2, dt);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ // // minus(d1, d3) should recover delta_2
+ Matrix<double, 19, 1> bet;
+ between(d1, d3, dt, bet);
+ ASSERT_MATRIX_APPROX(bet, d2, 1e-10);
+
+ // minus(d3, d1, -dt) should recover inverse(d2, dt)
+ bet = between(d3, d1, -dt);
+ ASSERT_MATRIX_APPROX(bet, inverse(d2, dt), 1e-10);
+};
+
+TEST(FTI_tools, compose_between_with_state)
+{
+ VectorXd x(13), d(19), x2(13), x3(13), d2(19), d3(19);
+ double dt = 0.1;
+
+ x << VectorXd::Random(9), Vector4d::Random().normalized();
+ d << VectorXd::Random(15), Vector4d::Random().normalized();
+
+ {
+ // 1. compose over state with IMU delta
+ composeOverState(x, d, dt, x2);
+ x3 = composeOverState(x, d, dt);
+ ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
+
+ // betweenStates(x, x2) should recover d
+ betweenStates(x, x2, dt, d2);
+ d3 = betweenStates(x, x2, dt);
+ ASSERT_MATRIX_APPROX(d2, d3, 1e-10);
+ VectorComposite dc(d, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorComposite d2c(d2, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorComposite d3c(d3, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorXd de = dc.vector("PVLO");
+ VectorXd d2e = d2c.vector("PVLO");
+ VectorXd d3e = d3c.vector("PVLO");
+ ASSERT_MATRIX_APPROX(d2c.vector("PVLO"), dc.vector("PVLO"), 1e-10);
+ ASSERT_MATRIX_APPROX(d3c.vector("PVLO"), dc.vector("PVLO"), 1e-10);
+ }
+ {
+ // 2. compose over state with FT delta
+ composeOverState(x, d, dt, x2, fti::FT);
+ x3 = composeOverState(x, d, dt, fti::FT);
+ ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
+
+ // betweenStates(x, x2) should recover d
+ betweenStates(x, x2, dt, d2);
+ d3 = betweenStates(x, x2, dt);
+ ASSERT_MATRIX_APPROX(d2, d3, 1e-10);
+ VectorComposite dc(d, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorComposite d2c(d2, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorComposite d3c(d3, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ VectorXd de = dc.vector("pvLO");
+ VectorXd d2e = d2c.vector("pvLO");
+ VectorXd d3e = d3c.vector("pvLO");
+ ASSERT_MATRIX_APPROX(d2c.vector("pvLO"), dc.vector("pvLO"), 1e-10);
+ ASSERT_MATRIX_APPROX(d3c.vector("pvLO"), dc.vector("pvLO"), 1e-10);
+ }
+
+ composeOverState(x, d, dt, x2);
+ VectorComposite ddc(d, "PVpvLO", {3, 3, 3, 3, 3, 4});
+ ddc.at('p') = ddc.at('P');
+ ddc.at('v') = ddc.at('V');
+ ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), ddc.vector("PVpvLO"), 1e-10);
+
+ // x + (x2 - x) = x2
+ ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
+}
+
+TEST(FTI_tools, compose_between_with_state_composite)
+{
+ double dt = 0.1;
+
+ VectorComposite x(VectorXd::Random(13), "PVLO", {3,3,3,4});
+ x.at('O').normalize();
+ VectorComposite d(VectorXd::Random(19), "PVpvLO", {3,3,3,3,3,4});
+ d.at('O').normalize();
+
+ VectorComposite xi = composeOverState(x, d, dt, fti::IMU);
+ VectorComposite xf = composeOverState(x, d, dt, fti::FT);
+
+ ASSERT_MATRIX_APPROX(xi.at('L'), xf.at('L'), 1e-10);
+ ASSERT_MATRIX_APPROX(xi.at('O'), xf.at('O'), 1e-10);
+
+ // Obtained delta must conform to IMU "PV"
+ VectorComposite d2 = betweenStates(x, xi, dt);
+
+ ASSERT_MATRIX_APPROX(d2.at('P'), d.at('P'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('V'), d.at('V'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('p'), d.at('P'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('v'), d.at('V'), 1e-10);
+
+ // Obtained delta must conform to FT "pv"
+ d2 = betweenStates(x, xf, dt);
+
+ ASSERT_MATRIX_APPROX(d2.at('P'), d.at('p'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('V'), d.at('v'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('p'), d.at('p'), 1e-10);
+ ASSERT_MATRIX_APPROX(d2.at('v'), d.at('v'), 1e-10);
+}
+
+TEST(FTI_tools, exp_log)
+{
+ VectorXd tangent(18);
+ tangent << VectorXd::Random(15), .1 * Vector3d::Random(); // use angles in the ball theta < pi
+
+ // transform to delta
+ VectorXd delta = exp_fti(tangent);
+
+ // expected delta
+ Vector3d dpi = tangent.segment<3>(0);
+ Vector3d dvi = tangent.segment<3>(3);
+ Vector3d dpd = tangent.segment<3>(6);
+ Vector3d dvd = tangent.segment<3>(9);
+ Vector3d dL = tangent.segment<3>(12);
+ Quaterniond dq = v2q(tangent.segment<3>(15));
+ VectorXd delta_true(19);
+ delta_true << dpi, dvi, dpd, dvd, dL, dq.coeffs();
+ ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+
+ // transform to a new tangent -- should be the original tangent
+ VectorXd d_from_delta = log_fti(delta);
+ ASSERT_MATRIX_APPROX(d_from_delta, tangent, 1e-10);
+
+ // transform to a new delta -- should be the previous delta
+ VectorXd delta_from_d = exp_fti(d_from_delta);
+ ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
+
+TEST(FTI_tools, plus)
+{
+ VectorXd d1(19), d2(19), d3(19);
+ VectorXd err(18);
+ Vector4d qv = Vector4d::Random().normalized();
+ d1 << VectorXd::Random(15), qv;
+ err << Matrix<double, 18, 1>::Random();
+
+ d3.head<3>() = d1.head<3>() + err.head<3>();
+ d3.segment<3>(3) = d1.segment<3>(3) + err.segment<3>(3);
+ d3.segment<3>(6) = d1.segment<3>(6) + err.segment<3>(6);
+ d3.segment<3>(9) = d1.segment<3>(9) + err.segment<3>(9);
+ d3.segment<3>(12) = d1.segment<3>(12) + err.segment<3>(12);
+ d3.tail<4>() = (Quaterniond(qv.data()) * exp_q(err.tail<3>())).coeffs();
+
+ plus(d1, err, d2);
+ ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(18), 1e-10);
+}
+
+TEST(FTI_tools, diff)
+{
+ VectorXd d1(19), d2(19);
+ VectorXd err(18);
+ Vector4d qv = Vector4d::Random().normalized();
+ d1 << VectorXd::Random(15), qv;
+ d2 = d1;
+
+ diff(d1, d2, err);
+ ASSERT_MATRIX_APPROX(err, VectorXd::Zero(18), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(18), 1e-10);
+
+ VectorXd d3(19);
+ d3.setRandom();
+ d3.tail<4>().normalize();
+ err.head<3>() = d3.head<3>() - d1.head<3>();
+ err.segment<3>(3) = d3.segment<3>(3) - d1.segment<3>(3);
+ err.segment<3>(6) = d3.segment<3>(6) - d1.segment<3>(6);
+ err.segment<3>(9) = d3.segment<3>(9) - d1.segment<3>(9);
+ err.segment<3>(12) = d3.segment<3>(12) - d1.segment<3>(12);
+ err.tail<3>() = log_q(Quaterniond(d1.data() + 15).conjugate() * Quaterniond(d3.data() + 15));
+
+ ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
+}
+
+TEST(FTI_tools, compose_jacobians)
+{
+ VectorXd d1(19), d2(19), d3(19), d1_pert(19), d2_pert(19), d3_pert(19), d3_pert_approx(19); // deltas
+ VectorXd h(18), j_h(18); // tangent perturbation
+ Matrix<double, 18, 18> J_d3_d1, J_d3_d2, J_d3_d1_num, J_d3_d2_num;
+ Vector4d qv1, qv2;
+ double dt = 0.1;
+ double dx = 1e-6;
+ qv1 = Vector4d::Random().normalized();
+ d1 << VectorXd::Random(15), qv1;
+ qv2 = Vector4d::Random().normalized();
+ d2 << VectorXd::Random(15), qv2;
+
+ // analytical jacobians
+ compose(d1, d2, dt, d3, J_d3_d1, J_d3_d2);
+
+ ASSERT_MATRIX_APPROX(d3, compose(d1, d2, dt), 1e-12);
+
+ // Check Jacobians using f( x (+) h ) ~ f(x) (+) J_x_d * h
+ // this implies
+ // - compose( d1(+)h , d2 ) ~ compose( d1, d2) (+) J_d3_d1*h
+ // - compose( d1 , d2(+)h ) ~ compose( d1, d2) (+) J_d3_d2*h
+
+ h = dx * VectorXd::Random(18);
+
+ // Jac wrt. d1
+ d1_pert = plus(d1, h);
+ d3_pert = compose(d1_pert, d2, dt);
+ j_h = J_d3_d1 * h;
+ d3_pert_approx = plus(d3, j_h);
+
+ ASSERT_MATRIX_APPROX(d3_pert, d3_pert_approx, 1e-8);
+
+ // Jac wrt. d2
+ d2_pert = plus(d2, h);
+ d3_pert = compose(d1, d2_pert, dt);
+ j_h = J_d3_d2 * h;
+ d3_pert_approx = plus(d3, j_h);
+
+ ASSERT_MATRIX_APPROX(d3_pert, d3_pert_approx, 1e-12);
+}
+
+TEST(FTI_tools, data2tangent_hover)
+{
+ Vector3d am(1, 2, 3), wm(1, 2, 3), ab(0, 0, 0), wb(0, 0, 0), fb(0, 0, 0), mb(0, 0, 0), r_bc(0, 0, 0);
+
+ std::list<Propeller> propellers;
+ propellers.emplace_back(Vector3d(1, 1, 0), Matrix3d::Identity(), 1, 1, 1); // FL
+ propellers.emplace_back(Vector3d(-1, 1, 0), Matrix3d::Identity(), -1, 1, 1); // RL
+ propellers.emplace_back(Vector3d(-1, -1, 0), Matrix3d::Identity(), 1, 1, 1); // RR
+ propellers.emplace_back(Vector3d(1, -1, 0), Matrix3d::Identity(), -1, 1, 1); // FR
+
+ Vector3d a, w, f, m;
+
+ Vector4d n(2, 2, 2, 2); // FL, RL, RR, FR
+
+ data2tangent(am, wm, n, ab, wb, fb, mb, r_bc, propellers, a, w, f, m);
+
+ ASSERT_MATRIX_APPROX(a, am - ab, 1e-12);
+ ASSERT_MATRIX_APPROX(w, wm - wb, 1e-12);
+ ASSERT_MATRIX_APPROX(f, Vector3d(0, 0, 16), 1e-12);
+ ASSERT_MATRIX_APPROX(m, Vector3d::Zero(), 1e-12);
+}
+
+TEST(FTI_tools, data2tangent_yawing)
+{
+ Vector3d am(1, 2, 3), wm(1, 2, 3), ab(0, 0, 0), wb(0, 0, 0), fb(0, 0, 0), mb(0, 0, 0), r_bc(0, 0, 0);
+
+ std::list<Propeller> propellers;
+ propellers.emplace_back(Vector3d(1, 1, 0), Matrix3d::Identity(), 1, 1, 1); // FL
+ propellers.emplace_back(Vector3d(-1, 1, 0), Matrix3d::Identity(), -1, 1, 1); // RL
+ propellers.emplace_back(Vector3d(-1, -1, 0), Matrix3d::Identity(), 1, 1, 1); // RR
+ propellers.emplace_back(Vector3d(1, -1, 0), Matrix3d::Identity(), -1, 1, 1); // FR
+
+ Vector3d a, w, f, m;
+
+ Vector4d n(2, 0, 2, 0); // FL, RL, RR, FR
+
+ data2tangent(am, wm, n, ab, wb, fb, mb, r_bc, propellers, a, w, f, m);
+
+ ASSERT_MATRIX_APPROX(a, am - ab, 1e-12);
+ ASSERT_MATRIX_APPROX(w, wm - wb, 1e-12);
+ ASSERT_MATRIX_APPROX(f, Vector3d(0, 0, 8), 1e-12);
+ ASSERT_MATRIX_APPROX(m, Vector3d(0, 0, -8), 1e-12);
+}
+
+TEST(FTI_tools, data2tangent_jacobians)
+{
+ VectorXd data(10), bias(12), tangent(12);
+ Vector3d r_bc;
+
+ std::list<Propeller> propellers;
+ propellers.emplace_back(Vector3d(1, 1, 0), Matrix3d::Identity(), 1, 1, 1); // FL
+ propellers.emplace_back(Vector3d(-1, 1, 0), Matrix3d::Identity(), -1, 1, 1); // RL
+ propellers.emplace_back(Vector3d(-1, -1, 0), Matrix3d::Identity(), 1, 1, 1); // RR
+ propellers.emplace_back(Vector3d(1, -1, 0), Matrix3d::Identity(), -1, 1, 1); // FR
+
+ MatrixXd J_tan_data(12, 10), J_tan_bias(12, 12), J_tan_com(12, 3);
+
+ data.setRandom();
+ bias.setRandom();
+ r_bc.setRandom();
+
+ data2tangent(data, bias, r_bc, propellers, tangent, J_tan_data, J_tan_bias, J_tan_com);
+
+ // all jacobians
+ VectorXd tangent_pert(12), tangent_pert_aprox(12);
+ double dx = 1e-6;
+ VectorXd pert;
+
+ // jac wrt data
+ pert = data.Random(10) * dx;
+ VectorXd data_pert = data + pert;
+ data2tangent(data_pert, bias, r_bc, propellers, tangent_pert);
+ tangent_pert_aprox = tangent + J_tan_data * pert;
+
+ ASSERT_MATRIX_APPROX(tangent_pert, tangent_pert_aprox, 1e-10);
+
+ // jac wrt bias
+ pert = bias.Random(12) * dx;
+ VectorXd bias_pert = bias + pert;
+ data2tangent(data, bias_pert, r_bc, propellers, tangent_pert);
+ tangent_pert_aprox = tangent + J_tan_bias * pert;
+
+ ASSERT_MATRIX_APPROX(tangent_pert, tangent_pert_aprox, 1e-10);
+
+ // jac wrt com
+ pert = r_bc.Random(3) * dx;
+ VectorXd r_bc_pert = r_bc + pert;
+ data2tangent(data, bias, r_bc_pert, propellers, tangent_pert);
+ tangent_pert_aprox = tangent + J_tan_com * pert;
+
+ ASSERT_MATRIX_APPROX(tangent_pert, tangent_pert_aprox, 1e-10);
+}
+
+TEST(FTI_tools, forces2acc_onlyforce)
+{
+ Vector3d force(1, 2, 3);
+ Vector3d torque(0, 0, 0);
+ Vector3d angvel(0, 0, 0);
+ Vector3d com(0, 0, 0);
+ Vector3d inertia(1, 1, 1);
+ double mass(10);
+ Vector3d acc;
+
+ //
+ forces2acc(force, torque, angvel, com, inertia, mass, acc);
+ ASSERT_MATRIX_APPROX(acc, force / mass, 1e-12);
+
+ //
+ angvel << 3, 2, 1;
+
+ forces2acc(force, torque, angvel, com, inertia, mass, acc);
+ ASSERT_MATRIX_APPROX(acc, force / mass, 1e-12);
+
+ //
+ angvel << 0, 0, 0;
+ com << 1, 2, 3;
+
+ forces2acc(force, torque, angvel, com, inertia, mass, acc);
+ ASSERT_MATRIX_APPROX(acc, force / mass, 1e-12);
+
+ // must not pass
+ angvel << 3, 2, 1;
+ com << 1, 2, 3;
+
+ forces2acc(force, torque, angvel, com, inertia, mass, acc);
+ // ASSERT_MATRIX_APPROX(acc, force/mass, 1e-12);
+}
+
+TEST(FTI_tools, forces2acc_jacobians_matlab)
+{
+ Vector3d force(1, 2, 3);
+ Vector3d torque(4, 5, 6);
+ Vector3d angvel(3, 2, 1);
+ Vector3d com(6, 5, 4);
+ Vector3d inertia(1, -2, 3);
+ double mass = 2;
+
+ Vector3d acc;
+ Matrix3d J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia;
+ Vector3d J_acc_mass;
+
+ forces2acc(force, torque, angvel, com, inertia, mass,
+ acc,
+ J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com,
+ J_acc_inertia, J_acc_mass);
+
+ // Matlab results using symbolic Jacobians:
+ //
+ // acc =
+ // 50.5000
+ // -65.0000
+ // 22.5000
+ // J_acc_force =
+ // 0.5000 0 0
+ // 0 0.5000 0
+ // 0 0 0.5000
+ // J_acc_torque =
+ // 0 2.0000 1.6667
+ // 4.0000 0 -2.0000
+ // -5.0000 -3.0000 0
+ // J_acc_angvel =
+ // 0 24 12
+ // 6 -60 -38
+ // 12 36 4
+ // J_acc_com =
+ // 5.0000 2.0000 2.5000
+ // -14.0000 10.0000 -8.0000
+ // -8.5000 4.0000 13.0000
+ // J_acc_inertia =
+ // 4.0000 1.0000 -7.3333
+ // 12.0000 20.0000 8.0000
+ // -21.0000 -26.5000 1.0000
+ // J_acc_mass =
+ // -0.2500
+ // -0.5000
+ // -0.7500
+
+ WOLF_INFO("acc: \n", acc);
+ WOLF_INFO("J_acc_force: \n", J_acc_force);
+ WOLF_INFO("J_acc_torque: \n", J_acc_torque);
+ WOLF_INFO("J_acc_angvel: \n", J_acc_angvel);
+ WOLF_INFO("J_acc_com: \n", J_acc_com);
+ WOLF_INFO("J_acc_inertia: \n", J_acc_inertia);
+ WOLF_INFO("J_acc_mass: \n", J_acc_mass);
+}
+
+TEST(FTI_tools, forces2acc_jacobians)
+{
+ Vector3d force, torque, angvel, com, inertia;
+ double mass;
+ Vector3d acc;
+
+ force .setRandom().normalize();
+ torque .setRandom().normalize();
+ angvel .setRandom().normalize();
+ com .setRandom().normalize();
+ inertia .setRandom().normalize(); inertia *= 100;
+
+ inertia = (inertia.array() * inertia.array()).matrix(); // make positive
+ mass = 2.0;
+
+ MatrixXd J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia, J_acc_mass;
+
+ // analytical jacs
+ forces2acc(force, torque, angvel, com, inertia, mass, acc, J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com,
+ J_acc_inertia, J_acc_mass);
+
+ // linear approximations
+ Vector3d pert, acc_pert, acc_pert_approx;
+ double dx = 1e-5;
+ pert = dx * Vector3d::Random().normalized();
+
+ // force
+ forces2acc(force + pert, torque, angvel, com, inertia, mass, acc_pert);
+ acc_pert_approx = acc + J_acc_force * pert;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+
+ // torque
+ forces2acc(force, torque + pert, angvel, com, inertia, mass, acc_pert);
+ acc_pert_approx = acc + J_acc_torque * pert;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+
+ // angvel
+ forces2acc(force, torque, angvel + pert, com, inertia, mass, acc_pert);
+ acc_pert_approx = acc + J_acc_angvel * pert;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+
+ // com
+ forces2acc(force, torque, angvel, com + pert, inertia, mass, acc_pert);
+ acc_pert_approx = acc + J_acc_com * pert;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+
+ // inertia
+ forces2acc(force, torque, angvel, com, inertia + pert, mass, acc_pert);
+ acc_pert_approx = acc + J_acc_inertia * pert;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+
+ // mass
+ forces2acc(force, torque, angvel, com, inertia, mass + dx, acc_pert);
+ acc_pert_approx = acc + J_acc_mass * dx;
+
+ ASSERT_MATRIX_APPROX(acc_pert, acc_pert_approx, 1e-8);
+}
+
+TEST(FTI_tools, tangent2delta_jacobians)
+{
+ VectorXd tangent(12), delta(19), model(7);
+ MatrixXd J_delta_tangent(18, 12), J_delta_model(18, 7);
+ double dt = 1;
+
+ tangent.setRandom();
+ model.setRandom();
+ model.tail(4) *= 10; // augment inertia and mass
+
+ tangent2delta(tangent, model, dt, delta, J_delta_tangent, J_delta_model);
+
+ // linear approximations
+ VectorXd pert, delta_pert(19), delta_pert_approx(19), tangent_pert(12), model_pert(7);
+ double dx = 1e-6;
+
+ // tangent
+ pert = dx * tangent.Random(12);
+ tangent_pert = tangent + pert;
+ tangent2delta(tangent_pert, model, dt, delta_pert);
+ VectorXd step = J_delta_tangent * pert;
+ delta_pert_approx = plus(delta, step);
+
+ ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-9);
+
+ // model
+ pert = dx * model.Random(7);
+ model_pert = model + pert;
+ tangent2delta(tangent, model_pert, dt, delta_pert);
+ step = J_delta_model * pert;
+ delta_pert_approx = plus(delta, step);
+
+ ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-9);
+}
+
+TEST(FTI_tools, data2delta_hovering)
+{
+ VectorXd data(10), bias(12), model(7), delta(19);
+ double dt = 1;
+
+ std::list<Propeller> propellers;
+ propellers.emplace_back(Vector3d(1, 1, 0), Matrix3d::Identity(), 1, 1, 1); // FL
+ propellers.emplace_back(Vector3d(-1, 1, 0), Matrix3d::Identity(), -1, 1, 1); // RL
+ propellers.emplace_back(Vector3d(-1, -1, 0), Matrix3d::Identity(), 1, 1, 1); // RR
+ propellers.emplace_back(Vector3d(1, -1, 0), Matrix3d::Identity(), -1, 1, 1); // FR
+
+ Vector4d n(2, 2, 2, 2); // FL, RL, RR, FR
+
+ data << 0, 0, 0, 0, 0, 0, n; // acc, gyro, propeller speeds
+ bias.setZero();
+ model << 0, 0, 0, 1, 1, 1, 1; // com, inertia, mass
+
+ data2delta(data, bias, model, propellers, dt, delta);
+
+ VectorXd delta_exp(19);
+ delta_exp << 0, 0, 0, 0, 0, 0, 0, 0, 4 * 0.5 * 2 * 2, 0, 0, 4 * 2 * 2, 0, 0, 0, 0, 0, 0, 1;
+
+ ASSERT_MATRIX_APPROX(delta, delta_exp, 1e-12);
+}
+
+TEST(FTI_tools, data2delta_jacobians)
+{
+ VectorXd data(10), bias(12), model(7), delta(19);
+ double dt = 1;
+ MatrixXd J_delta_data(18, 10), J_delta_bias(18, 12), J_delta_model(18, 7);
+
+ std::list<Propeller> propellers;
+ propellers.emplace_back(Vector3d(1, 1, 0), Matrix3d::Identity(), 1, 1, 1); // FL
+ propellers.emplace_back(Vector3d(-1, 1, 0), Matrix3d::Identity(), -1, 1, 1); // RL
+ propellers.emplace_back(Vector3d(-1, -1, 0), Matrix3d::Identity(), 1, 1, 1); // RR
+ propellers.emplace_back(Vector3d(1, -1, 0), Matrix3d::Identity(), -1, 1, 1); // FR
+
+ VectorXd pert, step, data_pert(10), bias_pert(12), model_pert(7), delta_pert(19), delta_pert_approx(19);
+ double dx = 1e-6;
+
+ data.setRandom(); // acc, gyro, propeller speeds
+ bias.setRandom();
+ model.setRandom(); // com, inertia, mass
+
+ data2delta(data, bias, model, propellers, dt, delta, J_delta_data, J_delta_bias, J_delta_model);
+
+ // data
+ pert = dx * data.Random(10);
+ data_pert = data + pert;
+ data2delta(data_pert, bias, model, propellers, dt, delta_pert);
+ step = J_delta_data * pert;
+ delta_pert_approx = plus(delta, step);
+
+ ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-8);
+
+ // bias
+ pert = dx * bias.Random(12);
+ bias_pert = bias + pert;
+ data2delta(data, bias_pert, model, propellers, dt, delta_pert);
+ step = J_delta_bias * pert;
+ delta_pert_approx = plus(delta, step);
+
+ ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-8);
+
+ // model
+ pert = dx * model.Random(7);
+ model_pert = model + pert;
+ data2delta(data, bias, model_pert, propellers, dt, delta_pert);
+ step = J_delta_model * pert;
+ delta_pert_approx = plus(delta, step);
+
+ ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-8);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "FTI_tools.identity*";
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque.cpp
similarity index 96%
rename from test/gtest_processor_force_torque_preint.cpp
rename to test/gtest_processor_force_torque.cpp
index 3465ea6187358cffe667af6a2dd326d279e31e47..b00181b9bd356dbabce6e2d45c0d36a8626ac497 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
/**
- * \file gtest_factor_force_torque_preint.cpp
+ * \file gtest_factor_force_torque.cpp
*
* Created on: March 20, 2020
* \author: Mederic Fourmy
@@ -57,9 +57,9 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque.h"
#include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque.h"
#include <Eigen/Eigenvalues>
@@ -81,7 +81,7 @@ const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset
const Vector3d PBCZ = {0, 0, -0.1}; // Z axis offset
-class ProcessorForceTorquePreintImuOdom3dIkin2KF : public testing::Test
+class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test
{
public:
wolf::TimeStamp t0_;
@@ -92,7 +92,7 @@ public:
SensorForceTorquePtr sen_ft_;
ProcessorImuPtr proc_imu_;
ProcessorInertialKinematicsPtr proc_inkin_;
- ProcessorForceTorquePreintPtr proc_ftpreint_;
+ ProcessorForceTorquePtr proc_ft_;
FrameBasePtr KF1_;
void SetUp() override
@@ -138,7 +138,7 @@ public:
SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
// SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
- ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+ ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>();
params_sen_ft->sensor_ikin_name = "SenIK";
params_sen_ft->sensor_angvel_name = "SenImu";
params_sen_ft->nbc = 2;
@@ -150,16 +150,16 @@ public:
params_sen_ft->dist_traveled = 20000.0;
params_sen_ft->angle_turned = 1000;
params_sen_ft->voting_active = true;
- ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
- proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+ ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", sen_ft_, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ proc_ft_ = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
}
void TearDown() override {}
};
-class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF
+class Cst2KFZeroMotion : public ProcessorForceTorqueImuOdom3dIkin2KF
{
public:
FrameBasePtr KF2_;
@@ -212,7 +212,7 @@ public:
void SetUp() override
{
- ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
+ ProcessorForceTorqueImuOdom3dIkin2KF::SetUp();
t0_.set(0.0);
TimeStamp t1; t1.set(1*DT);
TimeStamp t2; t2.set(2*DT);
@@ -246,14 +246,14 @@ public:
proc_imu_->setOrigin(KF1_);
auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin0->process();
- proc_ftpreint_->setOrigin(KF1_);
+ proc_ft_->setOrigin(KF1_);
// T1
CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu1->process();
auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin1->process();
- auto CFTpreint1 = std::make_shared<CaptureForceTorquePreint>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
+ auto CFTpreint1 = std::make_shared<CaptureForceTorque>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
CFTpreint1->process();
@@ -265,7 +265,7 @@ public:
CImu2->process();
auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin2->process();
- auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
+ auto CFTpreint2 = std::make_shared<CaptureForceTorque>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
CFTpreint2->process();
changeForData3();
@@ -275,7 +275,7 @@ public:
CImu3->process();
CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin3->process();
- auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+ auto CFTpreint3 = std::make_shared<CaptureForceTorque>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint3->process();
// T4, just here to make sure the KF at t3 is created
@@ -283,7 +283,7 @@ public:
CImu4->process();
CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin4->process();
- auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+ auto CFTpreint4 = std::make_shared<CaptureForceTorque>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint4->process();
/////////////////////////////////////////////
diff --git a/test/gtest_processor_force_torque_inertial.cpp b/test/gtest_processor_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8be3e23b05e5c64f1a63ad0e36c9c3559f81e2c
--- /dev/null
+++ b/test/gtest_processor_force_torque_inertial.cpp
@@ -0,0 +1,79 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * gtest_processor_force_torque_inertial.cpp
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+using namespace wolf;
+
+/*
+ // You may use this to make some methods of Foo public
+ WOLF_PTR_TYPEDEFS(FooPublic);
+ class FooPublic : public Foo
+ {
+ // You may use this to make some methods of Foo public
+ }
+
+ class TestInit : public testing::Test
+ {
+ public:
+ // You may use this to initialize stuff
+ // Combine it with TEST_F(FooTest, testName) { }
+ void SetUp() override
+ {
+ // Init all you want here
+ // e.g. FooPublic foo;
+ }
+ void TearDown() override {} // you can delete this if unused
+ };
+
+ TEST_F(TestInit, testName)
+ {
+ // Use class TestInit
+ }
+ */
+
+TEST(TestGroup, testName)
+{
+ // individual tests
+}
+
+int main (int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_force_torque_inertial_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25df6c6d1d00683e5a77bc16f1a90511f41d991a
--- /dev/null
+++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,265 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_ProcessorForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+
+ protected:
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_processor_test.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+ }
+};
+
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, force_registraion)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+// Test to see if the processor works (yaml files). Here the dron is not moving
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
+{
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = - gravity();
+ data.segment<3>(6) = - 1.952*gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ 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);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+
+ //We check that, effectively, the dron has not moved
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), C_KF0->getFrame()->getStateBlock('P')->getState(), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), C_KF0->getFrame()->getStateBlock('V')->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
+
+
+ //Checking that the captures we have taken are the correct ones by looking both time stands
+ ASSERT_EQ(C_KF0->getTimeStamp(), C0_0->getTimeStamp());
+ ASSERT_EQ(C_KF1->getTimeStamp(), C5_1->getTimeStamp());
+
+
+ Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q0 = Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
+ Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q1 = Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d dpi;
+ Vector3d dvi;
+ Vector3d dpd;
+ Vector3d dvd;
+ Vector3d dL;
+ Quaterniond dq;
+ double dt = 1.0;
+ VectorXd betw_states(19);
+
+ betweenStates(p0,v0,L0,q0,p1,v1,L1,q1,dt,dpi,dvi,dpd,dvd,dL,dq);
+
+ betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
+
+ VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+
+ //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
+ ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
+
+}
+
+//Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zero)
+{
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data(0) = 2;
+ data.segment<3>(6) = - 1.952*gravity();
+ data(6) = 1.952*2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // Set CoM to zero so that an acceleration on X does not produce a torque, thereby producing non-null 'L' at KF1
+ S->getStateBlock('C')->setState(Vector3d::Zero());
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ P->print(4,1,1,1);
+
+ C_KF1->getBuffer().print(1,1,1,0,0);
+
+ //We check that, effectively, the drone has moved 1m in the x direction, the x component of the
+ //velocity is now 2m/s and nothing else has changed from the original state
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), Vector3d(1,0,0), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), Vector3d(2,0,0), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
+
+
+
+ Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q0 = Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
+ Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q1 = Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d dpi;
+ Vector3d dvi;
+ Vector3d dpd;
+ Vector3d dvd;
+ Vector3d dL;
+ Quaterniond dq;
+ double dt = 1.0;
+ VectorXd betw_states(19);
+
+ betweenStates(p0,v0,L0,q0,p1,v1,L1,q1,dt,dpi,dvi,dpd,dvd,dL,dq);
+
+ betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
+
+ VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+
+ //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
+ ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
+
+}
+
+//Test to see if the voteForKeyFrame works (distance traveled)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_dist)
+{
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data(0) = 2;
+ data.segment<3>(6) = - 1.952*gravity();
+ data(6) = 1.952*2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ p->setMaxTimeSpan(999);
+ p->setDistTraveled(0.995);
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+
+ P->print(4,1,1,1);
+
+}
+
+//Test to see if the voteForKeyFrame works (buffer)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_buffer)
+{
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data(0) = 2;
+ data.segment<3>(6) = - 1.952*gravity();
+ data(6) = 1.952*2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ p->setMaxTimeSpan(999);
+ p->setMaxBuffLength(3);
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+
+ P->print(4,1,1,1);
+
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialDynamics_yaml.not_moving_test";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc97b1c52db99beb3495c61311a48976c171352e
--- /dev/null
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,313 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#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>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ SolverCeresPtr solver;
+ Vector3d cdm_true;
+ Vector3d inertia_true;
+ double mass_true;
+
+ protected:
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser =
+ ParserYaml("problem_force_torque_inertial_dynamics_solve_test.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+ // CERES WRAPPER
+ solver = std::make_shared<SolverCeres>(P);
+ // solver->getSolverOptions().max_num_iterations = 1e4;
+ // solver->getSolverOptions().function_tolerance = 1e-15;
+ // solver->getSolverOptions().gradient_tolerance = 1e-15;
+ // solver->getSolverOptions().parameter_tolerance = 1e-15;
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+
+ cdm_true = {0, 0, 0.0341};
+ inertia_true = {0.017598, 0.017957, 0.029599};
+ mass_true = 1.952;
+ }
+};
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+// Hover test.
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
+{
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity(); // accelerometer
+ data.segment<3>(6) = -mass_true * gravity(); // force
+ MatrixXd data_cov = MatrixXd::Identity(12, 12) * 1e-6;
+
+ // We fix the parameters of the sensor except for the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+ // S->setStateBlockDynamic('I');
+
+ 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());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+
+ 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->process();
+ p->getOrigin()->getFrame()->fix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ P->print(4, 1, 1, 1);
+
+
+ ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ // S->setStateBlockDynamic('I');
+
+ 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());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ 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 cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
+ {
+ 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();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+ }
+
+
+ ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
+ S->getStateBlock('m')->setState(Vector1d(1.900));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm and the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+ // S->setStateBlockDynamic('I');
+
+ 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());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ 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 mass : ", mass_true, " Kg.");
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+
+
+ // Do a number of iterations with more keyframes, solving so that the calibration gets better and better
+ // Here we take advantage of the sliding window, thereby getting rid progressively
+ // of the old factors, which contained calibration parameters far from the converged values,
+ // 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.
+ 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
+ p->getOrigin()->getFrame()->fix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.cdm_only_hovering";
+ return RUN_ALL_TESTS();
+}
diff --git a/test/matlab/forces2acc.m b/test/matlab/forces2acc.m
new file mode 100644
index 0000000000000000000000000000000000000000..bd40e583279d6c56267964c711f094ec6575c338
--- /dev/null
+++ b/test/matlab/forces2acc.m
@@ -0,0 +1,115 @@
+%%
+function [acc, J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia, J_acc_mass] = forces2acc(force, torque, angvel, com, inertia, mass)
+
+I = diag(inertia);
+
+angacc = inv(I) * (torque - cross(angvel, I * angvel));
+
+acc = force / mass - cross(angacc, com) - cross(angvel, cross(angvel, com));
+
+% Jacobians
+
+fx = force(1);
+fy = force(2);
+fz = force(3);
+tx = torque(1);
+ty = torque(2);
+tz = torque(3);
+wx = angvel(1);
+wy = angvel(2);
+wz = angvel(3);
+cx = com(1);
+cy = com(2);
+cz = com(3);
+ix = inertia(1);
+iy = inertia(2);
+iz = inertia(3);
+
+J_acc_force = [...
+ [1/mass, 0, 0]
+ [ 0, 1/mass, 0]
+ [ 0, 0, 1/mass]];
+
+J_acc_torque = [...
+[ 0, -cz/iy, cy/iz]
+[ cz/ix, 0, -cx/iz]
+[-cy/ix, cx/iy, 0]];
+
+
+J_acc_angvel = [...
+[-((cy*iy*wy + cz*iz*wz)*(iy - ix + iz))/(iy*iz), 2*cx*wy - cy*wx + (cy*(ix*wx - iy*wx))/iz, 2*cx*wz - cz*wx + (cz*(ix*wx - iz*wx))/iy]
+[ 2*cy*wx - cx*wy - (cx*(ix*wy - iy*wy))/iz, -((cx*ix*wx + cz*iz*wz)*(ix - iy + iz))/(ix*iz), 2*cy*wz - cz*wy + (cz*(iy*wy - iz*wy))/ix]
+[ 2*cz*wx - cx*wz - (cx*(ix*wz - iz*wz))/iy, 2*cz*wy - cy*wz - (cy*(iy*wz - iz*wz))/ix, -((cx*ix*wx + cy*iy*wy)*(ix + iy - iz))/(ix*iy)]];
+
+
+J_acc_com = [...
+[ wy^2 + wz^2, (tz + ix*wx*wy - iy*wx*wy)/iz - wx*wy, - wx*wz - (ty - ix*wx*wz + iz*wx*wz)/iy]
+[- wx*wy - (tz + ix*wx*wy - iy*wx*wy)/iz, wx^2 + wz^2, (tx + iy*wy*wz - iz*wy*wz)/ix - wy*wz]
+[ (ty - ix*wx*wz + iz*wx*wz)/iy - wx*wz, - wy*wz - (tx + iy*wy*wz - iz*wy*wz)/ix, wx^2 + wy^2]];
+
+
+J_acc_inertia = [...
+[ (cy*wx*wy)/iz + (cz*wx*wz)/iy, (cz*(ty - ix*wx*wz + iz*wx*wz))/iy^2 - (cy*wx*wy)/iz, - (cy*(tz + ix*wx*wy - iy*wx*wy))/iz^2 - (cz*wx*wz)/iy]
+[- (cz*(tx + iy*wy*wz - iz*wy*wz))/ix^2 - (cx*wx*wy)/iz, (cx*wx*wy)/iz + (cz*wy*wz)/ix, (cx*(tz + ix*wx*wy - iy*wx*wy))/iz^2 - (cz*wy*wz)/ix]
+[ (cy*(tx + iy*wy*wz - iz*wy*wz))/ix^2 - (cx*wx*wz)/iy, - (cx*(ty - ix*wx*wz + iz*wx*wz))/iy^2 - (cy*wy*wz)/ix, (cx*wx*wz)/iy + (cy*wy*wz)/ix]];
+
+
+J_acc_mass = [...
+-fx/mass^2
+-fy/mass^2
+-fz/mass^2];
+
+
+end
+
+%%
+function f()
+%%
+syms fx fy fz tx ty tz wx wy wz cx cy cz ix iy iz mass real;
+
+force = [fx;fy;fz];
+torque = [tx;ty;tz];
+angvel = [wx;wy;wz];
+com = [cx;cy;cz];
+inertia = [ix;iy;iz];
+
+[acc, J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia, J_acc_mass] = forces2acc(force, torque, angvel, com, inertia, mass);
+
+J_acc_force_ = simplify(jacobian(acc, force));
+J_acc_torque_ = simplify(jacobian(acc, torque));
+J_acc_angvel_ = simplify(jacobian(acc, angvel));
+J_acc_com_ = simplify(jacobian(acc, com));
+J_acc_inertia_ = simplify(jacobian(acc, inertia));
+J_acc_mass_ = simplify(jacobian(acc, mass));
+
+simplify(J_acc_force_ - J_acc_force)
+simplify(J_acc_torque_ - J_acc_torque)
+simplify(J_acc_angvel_ - J_acc_angvel)
+simplify(J_acc_com_ - J_acc_com)
+simplify(J_acc_inertia_ - J_acc_inertia)
+simplify(J_acc_mass_ - J_acc_mass)
+
+
+end
+
+function g()
+%%
+force = [1;2;3];
+torque = [4;5;6];
+angvel = [3;2;1];
+com = [6;5;4];
+inertia = [1;-2;3];
+mass = 2;
+
+[acc, J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia, J_acc_mass] = forces2acc(force, torque, angvel, com, inertia, mass);
+
+acc
+J_acc_force
+J_acc_torque
+J_acc_angvel
+J_acc_com
+J_acc_inertia
+J_acc_mass
+
+%%
+end
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial.yaml b/test/yaml/problem_force_torque_inertial.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e2ae117aee579fb6f444cce6f378b2f1c673e739
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial.yaml
@@ -0,0 +1,39 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ follow: "./sensor_force_torque_inertial.yaml"
+
+ processors:
+ -
+ name: "proc FTI"
+ type: "ProcessorForceTorqueInertial"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ follow: "processor_force_torque_inertial.yaml"
+
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics.yaml b/test/yaml/problem_force_torque_inertial_dynamics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..207b69bba5597a63cc41167fbe7a3106ffa4606a
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics.yaml
@@ -0,0 +1,39 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ follow: "./sensor_force_torque_inertial.yaml"
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ follow: "processor_force_torque_inertial_dynamics.yaml"
+
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..233368f38accb87f4bbd4500e37bede35dd1378a
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -0,0 +1,58 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ force_noise_std: 0.1 # std dev of force noise in N
+ torque_noise_std: 0.1 # std dev of torque noise in N/m
+ acc_noise_std: 0.01 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+ acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+
+ 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.952 # mass [kg]
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.0001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.995
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8eb8204126f94bc008bcb01587ae2fa5482fec6b
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -0,0 +1,65 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ plugin: "core"
+ n_frames: 3
+ n_fix_first_frames: 1
+ viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+
+ # IMU
+ force_noise_std: 0.1 # std dev of force noise in N
+ torque_noise_std: 0.1 # std dev of torque noise in N/m
+ acc_noise_std: 0.01 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+ acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+
+ # 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]
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.0001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.995
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/processor_force_torque_inertial.yaml b/test/yaml/processor_force_torque_inertial.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..beafed9ea44a627f2c5b2ea30490bfd11db7868b
--- /dev/null
+++ b/test/yaml/processor_force_torque_inertial.yaml
@@ -0,0 +1,13 @@
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00000001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 1
+ max_buff_length: 5 # motion deltas
+ dist_traveled: 1 # meters
+ angle_turned: 1 # radians (1 rad approx 57 deg, approx 60 deg)
+
diff --git a/test/yaml/processor_force_torque_inertial_dynamics.yaml b/test/yaml/processor_force_torque_inertial_dynamics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a7160e79986f6252a1b8355adad7d8853fecb9c7
--- /dev/null
+++ b/test/yaml/processor_force_torque_inertial_dynamics.yaml
@@ -0,0 +1,13 @@
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00000001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 999
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
diff --git a/test/processor_imu.yaml b/test/yaml/processor_imu.yaml
similarity index 100%
rename from test/processor_imu.yaml
rename to test/yaml/processor_imu.yaml
diff --git a/test/yaml/sensor_force_torque_inertial.yaml b/test/yaml/sensor_force_torque_inertial.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e6d323ba4b29ca53840bd4ae4618ca6348d2d057
--- /dev/null
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -0,0 +1,10 @@
+force_noise_std: 0.1 # std dev of force noise in N
+torque_noise_std: 0.1 # std dev of torque noise in N/m
+acc_noise_std: 0.01 # std dev of acc noise in m/s2
+gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+
+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.952 # mass [kg]
\ No newline at end of file
diff --git a/test/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
similarity index 100%
rename from test/sensor_imu.yaml
rename to test/yaml/sensor_imu.yaml