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/.gitignore b/.gitignore
index 7ecae7d373f3c31b6d5e51edd4c90749667984b3..04169eb5e2228b32f411432a3680e120c71f03e6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,5 +33,3 @@ src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
\.vscode/
-
-wolf.found
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 9ca4dd5c2fccc846c026b27a12f511560f160c58..c2538cb479c738ed6becd5a484ed21ecd3c88e2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,7 +12,7 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(INCLUDE_INSTALL_DIR include/wolf)
set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
@@ -29,7 +29,7 @@ include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ set(CMAKE_CXX_STANDARD 14)
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
@@ -93,65 +93,71 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
"${WOLF_CONFIG_DIR} exists, but is not a directory.")
ENDIF()
+
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-# ============ INCLUDES ============
-INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
SET(HDRS_MATH
-include/bodydynamics/math/force_torque_delta_tools.h
+include/${PROJECT_NAME}/math/force_torque_delta_tools.h
+include/${PROJECT_NAME}/math/force_torque_inertial_delta_tools.h
)
SET(HDRS_CAPTURE
-include/bodydynamics/capture/capture_force_torque_preint.h
-include/bodydynamics/capture/capture_inertial_kinematics.h
-include/bodydynamics/capture/capture_leg_odom.h
-include/bodydynamics/capture/capture_point_feet_nomove.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/bodydynamics/factor/factor_force_torque.h
-include/bodydynamics/factor/factor_force_torque_preint.h
-include/bodydynamics/factor/factor_inertial_kinematics.h
-include/bodydynamics/factor/factor_point_feet_nomove.h
-include/bodydynamics/factor/factor_point_feet_altitude.h
+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_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/bodydynamics/feature/feature_force_torque.h
-include/bodydynamics/feature/feature_force_torque_preint.h
-include/bodydynamics/feature/feature_inertial_kinematics.h
+include/${PROJECT_NAME}/feature/feature_force_torque.h
+include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
)
SET(HDRS_PROCESSOR
-include/bodydynamics/processor/processor_force_torque_preint.h
-include/bodydynamics/processor/processor_inertial_kinematics.h
-include/bodydynamics/processor/processor_point_feet_nomove.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/bodydynamics/sensor/sensor_force_torque.h
-include/bodydynamics/sensor/sensor_inertial_kinematics.h
-include/bodydynamics/sensor/sensor_point_feet_nomove.h
+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
)
@@ -221,28 +227,28 @@ install(
${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
)
-# Specifies include directories to use when compiling the plugin target
-# This way, include_directories does not need to be called in plugins depending on this one
-target_include_directories(${PLUGIN_NAME} INTERFACE
+target_include_directories(${PLUGIN_NAME} PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
#install headers
INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
INSTALL(FILES ${HDRS_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
+DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
export(PACKAGE ${PLUGIN_NAME})
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..bdec1d9bb1fa78d0d15d705335b7c223854b30d4
--- /dev/null
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -0,0 +1,180 @@
+//--------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
+ */
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3, 4> // State Blocks: L, I, i, q
+{
+ public:
+ FactorAngularMomentum(const FeatureBasePtr& _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
+ const T* const _q, // quaternion
+ T* _res) const; // residual
+
+ template <typename D1, typename D2, typename D3, typename D4, typename D5>
+ bool residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
+ const QuaternionBase<D4>& _q,
+ MatrixBase<D5>& _res) const;
+
+ Vector3d residual() const;
+
+ private:
+ Vector3d measurement_ang_vel_; // gyroscope measurement data
+ Matrix3d sqrt_info_upper_;
+};
+
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+ const ProcessorBasePtr& _processor,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3, 4>(
+ "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'), // current linear momentum
+ _ftr->getCapture()->getStateBlock('I'), // IMU bias
+ _processor->getSensor()->getStateBlock('i'), // moment of inertia
+ _ftr->getFrame()->getStateBlock('O')), // quaternion
+ measurement_ang_vel_(_ftr->getMeasurement()),
+ sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper()) // Buscar funcio correcte
+{
+ //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
+ const QuaternionBase<D4>& _q,
+ MatrixBase<D5>& _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)*q.conjugate()*L
+ *
+ * Compute error between them
+ * err = w_m - w_b - i^(-1)*q.conjugate()*L
+ *
+ * Compute residual
+ * res = W * err , with W the sqrt of the info matrix.
+ */
+
+ typedef typename D5::Scalar T;
+
+ Matrix<T, 3, 1> w_real = measurement_ang_vel_ - _I.template segment<3>(3);
+ Matrix<T, 3, 1> L_local;
+ L_local = _q.conjugate()*_L; //we transform que _L from the global frame to the local frame
+ const auto& Lx = L_local(0);
+ const auto& Ly = L_local(1);
+ const auto& Lz = L_local(2);
+ const auto& ixx = _i(0);
+ const auto& iyy = _i(1);
+ const auto& 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 = getCapture()->getStateBlock('I')->getState();
+ Vector3d i = getSensor()->getStateBlock('i')->getState();
+ Quaterniond q;
+ q.coeffs() = getFrame()->getStateBlock('O')->getState();
+
+ residual(L, I, i, q, res);
+ return res;
+}
+
+template <typename T>
+inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, const T* const _q, 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<const Quaternion<T>> q(_q);
+ Map<Matrix<T, 3, 1>> res(_res);
+
+ residual(L, I, i, q, res);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_ANGULAR_MOMENTUM_H
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..e5a87d1bf0c6898ecbb5b5a10b3a5a4b4ad709f1
--- /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_angvel =
+ J_acc_angacc * J_angacc_angvel - angvel_x * J_angvelcom_angvel + skew(_angvel.cross(_com)); // (37)
+ _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
+ _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
+ _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..91a58b87620b66168be69be70ab84586917e605c
--- /dev/null
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -0,0 +1,266 @@
+//--------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_;
+
+ 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..21f42957039fea53471e2cef9eb60ef67d96e7ad
--- /dev/null
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -0,0 +1,156 @@
+//--------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 accb_initial_std;
+ double gyrob_initial_std;
+ double acc_drift_std;
+ double gyro_drift_std;
+ double force_noise_std;
+ double torque_noise_std;
+ Vector3d com;
+ Vector3d inertia;
+ double mass;
+ bool imu_bias_fix, com_fix, inertia_fix, mass_fix;
+
+ 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");
+ accb_initial_std = _server.getParam<double>(prefix + _unique_name + "/accb_initial_std");
+ gyrob_initial_std = _server.getParam<double>(prefix + _unique_name + "/gyrob_initial_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");
+ imu_bias_fix = _server.getParam<bool> (prefix + _unique_name + "/imu_bias_fix");
+ com_fix = _server.getParam<bool> (prefix + _unique_name + "/com_fix");
+ inertia_fix = _server.getParam<bool> (prefix + _unique_name + "/inertia_fix");
+ mass_fix = _server.getParam<bool> (prefix + _unique_name + "/mass_fix");
+ }
+ ~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" //
+ + "accb_initial_std: " + std::to_string(accb_initial_std) + "\n" //
+ + "gyrob_initial_std: " + std::to_string(gyrob_initial_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/capture/capture_inertial_kinematics.cpp b/src/capture/capture_inertial_kinematics.cpp
index 3640f6b966c49cde3ed4fb8f3db94856fbf14bc7..3f3c387b70822ef2989996d0eb5de4e8c1ab50b9 100644
--- a/src/capture/capture_inertial_kinematics.cpp
+++ b/src/capture/capture_inertial_kinematics.cpp
@@ -23,6 +23,8 @@
#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+
namespace wolf {
@@ -37,7 +39,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr,
nullptr,
nullptr,
- std::make_shared<StateBlock>(_bias_pbc, false)),
+ std::make_shared<StateParams3>(_bias_pbc, false)),
data_(_data),
B_I_q_(_B_I_q),
B_Lc_m_(_B_Lc_m)
@@ -56,7 +58,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr,
nullptr,
nullptr,
- std::make_shared<StateBlock>(3, false)),
+ std::make_shared<StateParams3>(Vector3d::Zero(), false)),
data_(_data),
B_I_q_(_B_I_q),
B_Lc_m_(_B_Lc_m)
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..7739f6d27f7447dc207f596b2ca81a9f46739e65
--- /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 ", getName(), " vote: time span");
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_motion_force_torque_->max_buff_length)
+ {
+ WOLF_DEBUG("PM ", getName(), " vote: buffer length");
+ return true;
+ }
+
+ // Some other measure of movement?
+
+ // WOLF_DEBUG( "PM ", getName(), " vote: 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..15c72e2f7a5dda65029222ad07e00b37c144dafe
--- /dev/null
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,522 @@
+//--------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_angular_momentum.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 gyro_cov = motion.data_cov_.block<3, 3>(3, 3);
+
+ auto ftr_base = FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase", measurement_gyro, gyro_cov);
+
+ FactorBase::emplace<FactorAngularMomentum>(ftr_base, ftr_base, shared_from_this(), params_->apply_loss_function);
+
+ // 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
+ params_->apply_loss_function); // 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 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 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.segment<6>(0) = awd - _bias; // awt = awd - awb
+ _tangent.segment<3>(6) = fd; // ft = fd
+ _tangent.segment<3>(9) = td - c.cross(fd); // tt = td - 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(). This function does:
+ *
+ * angacc = I.inv * ( tt - wt x ( I * wt ) )
+ * --> J_aa_i, J_aa_tt, J_aa_wt
+ * acc_dyn = ft / _mass - angacc x com - wt x ( wt x com )
+ * --> J_ad_ft, J_ad_m, J_ad_aa, J_ad_c, J_ad_wt
+ * ==> J_ad_wt, J_ad_ft, J_ad_tt, J_ad_c, J_ad_i, J_ad_m
+ *
+ * delta_p_imu = 0.5 * at * dt^2 --> J_dpi_at
+ * delta_v_imu = at * _dt --> J_dvi_at
+ * delta_p_dyn = 0.5 * acc_dyn * dt^2 --> J_dpd_ad
+ * delta_v_dyn = acc_dyn * _dt --> J_dvd_ad
+ * delta_L = tt * _dt --> J_dL_tt
+ * delta_q = exp_q(wt * _dt) --> J_dq_wt
+ *
+ * Assembling Jacobians gives:
+ *
+ * at wt ft tt
+ * J_delta_tangent = [ J_dpi_at 0 0 0
+ * J_dvi_at 0 0 0
+ * 0 J_dpd_wt J_dpd_ft J_dpd_tt
+ * 0 J_dvd_wt J_dvd_ft J_dvd_tt
+ * 0 0 0 J_dL_tt
+ * 0 J_dq_wt 0 0 ]
+ * with
+ * J_dpd_wt = J_dpd_ad * ( J_ad_wt + J_ad_aa * J_aa_wt )
+ * J_dpd_ft = J_dpd_ad * J_ad_ft
+ * J_dpd_tt = J_dpd_ad * J_ad_aa * J_aa_tt
+ * J_dvd_wt = J_dvd_ad * ( J_ad*wt + J_ad_aa * J_aa_wt )
+ * J_dvd_ft = J_dvd_ad * J_ad_ft
+ * J_dvd_tt = J_dvd_ad * J_ad_aa * J_aa_tt
+ *
+ * c i m
+ * J_delta_model = [ 0 0 0
+ * 0 0 0
+ * J_dpd_c J_dpd_i J_dpd_m
+ * J_dvd_c J_dvd_i J_dvd_m
+ * 0 0 0
+ * 0 0 0 ]
+ * with
+ * J_dpd_c = J_dpd_ad * J_ad_c
+ * J_dpd_i = J_dpd_ad * J_ad_aa * J_aa_i
+ * J_dpd_m = J_dpd_ad * J_ad_m
+ * J_dvd_c = J_dvd_ad * J_ad_c
+ * J_dvd_i = J_dvd_ad * J_ad_aa * J_aa_i
+ * J_dvd_m = J_dvd_ad * J_ad_m
+ *
+ * 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
+ */
+
+ /*
+ * 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);
+
+ // 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 ", getName(), " vote: time span");
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
+ {
+ WOLF_DEBUG("PM ", getName(), " 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 ", getName(), " 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 ", getName(), " 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..0c3a35d31d95e34543339bfe1e706264df19e287
--- /dev/null
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -0,0 +1,90 @@
+//--------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(), _params->imu_bias_fix), // IMU bias
+ 12,
+ false,
+ false,
+ false),
+ params_fti_(_params)
+{
+ addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix)); // centre of mass
+ addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix)); // inertial vector
+ addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix)); // total mass
+ setStateBlockDynamic('I', false);
+ setStateBlockDynamic('C', false);
+ setStateBlockDynamic('i', false);
+ setStateBlockDynamic('m', false);
+
+ VectorXd noise_std_vector(12);
+ Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std;
+ Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std;
+ Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
+ Vector3d torque_noise_std_vector =Vector3d::Ones() * params_fti_->torque_noise_std;
+ noise_std_vector << acc_noise_std_vector, gyro_noise_std_vector, force_noise_std_vector, torque_noise_std_vector;
+ setNoiseStd(noise_std_vector);
+}
+
+// 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/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index 33369c5f00e3d7a65383e28cb0d65d102bcc4de7..5cfaa76228efa3189d41ac757e3be82f6ceb8184 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
namespace wolf {
@@ -30,7 +30,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
SensorBase("SensorInertialKinematics",
nullptr, // no position
nullptr, // no orientation
- std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
+ std::make_shared<StateParams3>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
(Eigen::Vector6d() << _params->std_pbc,_params->std_pbc,_params->std_pbc,
_params->std_vbc,_params->std_vbc,_params->std_vbc).finished(),
false, false, true),
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5850e2db6763d74e6385f16417ed48102091ba5b..c6d84c19a4e3c5bf6140d5ae057524ad32cffd3c 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,7 @@ 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_simulation_problem_force_torque_inertial_dynamics gtest_simulation_problem_force_torque_inertial_dynamics.cpp)
+
+wolf_add_gtest(gtest_solve_problem_force_torque_inertial_dynamics gtest_solve_problem_force_torque_inertial_dynamics.cpp)
+
diff --git a/test/dades_simulacio_pep.csv b/test/dades_simulacio_pep.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e52426eecfc24f699e66baf17a7547171b0dcd50
--- /dev/null
+++ b/test/dades_simulacio_pep.csv
@@ -0,0 +1,381 @@
+timestamp,pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w,vlin_x,vlin_y,vlin_z,vang_x,vang_y,vang_z,f_x,f_y,f_z,m_x,m_y,m_z
+0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,30.24140688670505,0.00013608714916246711,0.0014521640071535336,5.128857392352534e-05
+0.02,1.225134672257865e-08,-1.2259260745627694e-09,0.002128145411468811,1.9224644385208175e-06,1.9212242769870645e-05,4.1019743724623865e-07,0.9999999998135127,-1.4317549759835751e-06,1.4326791822140724e-07,0.10640727057210408,0.00019224644386403226,0.0019212242771064925,4.101974372717375e-05,0.0,0.0,29.887893140652935,5.355186669397938e-05,0.000632055519802166,4.980384573760899e-05
+0.04,3.164793119129389e-07,-3.145199751668115e-08,0.006314985419629783,4.601336319320625e-06,4.678476956232382e-05,1.2166216785179206e-06,0.9999999988942664,1.3954502268081405e-06,-1.4561235248549083e-07,0.20934200098822997,0.00026788676993719067,0.002757252725995719,8.064227015362827e-05,0.0,0.0,29.540464010158914,6.529613184591554e-06,0.0001467468847405895,4.8374456304805384e-05
+0.06,1.2380977715680128e-06,-1.2186983166693075e-07,0.012492280418762047,7.372358116038826e-06,7.629798429331784e-05,2.406581495967047e-06,0.9999999970592371,8.064988856122653e-06,-8.22739440841769e-07,0.30886475336605246,0.00027710020369252434,0.002951321689266104,0.00011899536599298057,0.0,0.0,29.198910712249972,-1.8547764069065664e-05,-0.00012464283122537445,4.6998259156440075e-05
+0.08,3.097877446883576e-06,-3.0210021946895046e-07,0.020592960737205568,9.881332287758513e-06,0.00010416347763031251,3.968358568423615e-06,0.9999999945182907,1.98959164523503e-05,-2.0235505135256084e-06,0.4050340262559032,0.00025089220835405955,0.0027865498928493805,0.00015617630694591205,0.0,0.0,28.863025564019104,-3.0351715194498752e-05,-0.00026189346931571045,4.5673342991900894e-05
+0.1,6.184536975284903e-06,-5.976110773142468e-07,0.0305510856069671,1.196158501223723e-05,0.00012856755356196802,5.890994766694367e-06,0.9999999916463004,3.845464710395817e-05,-3.900771944711037e-06,0.4979062661756617,0.00020801493173149216,0.0024404086842509344,0.00019226117408965902,0.0,0.0,28.53260186078871,-3.437789080273923e-05,-0.0003172687898949089,4.4397966278411e-05
+0.12,1.0741866558453952e-05,-1.0286174094126725e-06,0.042301802349252376,1.3556311555146249e-05,0.00014877933433270332,8.164145814907884e-06,0.9999999888071414,6.491503804013118e-05,-6.559699256681581e-06,0.5875358781141918,0.0001594553377684207,0.0020211798764281767,0.00022731143112637397,0.0,0.0,28.20743377978916,-3.4066289298806396e-05,-0.00032464551987276824,4.3170511002704366e-05
+0.14,1.6964758003386982e-05,-1.6098581052830123e-06,0.05578130575425297,1.4669972065107598e-05,0.00016470242579285052,1.07779808308616e-05,0.9999999862708688,9.986526609854465e-05,-1.004211670665144e-05,0.6739752352525765,0.00011134016457341262,0.0015923118001836536,0.0002613784838798486,0.0,0.0,27.887316305105017,-3.1531907447757135e-05,-0.00030565857607245395,4.198945051493422e-05
+0.16,2.500042579451631e-05,-2.350860516675195e-06,0.07092679764020307,1.533841142411551e-05,0.00017658869745700473,1.3723113150197237e-05,0.9999999841964204,0.00014333164313205587,-1.433221477027244e-05,0.757274688107806,6.680824198505375e-05,0.0011886307843512953,0.0002945068023644371,0.0,0.0,27.572045165552257,-2.804161484570855e-05,-0.0002738395995421916,4.085332740515585e-05
+0.18,3.495241247522585e-05,-3.2564484559282456e-06,0.0876764465807755,1.561096353220603e-05,0.000184859339958222,1.69905522905314e-05,0.9999999826473215,0.00019489192643186188,-1.937047455305273e-05,0.8374825731884221,2.7208863368858957e-05,0.0008270689021236481,0.0003267360381102831,0.0,0.0,27.261416778835994,-2.432504458349527e-05,-0.00023740142256944985,3.9760738377768134e-05
+0.2,4.688585843664052e-05,-4.327344872539917e-06,0.10596934779176233,1.554014637097892e-05,0.00018999631381550708,2.0571670093161243e-05,0.9999999816183553,0.00025381091328892526,-2.5068584840409175e-05,0.9146452211820351,-7.139186698592418e-06,0.0005137031066314508,0.0003581024473668731,0.0,0.0,26.955228197803766,-2.0776635640817886e-05,-0.00020110674489681735,3.871032394162904e-05
+0.22,6.083310972943089e-05,-5.560782674882313e-06,0.12574548316854894,1.5176092622285518e-05,0.00019247944176311945,2.4458176297022176e-05,0.999999981061574,0.0003191658105763353,-3.132235016220051e-05,0.9888069646673194,-3.647410731137493e-05,0.0002483195893832517,0.00038863983868462926,0.0,0.0,26.653277056500798,-1.7586204964503693e-05,-0.00016751672606418744,3.770076138167401e-05
+0.24,7.679912868170565e-05,-6.9510768667761574e-06,0.14694568146580123,1.4563870747744718e-05,0.00019275261391040672,2.8642100326515915e-05,0.9999999809069766,0.0003899482422690305,-3.802159791118977e-05,1.0600101453376,-6.13020498670971e-05,2.7325064132054024e-05,0.00041838019682400354,0.0,0.0,26.355361514911202,-1.4822605152176038e-05,-0.00013781976443549837,3.673075994254958e-05
+0.26,9.47664248860185e-05,-8.490135874145494e-06,0.169511578610433,1.3742514482432002e-05,0.00019120813676605711,3.311577712114471e-05,0.9999999810769683,0.0004651400441809517,-4.5057099434768435e-05,1.1282951207251761,-8.222628020747176e-05,-0.00015443884682139983,0.0004473540879359478,0.0,0.0,26.06128020191197,-1.248665044184527e-05,-0.00011237648127615518,3.579905749707245e-05
+0.28,0.0001146993832187328,-1.0167904779292288e-05,0.19338557813850527,1.2745013010223804e-05,0.0001881820159120577,3.787183555034989e-05,0.9999999814954086,0.0005437650023037907,-5.2324952084938885e-05,1.1937002704183637,-9.98511068502665e-05,-0.0003026022483523487,0.0004755909181486874,0.0,0.0,25.770832156275954,-1.0543995748291124e-05,-9.107345409031797e-05,3.4904418193991524e-05
+0.3,0.00013654796202310516,-1.1972741640004973e-05,0.21851081174630843,1.1598797380307315e-05,0.00018395549439779657,4.290318844542166e-05,0.99999998209258,0.0006249207221359291,-5.972899744876074e-05,1.2562620017647255,-0.00011473224925839966,-0.000422641401451132,0.0005031190949293762,0.0,0.0,25.48381676568331,-8.945026727102956e-06,-7.354870443132544e-05,3.4045630751051904e-05
+0.32,0.00016025078983072265,-1.3891732485642351e-05,0.24483109994552332,1.0326442259004211e-05,0.00017875986641732912,4.820302361314376e-05,0.9999999828073715,0.0007077952482184457,-6.718182979192059e-05,1.3160147550539718,-0.0001273552949626109,-0.0005195511959160284,0.0005299661248519798,0.0,0.0,25.20003370373022,-7.636417926670802e-06,-5.933180811457106e-05,3.32215071621932e-05
+0.34,0.00018573771765226005,-1.5910952505941744e-05,0.2722909128120399,8.946419825077345e-06,0.00017278271549180994,5.376479542160372e-05,0.9999999835877207,0.000791672693879034,-7.460486386955082e-05,1.3729910081731154,-0.0001381304756607396,-0.0005977027007750253,0.0005561586705364821,0.0,0.0,24.919282864910798,-6.567422575320947e-06,-4.7927735263297855e-05,3.2430881669300074e-05
+0.36,0.00021293189584760944,-1.801568139927669e-05,0.30083533081772834,7.4738170352390416e-06,0.00016617447059009947,5.9582216700792056e-05,0.9999999843900733,0.0008759314520642052,-8.192782837096444e-05,1.4272212807251576,-0.00014739632136483074,-0.0006608113712693733,0.0005817225820474456,0.0,0.0,24.64136429751868,-5.69289873331158e-06,-3.886411208742224e-05,3.167260989641808e-05
+0.38,0.00024175144651356973,-2.0190580482276536e-05,0.33041000573419105,5.920973576027082e-06,0.0001590546571491374,6.564925080213585e-05,0.9999999851783669,0.0009600377908456243,-8.908795302508147e-05,1.4787341376012537,-0.00015542758413341926,-0.0007119675595164921,0.0006066829129003706,0.0,0.0,24.366078134381194,-4.974373055688375e-06,-3.171515259203783e-05,3.0945568083510296e-05
+0.4,0.00027211079856612533,-2.2419838396331563e-05,0.36096112159726595,4.298025521235641e-06,0.0001515175281673838,7.196010372789714e-05,0.9999999859228544,0.0010435369191785571,-9.602903355600483e-05,1.527556191995093,-0.00016244466125909436,-0.0007536985076862657,0.0006310639273148624,0.0,0.0,24.093224521305874,-4.37997698465864e-06,-2.6111103254233825e-05,3.0248652376453937e-05
+0.42,0.00030392174605596784,-2.46872913318105e-05,0.3924353557207975,2.6133556236762282e-06,0.00014363695178116296,7.850921628435798e-05,0.9999999865989496,0.0011260429953360142,-0.00010270049204560422,1.5737121078472105,-0.00016862292692206813,-0.0007880427000593195,0.0006548891029633633,0.0,0.0,23.82260354308645,-3.883782088376364e-06,-2.1739039836821128e-05,2.95807781491162e-05
+0.44,0.00033709428250247906,-2.697652277570891e-05,0.42477983974792916,8.739596396764638e-07,0.00013547054576643254,8.52912562395986e-05,0.9999999871861844,0.001207229057748464,-0.00010905650013402395,1.6172246017060508,-0.00017410112519403655,-0.0008166251707300359,0.000678181131856761,0.0,0.0,23.55401514688409,-3.4648590918617828e-06,-1.833881567026019e-05,2.8940879342710454e-05
+0.46,0.00037153725622228717,-2.927094695306558e-05,0.45794212072790047,-9.142580286413045e-07,0.00012706311323446766,9.230111048273954e-05,0.9999999876673171,0.001286817480569591,-0.00011505519663431869,1.6581144439918098,-0.00017898843551235287,-0.0008407273843924232,0.0007009619209358499,0.0,0.0,23.28725906276722,-3.1062536953729847e-06,-1.5696584158342297e-05,2.8327907813671338e-05
+0.48,0.0004071588843600551,-3.1553879414390404e-05,0.4918701222060494,-2.746244927451395e-06,0.00011844946562191448,9.953387719043742e-05,0.9999999880275947,0.001364571279465591,-0.00012065800684957773,1.696400459648282,-0.0001833700943022203,-0.0008613485070478094,0.0007232525932345714,0.0,0.0,23.022134721159553,-2.7939881004357403e-06,-1.3637397384913541e-05,2.774083268700711e-05
+0.5,0.0004438671568986315,-3.380859763213172e-05,0.5265121053144176,-4.617603312147996e-06,0.00010965673109070238,0.00010698485801072983,0.9999999882541595,0.001440286399052448,-0.00012582905553707214,1.7320995281671026,-0.00018731161013678182,-0.0008792568651640862,0.0007450734900301386,0.0,0.0,22.758441166909613,-2.516144231856643e-06,-1.201777398018855e-05,2.7178639715342917e-05
+0.52,0.0004815701564426534,-3.6018394005446324e-05,0.5618166298500358,-6.524422171465399e-06,0.00010070624669678814,0.00011464955027574895,0.9999999883355821,0.001513784984536765,-0.00013053465678020834,1.7652265829678462,-0.0001908616887990171,-0.0008950315689687848,0.0007664441741152649,0.0,0.0,22.49597696965455,-2.2620524688932164e-06,-1.0718733671533087e-05,2.6640330642770012e-05
+0.54,0.0005201763150304116,-3.816662332264446e-05,0.5977325153276241,-8.463127112576458e-06,9.161512638575555e-05,0.00012252363925452665,0.9999999882615008,0.0015849095608419749,-0.0001347428601365797,1.7957946101163627,-0.00019405401818309043,-0.0009090949299985471,0.0007873834341614194,0.0,0.0,22.234540130106808,-2.0215909726228887e-06,-9.639544573558645e-06,2.612492257810989e-05
+0.56,0.0005595946255653072,-4.023674647932752e-05,0.6342088019930675,-1.0430338277520445e-05,8.239758734523268e-05,0.00013060299045586693,0.9999999880223522,0.0016535179997827969,-0.00013842303188087653,1.8238146463624958,-0.00019690807011360425,-0.0009217366257355021,0.0008079092900505667,0.0,0.0,21.973927981841808,-1.7845907109848014e-06,-8.692274912180942e-06,2.5631447378592437e-05
+0.58,0.000599734822569925,-4.22123720808402e-05,0.6711947117836287,-1.2422727964662252e-05,7.306610652389625e-05,0.0001388836419895804,0.9999999876091769,0.0017194791392778374,-0.000141545451874534,1.8492957764768965,-0.00019942906926489473,-0.0009331306833716197,0.0008280389990108131,0.0,0.0,21.713937088108732,-1.5403367776833932e-06,-7.797150893829041e-06,2.515895104800238e-05
+0.6,0.0006405075447391281,-4.4077297453914274e-05,0.7086396092204232,-1.4436870999247184e-05,6.3632468399211e-05,0.00014736179699255714,0.9999999870134931,0.0017826689201577296,-0.00014408090980208148,1.8722451298649925,-0.00020160726608800175,-0.0009433463554822843,0.0008477890623779501,0.0,0.0,21.45436313312252,-1.2771554757096126e-06,-6.8786745212046085e-06,2.4706493152370404e-05
+0.62,0.0006818244900890152,-4.581555053893739e-05,0.7464929622182092,-1.646908211867505e-05,5.410875496982907e-05,0.00015603381612440723,0.99999998622723,0.0018429669198775345,-0.00014600028869160502,1.8926678764342992,-0.00020341663579537278,-0.0009523538982479262,0.0008671752328109703,0.0,0.0,21.195000807223682,-9.820774751201355e-07,-5.862434029113572e-06,2.4273146256859216e-05
+0.64,0.0007235985732570138,-4.741143411702785e-05,0.784704302797023,-1.851523572320959e-05,4.450831973999729e-05,0.00016489621013581664,0.9999999852427176,0.0019002531840888656,-0.0001472741284450862,1.9105672216891183,-0.0002048131118278433,-0.0009600261730955736,0.0008862125218045072,0.0,0.0,20.935643685211176,-6.405692328792512e-07,-4.6725349894805035e-06,2.3857995387433176e-05
+0.66,0.0007657440936044365,-4.884957383224674e-05,0.8232231876796332,-2.0570564317383497e-05,3.484677921072753e-05,0.00017394563251140198,0.9999999840527353,0.0019544052835758583,-0.0001478721673405305,1.9259444010242788,-0.00020573245119390835,-0.0009661368970309222,0.0009049152073612103,0.0,0.0,20.676084097061988,-2.363272203487199e-07,-3.2295850930230685e-06,2.3460137520346756e-05
+0.68,0.00080817692211612,-5.01149714871848e-05,0.8619991587581588,-2.2629432864851066e-05,2.5143048028651446e-05,0.00018317887218771836,0.9999999826506182,0.002005295553490065,-0.00014776286501515064,1.9387986731868971,-0.00020608782138622535,-0.0009703562780524738,0.0009232968417082108,0.0,0.0,20.41611299014797,2.4886809246726926e-07,-1.4491755564449704e-06,2.3078681104287097e-05
+0.7000000000000001,0.0008508147146017904,-5.119306510922231e-05,0.9009817034125251,-2.4685087133546914e-05,1.5420437336278718e-05,0.00019259284634617788,0.9999999810304259,0.002052788502806585,-0.0001469129162397186,1.9491273128721744,-0.00020576719426255668,-0.0009722446963724164,0.0009413702589631916,0.0,0.0,20.15551978194459,8.352392587562818e-07,7.591859008115875e-07,2.2712745617763053e-05
+0.72,0.0008935771583092904,-5.2069797295202786e-05,0.9401202146626866,-2.6729374950802616e-05,5.707829857251853e-06,0.00020218459327994823,0.9999999791871755,0.002096738413607229,-0.0001452867708143035,1.9569256024160053,-0.0002046306311694576,-0.0009712450324566849,0.0009591475826746313,0.0,0.0,19.894092202096353,1.5450387185200753e-06,3.492723536835385e-06,2.23614611660418e-05
+0.74,0.0009363862586978747,-5.273169333505811e-05,0.9793639511357344,-2.875244014628547e-05,-3.959060321305788e-06,0.00021195126533338508,0.9999999771171418,0.0021369871816076104,-0.0001428461811272871,1.9621868225436647,-0.00020250754515343576,-0.0009666741990283394,0.0009766402331763623,0.0,0.0,19.631616122559407,2.4025127408622282e-06,6.855645507175545e-06,2.2023968121615554e-05
+0.76,0.0009791666727349104,-5.3165950581615595e-05,1.0186619968281212,-3.0742389856649796e-05,-1.3536340585725084e-05,0.00022189012191221576,0.999999974818223,0.0021733624808538874,-0.0001395498052787567,1.9649022421300921,-0.00019919402991304503,-0.0009577144048695558,0.0009938589347135492,0.0,0.0,19.367875374382386,3.433798598906357e-06,1.0959012031963589e-05,2.1699416811902633e-05
+0.78,0.0010218460946226015,-5.336054046655318e-05,1.0579632206422716,-3.2684936814569306e-05,-2.2970507674419493e-05,0.00023199852256245462,0.9999999722903677,0.002205676366337275,-0.00013535290011075399,1.965061106923386,-0.00019445035045097007,-0.0009434046610062072,0.0010108137223060576,0.0,0.0,19.10265154951177,4.666750477499981e-06,1.592022552687755e-05,2.138696725701853e-05
+0.8,0.0010643556992898497,-5.3304324453961666e-05,1.097216235675808,-3.456301925481214e-05,-3.2196938046545044e-05,0.00024227392011590381,0.9999999695360507,0.002233724457983445,-0.00013020714490609027,1.9626506271791535,-0.00018799869654038915,-0.0009226330318399689,0.0010275139483265133,0.0,0.0,18.835723785820342,6.130689457539695e-06,2.186214369204187e-05,2.1085788962501225e-05
+0.8200000000000001,0.00110663064826069,-5.29871850665428e-05,1.1363693582405012,-3.635640214352373e-05,-4.11383131962036e-05,0.000252713853900019,0.999999966560779,0.0022572858775527327,-0.00012406064277050528,1.9576559641495144,-0.00017952130650774088,-0.0008941301319079633,0.0010439682887754987,0.0,0.0,18.566868533358154,7.856073285572585e-06,2.89118140897604e-05,2.079506076738212e-05
+0.84,0.0011486106616029356,-5.2400172909227246e-05,1.1753705665868723,-3.804126456798197e-05,-4.9702997386950746e-05,0.00026331594300986575,0.9999999633735935,0.0022761241357934277,-0.00011685815261471801,1.9500602153671027,-0.00016865907463323052,-0.0008564643701474082,0.0010601847492395734,0.0,0.0,18.29585929962213,9.874082403826812e-06,3.7198828491680214e-05,2.0513970751809363e-05
+0.86,0.0011902406585421546,-5.153567034587691e-05,1.2141674593091312,-3.9589779301633556e-05,-5.778339031448902e-05,0.0002740778796398478,0.9999999599875218,0.002289989189943598,-0.00010854160996990479,1.939844398661747,-0.00015501076015683312,-0.000808039444918882,0.001076170670520538,0.0,0.0,18.022466371440913,1.2216119986252494e-05,4.685329691617168e-05,2.0241716201427698e-05
+0.88,0.0012314714679759241,-5.038757215276142e-05,1.2527072134038761,-4.0969691765205035e-05,-6.52542822288666e-05,0.00028499742247276624,0.9999999564199151,0.0022986209104575795,-9.905099933325376e-05,1.9269874348462328,-0.00013813291876485934,-0.0007470945920626131,0.0010919327339182971,0.0,0.0,17.746456510892813,1.4913225338308855e-05,5.8003444410648086e-05,1.9977503628474362e-05
+0.9,0.0012722606085263245,-4.895148307493164e-05,1.2909365419547318,-4.214390681563898e-05,-7.197124401809258e-05,0.00029607239012358256,0.9999999526925843,0.0023017542096006654,-8.83256440428264e-05,1.9114661290085264,-0.00011754067797342899,-0.000671708082966203,0.0011074769661466384,0.0,0.0,17.467592622540536,1.799540185531967e-05,7.077283768980314e-05,1.972054884087476e-05
+0.92,0.0013125731359159274,-4.7224931747085886e-05,1.3288016514139456,-4.307009298466488e-05,-7.77690896164731e-05,0.00030730065463488746,0.9999999488316204,0.002299126092177917,-7.63059814900356e-05,1.8932551503523074,-9.270947577901866e-05,-0.0005798044586913464,0.0011228087438456122,0.0,0.0,17.185633389213617,2.149086275560208e-05,8.527725273066444e-05,1.9470077049578047e-05
+0.9400000000000001,0.0013523825533415126,-4.520759990929865e-05,1.3662481984509973,-4.370031492894159e-05,-8.246045279824388e-05,0.000318680135020521,0.9999999448667624,0.00229048488895564,-6.293589145205672e-05,1.8723270105374077,-6.307787699590453e-05,-0.0004691659697845806,0.0011379327976393941,0.0,0.0,16.90033287365644,2.542519959525258e-05,0.00010162119719525631,1.922532299194753e-05
+0.96,0.00139167177815442,-4.290156525194739e-05,1.4032212463376617,-4.3980705918357205e-05,-8.583452490884345e-05,0.0003302087908528987,0.9999999408301413,0.0022756019251244596,-4.8165643120346525e-05,1.8486520404893387,-2.8051574194079355e-05,-0.0003374486708371048,0.0011528532156588086,0.0,0.0,16.61144008367137,2.9820479796827115e-05,0.00011989410108381726,1.8985531042157285e-05
+0.98,0.0014304341555470817,-4.031155556692856e-05,1.4396652208389147,-4.385119316005755e-05,-8.76560043733514e-05,0.00034188461588831516,0.9999999367542016,0.002254285858331002,-3.1955521697119344e-05,1.8221983656761673,1.2991329174000386e-05,-0.00018220359839085954,0.0011675734464123776,0.0,0.0,16.318698499049297,3.469427954361226e-05,0.00014016618274487191,1.874995525769601e-05
+1.0,0.001468674507108085,-3.744521116902669e-05,1.475523865579948,-4.324528963016294e-05,-8.766431297689452e-05,0.0003537056317226157,0.9999999326685673,0.002226399892413293,-1.4280187954796318e-05,1.7929318798967526,6.069366548599252e-05,-9.034475892517283e-07,0.0011820963008337853,0.0,0.0,16.021845559789746,4.005865649203688e-05,0.000162483981261341,1.8517859292026906e-05
+1.02,0.001506410199064022,-3.431335178872146e-05,1.510740196861797,-4.2089966886916746e-05,-8.55731381328223e-05,0.0003656698814669736,0.9999999285956027,0.002191882033266351,4.866186302216029e-06,1.7608162176942668,0.0001157110987402225,0.00020902483046624857,0.0011964239532602425,0.0,0.0,15.720612117152145,4.591906158624148e-05,0.00018686551438923393,1.828851606651494e-05
+1.04,0.0015436722117995088,-3.093024333341369e-05,1.5452564579024297,-4.030562413648687e-05,-8.107036503488823e-05,0.00037777542342974575,0.9999999245443881,0.0021507685017854773,2.5463977482170326e-05,1.7258127256136366,0.0001787024890274656,0.0004501610427606256,0.0012105579409895301,0.0,0.0,15.414721852380449,5.227317854250568e-05,0.0002132949640330506,1.8061207047117212e-05
+1.06,0.0015805061888614685,-2.7313859075896887e-05,1.579014072487553,-3.780616975755779e-05,-7.381846844056831e-05,0.0003900203247850978,0.9999999205028336,0.0021032203554712794,4.7461351404589304e-05,1.687880432676413,0.0002503186314450222,0.0007250457948897646,0.0012244991619171106,0.0,0.0,15.103890673156826,5.910965842026439e-05,0.00024171669211869862,1.7835220906830366e-05
+1.08,0.0016169734401299278,-2.348612896326248e-05,1.6119535980273325,-3.4499232844681466e-05,-6.345544208650764e-05,0.0004024026552017028,0.999999916427653,0.002049553295313277,7.076669328609663e-05,1.646976020673815,0.00033118923316386253,0.001036126732878039,0.0012382478695524273,0.0,0.0,14.78782610594287,6.64066752843473e-05,0.00027202821923844045,1.7609851258719636e-05
+1.1,0.001653151870182683,-1.9473169847869088e-05,1.644014678033785,-3.028652465444309e-05,-4.9596356511907836e-05,0.00041492048039454743,0.9999999122319575,0.0019902705487267456,9.524025551524635e-05,1.6030537952105652,0.000421907895064738,0.001385695709064288,0.0012518036644383974,0.0,0.0,14.466226714867066,7.413015154134062e-05,0.0003040715069428934,1.738439302001736e-05
+1.12,0.0016891367990783976,-1.5305488516725146e-05,1.6751359940618071,-2.5064374160441842e-05,-3.1835656975890705e-05,0.0004275718555487583,0.9999999077702839,0.0019260986238547363,0.00012068507625292176,1.5560656589159119,0.0005230146641069133,0.0017758147814023405,0.001265165480635101,0.0,0.0,14.138781597017049,8.223137033946948e-05,0.0003376213971820752,1.7158136790890644e-05
+1.1400000000000001,0.0017250416387769808,-1.1018148416065836e-05,1.705255217199113,-1.8724469977156014e-05,-9.750348221072232e-06,0.00044035481854513527,0.9999999028209746,0.0018580256272138162,0.00014683724664945182,1.5059610889437332,0.0006349753485708337,0.0022082275401313595,0.001278331565449354,0.0,0.0,13.80517003347499,9.064349858056708e-05,0.0003723692799050826,1.6930360377925884e-05
+1.16,0.0017609983841042724,-6.650889904001482e-06,1.7343089592532526,-1.1154855233176231e-05,1.7095728334155853e-05,0.00045326738289072003,0.9999998970659871,0.0017873417270810202,0.0001733556286872019,1.4526871219146964,0.0007581561602975608,0.002684249879721549,0.0012912994499198814,0.0,0.0,13.46506142074264,9.927628443806569e-05,0.00040789885681447036,1.670031621867823e-05
+1.18,0.0017971578724005456,-2.2481926851401403e-06,1.762232723877023,-2.2412459029954387e-06,4.914622215584608e-05,0.00046630753022079317,0.9999998900684505,0.0017156812285402596,0.00019981114923178423,1.396188350965737,0.0008927912849149188,0.0032046304939722476,0.0013040659065590764,0.0,0.0,13.118115676956961,0.00010800783408559944,0.00044364906755506306,1.646721287969829e-05
+1.2,0.001833689760589928,2.1407322408881417e-06,1.788960858011283,8.131218807916963e-06,8.684474576009318e-05,0.00047947320218536975,0.9999998812486538,0.0016450655951205997,0.0002256758217759048,1.3364069417998015,0.0010389395616226913,0.0037693653638232734,0.001316626889284294,0.0,0.0,12.763984424651412,0.00011667179169583264,0.000478856545860773,1.62301878957993e-05
+1.22,0.0018707821621939675,6.462035288327101e-06,1.8144265042277703,2.007562588286322e-05,0.0001306247818319582,0.0004927622914551494,0.9999998698597214,0.0015779465841785191,0.0002503116830248811,1.2732826779231874,0.0011964243020104487,0.004377441291169292,0.0013289774481265906,0.0,0.0,12.402313416022983,0.00012503722475576406,0.0005124658746931687,1.5988267781081184e-05
+1.24,0.0019086408796754083,1.0657421569920133e-05,1.8385615548557541,3.370007573149198e-05,0.00018089592585745625,0.0005061726314679754,0.9999998549651075,0.0015172484105198784,0.0002729598889154119,1.2067530501367558,0.0013647468382522916,0.00502646950588779,0.0013411116079679064,0.0,0.0,12.032746912951565,0.00013277445813686395,0.0005429815915139469,1.574066721378354e-05
+1.26,0.0019474881573701191,1.4664463389358336e-05,1.8612966092214678,4.9103164316624205e-05,0.00023802466053654363,0.0005197019870884877,0.9999998354214789,0.0014664074321276716,0.00029273030956777246,1.136753412536819,0.0015429556615896146,0.005712138928228991,0.001353022467232675,0.0,0.0,11.654935085781528,0.00013941147585438962,0.0005682745189826144,1.548561289576385e-05
+1.28,0.0019875608806281847,1.8417050222092967e-05,1.8825609359823987,6.636760491775683e-05,0.00030230733977743723,0.0005333480371492095,0.9999998098727242,0.0014294072396541454,0.0003085920845113855,1.0632172376737414,0.0017294598767311575,0.00642743724921852,0.0013647013982765962,0.0,0.0,11.268545938273384,0.00014425774471393815,0.0005852589371075356,1.5219210598408263e-05
+1.3,0.0020291081355949298,2.1845998030147914e-05,1.9022824434773342,8.555102188837693e-05,0.00037393152895423456,0.0005471083403370218,0.9999997767643242,0.001410804837338228,0.0003193661559216246,0.9860765177858607,0.0019217456318929294,0.007161486743645971,0.0013761364004810364,0.0,0.0,10.873283639653724,0.0001462384938636374,0.0005892247126642602,1.4942654591777711e-05
+1.32,0.002072388007795087,2.487985069578418e-05,1.9203876622961615,0.00010667180092793813,0.000452917697180744,0.0005609803385766387,0.9999997343938378,0.001415741883191005,0.00032372112876311417,0.9052623762082537,0.002115880478763752,0.007897579793742147,0.001387315871783246,0.0,0.0,10.468914841980165,0.0001437506023028723,0.0005732024893719823,1.4650799549180987e-05
+1.34,0.0021176635803512992,2.7445882312585917e-05,1.9368017458250475,0.00012968825184094904,0.0005390340026444215,0.0005749613117272527,0.9999996810213448,0.0014499332787079097,0.00032017408560315613,0.8207059664420872,0.0023058427848359295,0.00861048864799949,0.0013982237773624377,0.0,0.0,10.055301888741688,0.00013426943913319445,0.000526359640621421,1.4340426720078128e-05
+1.36,0.0021651980159363184,2.947133283936838e-05,1.9514484958240934,0.00015446617897862987,0.0006316653651406534,0.0005890483456810099,0.9999996150804822,0.001519610763744835,0.00030710201587349134,0.7323397237714716,0.0024823593884057793,0.009261894921148407,0.0014088410341568775,0.0,0.0,9.632433219709114,0.00011370376003089211,0.00043145575262482083,1.4003848675377564e-05
+1.3800000000000001,0.002215248683886902,3.0884893181499103e-05,1.9642504195523571,0.0001807269266487254,0.0007296074635374591,0.0006032382666609615,0.9999995355570525,0.001631396274747139,0.0002827698340219759,0.6400989419775581,0.002630947924088747,0.009792883411318763,0.0014191421648980706,0.0,0.0,9.200422135240945,7.500704300589245e-05,0.0002601256889447623,1.363326179399456e-05
+1.4000000000000001,0.002268060372989986,3.161845892188782e-05,1.9751288191422973,0.00020795880852435535,0.0008307315784578456,0.0006175275797488216,0.999999442648778,0.0017920539225420185,0.00024538725357845426,0.5439233838847661,0.0027282462416125123,0.010111029107925744,0.0014290955257488273,0.0,0.0,8.759416630676654,5.9224872971941345e-06,-3.32982957525485e-05,1.32180045953285e-05
+1.42,0.0023238581072786957,3.160906621504949e-05,1.9840038979909491,0.00023526477275006728,0.0009314521890410814,0.0006319124110505699,0.9999993388667869,0.002008026943918258,0.00019321616091891395,0.4437581279282786,0.002735621846094594,0.010070677011409535,0.00143866255877871,0.0,0.0,8.309340800579037,-0.00011341178930313056,-0.0005170911025723091,1.2749079277874663e-05
+1.44,0.0023828409309284695,3.080062606355726e-05,1.990794839685318,0.00026111948911970034,0.0010259459483275921,0.0006463885196668561,0.9999992307164067,0.0022845607830903956,0.00012478000431080736,0.3395521865473096,0.002590095273873209,0.009448086550713304,0.0014478018790237022,0.0,0.0,7.849355230911976,-0.0003122847767576542,-0.0012865142670025198,1.2225268111288317e-05
+1.46,0.0024451787348663654,2.9144725531188213e-05,1.9954197680886347,0.0002830195104950638,0.0011050902671257028,0.0006609514491073395,0.9999991309089423,0.0026240603154565586,3.9284204395819835e-05,0.23125254935531062,0.0021936247815752753,0.00791340071447829,0.001456478488285123,0.0,0.0,7.376521136508651,-0.0006310843157646406,-0.0024685852540669018,1.166883701943257e-05
+1.48,0.002511014658898052,2.6599542888705284e-05,1.9977953650908702,0.0002970166657636101,0.001155098277566318,0.0006755969367364152,0.999999060548483,0.003023196107316387,-6.25805391149617e-05,0.11878704176514422,0.0014014031305057738,0.005000293827670246,0.0014646836290186681,0.0,0.0,6.8788084060216255,-0.0010997820664007119,-0.004150182446593764,1.1194493756706028e-05
+1.5,0.0025804731708883883,2.3126550075375276e-05,1.9978349656399208,0.0002974216378151039,0.0011567521938394495,0.0006903219158655309,0.9999990484597392,0.003468099049434656,-0.00017721066242340516,0.00198815504816445,3.890838443533018e-05,0.00016580286494318787,0.0014724950005191343,0.0,0.0,18.51836419726961,0.44936437173582827,1.5059908207012014,0.18729616038545208
+1.52,0.002632454171935659,2.6333855787167334e-05,1.9978915257913066,0.006554129811626445,0.01970297929125427,0.0020671325510581004,0.9997822577703992,0.002540103204491672,0.00017277702006931588,0.0028806506197515407,0.6268379216853844,1.8543740638136705,0.13787688761162972,0.0,0.0,18.35563541311824,0.16091224654634234,0.8159975151196931,0.19808217197896544
+1.54,0.0028187739886145263,-1.5946538692406762e-05,1.9978524915164102,0.015149946190354174,0.04822693980054579,0.004944754335420059,0.9987092624044628,0.009411636232906376,-0.0022150896124153202,-0.0012683832223961329,0.8604528666549536,2.8544249649275857,0.2862054540021749,0.0,0.0,19.191367665457445,0.011210885121456404,0.32355979405103674,0.19482003920099808
+1.56,0.003380144406601722,-0.0001739690393038598,1.9978011617876794,0.02385733888117927,0.08074607573453739,0.009358437501834608,0.9964051978399677,0.02803450838551585,-0.008328033539651414,0.0013774651963374455,0.8680676475032548,3.2618387537912286,0.43474862077143833,0.0,0.0,20.274927578463746,-0.056706446510644604,-0.0052345729602146784,0.18679455893791014
+1.58,0.004616516196606919,-0.0005209055197172559,1.9978823393594416,0.031686343137567685,0.11311639950896364,0.015266723153315278,0.992959003677824,0.0593507681870581,-0.018265806430643988,0.016863159924160882,0.7710427819009685,3.260619615407251,0.5780302745717278,0.0,0.0,21.172913353059755,-0.08731583468965642,-0.19474447464449696,0.1791026230297918
+1.6,0.006840618088389445,-0.0011196806970899937,1.9981998146618434,0.03823152504933126,0.1428922284969528,0.022615347300577706,0.9887409709292172,0.10209851266974644,-0.03185210612482418,0.04561499316006298,0.6257124951315041,3.018127029367769,0.7159353064566114,0.0,0.0,21.83048250617594,-0.09642290465736414,-0.2919351071593176,0.17189076231246936
+1.62,0.010344357631584901,-0.002015588069609184,1.9988042245350242,0.04341722116894763,0.16880831704644922,0.03134820317332547,0.984193063966474,0.15425741472530255,-0.048997107848545815,0.0861581129691169,0.46400429358676815,2.649426590407973,0.8488694439158168,0.0,0.0,22.264530642361564,-0.09361185737058841,-0.33098390944337797,0.16498223091164343
+1.6400000000000001,0.015379749754567806,-0.003234121940764242,1.9997033349026891,0.04735209666365171,0.19035513069646604,0.041406862494033705,0.9796980018850134,0.2138275717194289,-0.0698129168436788,0.136156944405886,0.30456139558072237,2.2280628475955715,0.9770290681756331,0.0,0.0,22.510336759248982,-0.0852153306272262,-0.3344644062662644,0.1583077372849761
+1.6600000000000001,0.02215086714597756,-0.004782226984238618,2.0008757465191245,0.05023132364867353,0.2074853708442581,0.052730895821322926,0.9755234941486564,0.27912048138907036,-0.09460074387211115,0.19289565634355121,0.1572283512779582,1.7995439841171805,1.1004729523187637,0.0,0.0,22.60776267159686,-0.07513947028812651,-0.31718305561528937,0.15184414195950355
+1.68,0.030813202311342075,-0.00665133462147483,2.0022831622378066,0.05227389839270899,0.2204172081690369,0.06525923619117661,0.9718152735893486,0.3487830643354794,-0.12378620006100555,0.25362506955209546,0.026132339808656035,1.3907113955104233,1.2192144431004788,0.0,0.0,22.59421564446368,-0.06558044031957777,-0.28872050899653334,0.14557902716658427
+1.7,0.041477270300358554,-0.008820986272270027,2.0038797612190904,0.053686035037046616,0.22950664565101714,0.07893166681800802,0.968614629879887,0.421704232029634,-0.15784738983088725,0.31579050839185946,-0.08808645271562086,1.0161764668940894,1.3332693538492686,0.0,0.0,22.50143261200271,-0.057609109701168504,-0.25514507636413675,0.13950027551501365
+1.72,0.05421413854325978,-0.011262323893322747,2.0056186097318136,0.054642557636573,0.23516787308251325,0.0936898557260294,0.9658842960203963,0.4968965242501052,-0.19725601912162521,0.37715547192904525,-0.18672500937415926,0.6827280032146,1.4426727819948337,0.0,0.0,22.354641785310605,-0.0516020065519216,-0.22019154799743967,0.1335952058409851
+1.74,0.06906143490373187,-0.01394110448977473,2.0074555973494674,0.055279793129061755,0.23782567799453694,0.10947777479603517,0.9635339683600965,0.5733983712278709,-0.24243584538276577,0.4358499445520357,-0.27199972083153745,0.39230148312430146,1.5474809403585401,0.0,0.0,22.173016797065053,-0.04753488570386177,-0.18606468174316937,0.1278515537707896
+1.76,0.08602905511215939,-0.016820133267995675,2.009351534434174,0.05569528686516358,0.23788853331796986,0.12624157498872307,0.9614417015509567,0.6502116549023496,-0.29373538132388777,0.49037204185373157,-0.34641433636558183,0.14394386016183874,1.647767237666237,0.0,0.0,21.970670563979134,-0.04517207115899535,-0.15398009114971511,0.1222580623907702
+1.78,0.10510422870308238,-0.019861135438766796,2.0112729972974708,0.0559512282215487,0.23573463385805732,0.143929076882095,0.9594701992705015,0.7262730871635286,-0.351409947618638,0.5395642637748408,-0.412391039894985,-0.06490856978177012,1.7436169153486096,0.0,0.0,21.757747769971385,-0.0441837958143243,-0.12452520027081537,0.1168045475571143
+1.8,0.12625585150714486,-0.023026139373029483,2.0131923857178897,0.0560796261071442,0.23170582466534592,0.16248904437919826,0.9574783531801488,0.8004516416975213,-0.41560899419098046,0.5825777161064625,-0.4720736686196614,-0.23761468890620352,1.8351220357208426,0.0,0.0,21.541406171091396,-0.04421621961088518,-0.09789847179767674,0.11148170898713657
+1.82,0.14943811523968503,-0.026278457077641496,2.0150875286514895,0.05608805286230672,0.22610619836526186,0.18187038413495193,0.9553288861756086,0.8715633186744672,-0.4863661823163585,0.6188319012382629,-0.5272399198473546,-0.377820710817416,1.9223774397586602,0.0,0.0,21.326615438864245,-0.04493119313749805,-0.07406676187701333,0.10628088844635278
+1.84,0.17459351356559655,-0.02958334325070113,2.016941065040282,0.055965276591769975,0.2192033464077939,0.2020213771131345,0.9528930390809579,0.9383958501659577,-0.5635911384285928,0.6479741832567238,-0.5792802343569964,-0.48915491662940286,2.0054777594772166,0.0,0.0,21.11677154402731,-0.046027046199828114,-0.052867778703824975,0.10119386576526901
+1.86,0.2016553149519231,-0.03290840320669837,2.018739744178724,0.055686415852145565,0.21123102275862907,0.22288901280807388,0.9500531385576955,0.9997379089495261,-0.6470627607514706,0.6698411235773369,-0.6292155880594934,-0.5750543259776292,2.084515349868018,0.0,0.0,20.914152784235938,-0.047247706124085465,-0.03407582990411251,0.09621271703146293
+1.8800000000000001,0.2305495867872036,-0.03622380819404319,2.0204737318059,0.055217443248825424,0.20239246468422575,0.24441846593243266,0.946703722244369,1.0544092058690413,-0.7364244801457386,0.6844228892856822,-0.6777362535001098,-0.6386761023274937,2.1595789387727082,0.0,0.0,20.72025060188015,-0.048384828468953645,-0.01744309037004199,0.09132972979037955
+1.9000000000000001,0.26119684382776925,-0.03950236611031335,2.022135968870354,0.05451898062440699,0.19286391720816526,0.2665527314627648,0.9427517337775289,1.1012892867470387,-0.8311820528175327,0.691831402400733,-0.7252501405519753,-0.6828623225360153,2.23075279751247,0.0,0.0,20.53600609063089,-0.049275919248627176,-0.002724605915965239,0.08653736045869602
+1.92,0.2935133822349063,-0.042719486792036354,2.023721604656059,0.05354939682135803,0.18279809430532315,0.2892324168558906,0.9381161590432772,1.1393438396153468,-0.9307044175740529,0.6922725716847845,-0.7719336548893102,-0.7101373620133702,2.2981162672627446,0.0,0.0,20.361977639089986,-0.04980032575789306,0.010307451081012498,0.08182821839709863
+1.94,0.32741235026845816,-0.04585307373363848,2.025227510452367,0.05226725742014184,0.17232742510398372,0.3123956791722255,0.9327273620910514,1.1676479614529864,-1.034227987269286,0.6860227111943278,-0.8177808584673237,-0.7227242474267358,2.361743514433154,0.0,0.0,20.19845932901024,-0.049874260301392526,0.0218565647680109,0.07719506363583456
+1.96,0.3628045985035336,-0.04888336794716167,2.0266518710299803,0.05063319554674714,0.1615670048510245,0.3359782885898483,0.9265262932310289,1.1854062007857662,-1.1408645366560632,0.6734090574677675,-0.862648571917707,-0.7225710717518894,2.42170342308511,0.0,0.0,20.045564613539,-0.049445560327536886,0.03209919490294566,0.07263080860821092
+1.98,0.3995993459862607,-0.05179276442829603,2.0279938465782577,0.04861128080164744,0.15061721501848466,0.359913797133755,0.9194636787560785,1.1919693739475787,-1.2496126432460275,0.6547941401999132,-0.9062962534856027,-0.7113817403412606,2.478059560460135,0.0,0.0,19.90328575066265,-0.04848859216517876,0.041184935770003483,0.06812851743991019
+2.0,0.43770469360589875,-0.05456561715666478,2.029253295885615,0.04616996404409675,0.13956600485802323,0.38413379126076386,0.9114992567850886,1.186848218632537,-1.3593724587500886,0.6305636412280741,-0.9484202430374584,-0.6906474484319238,2.5308701728316096,0.0,0.0,19.771536409813095,-0.0469995178700382,0.049238361482462634,0.06368139905748146
+2.02,0.47702801172354936,-0.0571880446231592,2.0304305513247103,0.04328267206982489,0.1284908440492464,0.4085682083109475,0.9026010926492336,1.1697239510971427,-1.468963446272083,0.6011172963384981,-0.9886824215278178,-0.6616767272587817,2.5801881842112473,0.0,0.0,19.65018263442922,-0.04499202926681056,0.05636177435937184,0.05928279258355197
+2.04,0.5174762254513904,-0.05964774449233677,2.0315262369120015,0.0399281200514965,0.11746036654034765,0.4331456990613365,0.8927449864916781,1.1404557694689839,-1.5771446137379193,0.5668623535009829,-1.0267336075466356,-0.6256228637926649,2.6260611813371195,0.0,0.0,19.539065741147155,-0.042493585683585966,0.06263833053034984,0.05492614522889809
+2.06,0.5589560177182539,-0.061933823147729244,2.032541121868371,0.0360904022135889,0.10653573154864444,0.45779402120456686,0.8819139725170528,1.0990853183604177,-1.6826367035012657,0.528209095913379,-1.0622321622700728,-0.5835081446806505,2.6685313761290033,0.0,0.0,19.43801960159797,-0.03954215497146307,0.06813520015987296,0.05060498422117005
+2.08,0.6013739672746913,-0.06403664351347196,2.033476003406525,0.031758913376224475,0.09577173046988482,0.48244045120435347,0.8700979014488571,1.0458381125687886,-1.784145756345888,0.4855679624295228,-1.0948583484458312,-0.5362447939129511,2.7076355424300376,0.0,0.0,19.34688395463611,-0.03618343870103202,0.07290655257904932,0.04631288524536536
+2.1,0.6446366360345192,-0.06594769267382578,2.0343316137234146,0.026928146204371915,0.08521766915890622,0.5070122044606657,0.8572930935992983,0.9811219160541154,-1.8803874497676722,0.4393478440172762,-1.1243250171935408,-0.48465273021768923,2.7434049280636166,0.0,0.0,19.26551483749173,-0.032468552763394776,0.07699625346475547,0.04204344040324388
+2.12,0.6886506176110424,-0.06765946939103781,2.0351085472759376,0.02159740151741513,0.07491805441742172,0.531436855946959,0.8435020483147714,0.9055220878212734,-1.9701116107071566,0.38995519509834553,-1.1503851950258135,-0.42947440951385785,2.775865146444159,0.0,0.0,19.193792833379874,-0.028452127547503148,0.08044023019532154,0.037790228810578294
+2.14,0.7333225565961099,-0.06916539061060538,2.035807205315655,0.015770442016323546,0.06491311208907383,0.5556427554094134,0.8287331955451983,0.8197939371300909,-2.0521263179729208,0.33779366576423386,-1.172837120160253,-0.37138707449245756,2.8050360543894803,0.0,0.0,19.13162956106831,-0.024190783542597405,0.08326850714767675,0.03354679264083402
+2.16,0.7785591460672869,-0.07045971539307273,2.0364277553398624,0.009455113398653969,0.05523916230712617,0.5795594328235483,0.8130006762626171,0.7248521734931217,-2.1253210364859165,0.28326402880596124,-1.1915272336754368,-0.31101273614087294,2.830931624432916,0.0,0.0,19.078972645305395,-0.019741927269432247,0.08550693754118543,0.029306620725614692
+2.18,0.8242671090095999,-0.0715374843699232,2.036970103591316,0.002662951191561074,0.045928875443886026,0.6031179910854884,0.796324139983472,0.62175758857156,-2.188688261985791,0.22676423946696309,-1.2063515666141755,-0.24892618594647772,2.853559820776741,0.0,0.0,19.03580928443065,-0.015162799938915894,0.08717866433529964,0.025063140815918525
+2.2,0.8703531678142447,-0.07239447273265345,2.037433879028826,-0.004591213090961171,0.03701143035491061,0.6262514838903065,0.778728549438523,0.5117011637586697,-2.2413431994313067,0.1686895225808091,-1.2172558786301548,-0.18566130530854552,2.872922487988015,0.0,0.0,19.002168460443926,-0.010509700356831742,0.08830533178744482,0.02080972041734446
+2.22,0.9167240047703781,-0.07302715487359983,2.0378184273237943,-0.012289636468446377,0.028512594687542036,0.6488952774583459,0.760243984304982,0.3959858556142712,-2.282541050043142,0.10943242957742177,-1.2242348005309496,-0.12171592032350374,2.8890152605607065,0.0,0.0,18.978121810586327,-0.005837299033936927,0.08890804481387116,0.016539674887812335
+2.24,0.9632862154933962,-0.07343267905163599,2.038122813445268,-0.020412031982020445,0.020454745334950925,0.6709873952827357,0.7409054377687729,0.2760063682721834,-2.3116915403325584,0.04938284640364831,-1.2273301209377694,-0.05755546159854681,2.9018274995611977,0.0,0.0,18.963783193744366,-0.0011979674174460442,0.08900804030904019,0.01224628037898412
+2.2600000000000002,1.0099462565241257,-0.07360885080669652,2.0383458313138796,-0.028935912246382374,0.012856845475002146,0.6924688454920477,0.7207526014728023,0.15322727442134051,-2.3283703914470886,-0.011072036585020727,-1.2266282514510687,0.006384259917621288,2.911342259822887,0.0,0.0,18.959307034194502,0.0033589311097426133,0.08862700037193222,0.007922788329691233
+2.2800000000000002,1.0566103878749202,-0.07355412425127836,2.0384860188746434,-0.03783690142225711,0.005734392864123649,0.7132839308861469,0.6998296361035653,0.02915989124575047,-2.33232749873689,-0.0715477590049301,-1.222256825711033,0.06969475758739598,2.9175362877168793,0.0,0.0,18.964885599423166,0.0077878017537799105,0.08778691151007711,0.0035624376374518564
+2.3000000000000003,1.1031846110763892,-0.07326760078932705,2.0385416768089293,-0.04708900577347477,-0.0009006480734884934,0.7333805423619453,0.6781849264420526,-0.09466164865857256,-2.3234916692477423,-0.13166257877476653,-1.2143803581755919,0.1319925270224591,2.920380045606629,0.0,0.0,18.980745451754768,0.012047589169754347,0.0865093672915111,-0.0008415396677877284
+2.32,1.1495746032788137,-0.07274903522736513,2.0385108890338275,-0.056664829624198144,-0.007039920549868956,0.7527104374413166,0.6558708210991048,-0.2167046724481222,-2.301971847659963,-0.19103735308948772,-1.2031949274164044,0.19291599826646239,2.9198377549251133,0.0,0.0,19.007143383115928,0.016102457156686234,0.08481423492429863,-0.005295923491113993
+2.34,1.1956856481739717,-0.0719988496349004,2.038391543184878,-0.06653571954827828,-0.012678749783763966,0.7712295070455307,0.6329433582797288,-0.33546212832164457,-2.2680548464672348,-0.24929524578309886,-1.1889219647179292,0.2521210186899172,2.9158674456387756,0.0,0.0,19.04436217394774,0.019922223729990973,0.08271766530212832,-0.009807525571318108
+2.36,1.2414225649184374,-0.07101815567967254,2.0381813495088594,-0.07667181573065089,-0.017816215321456202,0.7888980355490557,0.6094619796599502,-0.4494761919330351,-2.222199679828662,-0.3060611974706926,-1.1718014124454625,0.3092740693869881,2.9084209959747374,0.0,0.0,19.092706490814983,0.02348237232515471,0.08022951959122576,-0.014383227828291963
+2.38,1.286689636855042,-0.06980878652051192,2.0378778570650717,-0.08704198649419173,-0.022455223816127173,0.8056809613989586,0.5854892347001456,-0.5573609157917748,-2.1650286771293366,-0.3609611074965148,-1.1520847362367506,0.3640424969477742,2.897444143097268,0.0,0.0,19.15249914409517,0.026763477790921142,0.07735040010858701,-0.019030041739526743
+2.4,1.331390542646094,-0.0683733397108963,2.0374784668927557,-0.09761362081723256,-0.026602589171738817,0.8215481479872931,0.5610904773910871,-0.6578235752185962,-2.0973156159729425,-0.41362070198808965,-1.1300284764737687,0.416081260623968,2.882876443533087,0.0,0.0,19.224077773921024,0.029749959526980008,0.07406858252655485,-0.023755189400776595
+2.42,1.3754282934547124,-0.06671523298087743,2.0369804428778946,-0.1083522548647662,-0.03026913019405473,0.8364746766856249,0.5363335565547763,-0.7496844304286061,-2.019971160136488,-0.4636641022735429,-1.105889137834062,0.4650160467098874,2.8646511623688315,0.0,0.0,19.30779183631702,0.03242820537587976,0.07035721371051867,-0.028566203908037213
+2.44,1.418705181046171,-0.06483877525266517,2.0363809224488802,-0.11922101218968466,-0.03346979685377036,0.8504411756105975,0.5112884995548983,-0.8318946910458271,-1.9340259135651021,-0.5107121630450934,-1.0799201679170152,0.5104230378486737,2.8426950734247685,0.0,0.0,19.403999537077862,0.034784286969907985,0.06617212673268302,-0.033471042753333055
+2.46,1.4611227431386187,-0.06274925582018379,2.0356769309193212,-0.13017984262467613,-0.03622383382627942,0.8634341984422005,0.48602718792019795,-0.9035525257305348,-1.8406114085232688,-0.5543807179651554,-1.0523715071508861,0.5518060492195194,2.8169281592925777,0.0,0.0,19.51306411034494,0.03680168420182195,0.06145050526948581,-0.0384782037189768
+2.48,1.5025817540272544,-0.060453055273018315,2.0348654052118897,-0.14118455037494523,-0.03855498314714198,0.875446667269548,0.4606230224126769,-0.9639169955829312,-1.740939341268325,-0.5942789468687285,-1.0234916734863286,0.5885720280736993,2.7872632101525876,0.0,0.0,19.635348554536463,0.03845961441521828,0.05611040149066865,-0.04359682928055733
+2.5,1.5429822504694695,-0.05795778242017516,2.033943234742308,-0.15218560505258524,-0.04049171591712979,0.8864783920519741,0.43515057506777044,-1.0124197996011826,-1.6362793634150659,-0.6300081552889407,-0.9935315956372213,0.6200059269582894,2.753605332575546,0.0,0.0,19.771207618671774,0.0397326718891049,0.050050804968409746,-0.048836783732026406
+2.52,1.5822236051149081,-0.055272442070195014,2.032907329315154,-0.1631267276519154,-0.04206746575625372,0.896536677200239,0.40968522732114543,-1.0486747044040283,-1.5279357455991855,-0.6611613239549987,-0.9627485089651028,0.6452456099313841,2.7158513922144594,0.0,0.0,19.920975478653006,0.04059247288397316,0.04315164009322303,-0.05420868787212786
+2.54,1.6202046625117603,-0.05240763889095527,2.0317547259198325,-0.1739432344763822,-0.04332081455782869,0.9056370235372369,0.38430279702931486,-1.0724844971116323,-1.417223265458484,-0.6873238345367491,-0.9314073293372066,0.6632567196459073,2.673889425118076,0.0,0.0,20.08494710258195,0.041011830164380036,0.03527283639490586,-0.05972390034652485
+2.56,1.6568239561144287,-0.04937582246761754,2.030482748329158,-0.1845601049734384,-0.04429555532605722,0.9138039319245774,0.35907916317955824,-1.0838452645842729,-1.3054427421878354,-0.7080758051444799,-0.8997762315113679,0.6728064279987355,2.6275980585043515,0.0,0.0,20.263350585570322,0.04097164747019244,0.02625160811044963,-0.06539444545996057
+2.58,1.6919800290593854,-0.0461915777747044,2.0290892354794248,-0.1948897161561863,-0.045040529597844145,0.9210718129297238,0.3340899050502105,-1.0829477812249588,-1.1938567423918194,-0.722996482128791,-0.8681128983186813,0.6724339905028098,2.5768459787927047,0.0,0.0,20.456306484631877,0.04047030452643963,0.01589745868772119,-0.07123290795233098
+2.6,1.7255718871952228,-0.04287196316195447,2.0275728568444698,-0.20482916250995453,-0.04560911034359113,0.927486003383553,0.3094099816856925,-1.0701757934934335,-1.0836661003320691,-0.7316711525424959,-0.8366382037481279,0.6604155041039053,2.5214914661554895,0.0,0.0,20.66376856918616,0.03953496855582328,0.003985297148613065,-0.07725234259912853
+2.62,1.7574996204723863,-0.039436894114472563,2.0259335354345844,-0.21425706605865022,-0.04605817624255098,0.9331038815396693,0.2851134867044212,-1.0461010211655115,-0.9759880008431984,-0.7337010874471241,-0.805494858450137,0.6347208440108438,2.4613819777484385,0.0,0.0,20.88543910810273,0.038234265994488315,-0.009751726444523878,-0.08346626714880817
+2.64,1.7876652387921972,-0.035909564862266125,2.0241730014648778,-0.22302978879836127,-0.04644639811423322,0.9379960512529079,0.2612735218975027,-1.0114747338749424,-0.8718364303112488,-0.7287171127182996,-0.774689370289588,0.5929628670960474,2.3963537138657522,0.0,0.0,21.12065231025711,0.03669209483956615,-0.025626490723679896,-0.08988878370403264
+2.66,1.8159737809321332,-0.032316890601445866,2.0222955017587974,-0.23097700436317883,-0.046831628816402085,0.9422475227144771,0.2379622405682166,-0.9672157759403388,-0.7721057822703622,-0.7163975078311863,-0.7440161274580801,0.5323430756530615,2.3262310797072683,0.0,0.0,21.368210799553516,0.03510341728199551,-0.04399542696503944,-0.0965349568686509
+2.68,1.8423347706935134,-0.028689939724392596,2.0203086883027144,-0.23789667101284762,-0.04726714713302481,0.9459587442767757,0.2152511204366089,-0.9143948726643335,-0.677558296007204,-0.6964911103788661,-0.7129615782570472,0.44960291129813906,2.2508258809590833,0.0,0.0,21.626154579105105,0.033752630068603295,-0.0652428608445556,-0.10342149596936245
+2.7,1.8666641127580135,-0.025064306657099363,2.018224701092986,-0.2435496074983671,-0.04779645813968536,0.9492462219960872,0.19321153511850062,-0.8542149759367336,-0.5888158268536412,-0.6688466268235318,-0.680586139081188,0.3409982115464734,2.1699360991166223,0.0,0.0,21.891435117424663,0.03303300962143774,-0.0897501044899478,-0.11056761388451725
+2.72,1.8888865401889312,-0.02148034918041821,2.0160614385983253,-0.24765406159793799,-0.048446348605075434,0.9522423077488523,0.17191568984249364,-0.7879873204819149,-0.5063561951738651,-0.6334491878535405,-0.6453842473274283,0.2023171501355999,2.0833442664254,0.0,0.0,22.159438320080397,0.03346353870062169,-0.11784198205936586,-0.11799607249426292
+2.74,1.9089387450909643,-0.017983178942515835,2.0138439637784833,-0.249880945047328,-0.04921795314534058,0.9550935289929737,0.15143796507542254,-0.7171030203708124,-0.4305140082294483,-0.5904651412106647,-0.605128787381582,0.028971510287616242,1.990815572611395,0.0,0.0,22.42332385133251,0.035692228586269525,-0.14969931045884705,-0.12573380367279904
+2.7600000000000002,1.926773334376618,-0.0146222502173627,2.011605920465755,-0.2498508564748846,-0.05007580133625938,0.9579565698397288,0.13185664162338928,-0.6430005362928132,-0.36148541716238375,-0.5402954442289131,-0.5567219368731042,-0.1837940389141244,1.892096491089437,0.0,0.0,22.67303498430932,0.04047669093924555,-0.18521391728429654,-0.13381347354442244
+2.7800000000000002,1.9423637518003736,-0.011450345963304064,2.009390703036828,-0.24713477040817936,-0.050935170817784536,0.96099069552389,0.11325589055572129,-0.5671303717482913,-0.2993357586532729,-0.48363808084334664,-0.4960882356892689,-0.44049992145139694,1.7869138942681622,0.0,0.0,22.894141314822633,0.04860195880597562,-0.22376925947361143,-0.14227268838628163
+2.8000000000000003,1.9557102425260775,-0.008521719051075724,2.0072519666577193,-0.24126133288261112,-0.05164901510231737,0.9643451465475594,0.09572767011074974,-0.4909199360351296,-0.24400882610976565,-0.4215565531383624,-0.41820159090834064,-0.7447409047877825,1.6749774653788683,0.0,0.0,23.06653654415096,0.0607396075915867,-0.26390612487007187,-0.1511509532879647
+2.82,1.966846788832946,-0.005889147534336558,2.005252846830219,-0.23173529247590458,-0.05199670179295619,0.9681399809983968,0.07937300810845699,-0.41574403644056446,-0.1953364711488981,-0.35554886721281403,-0.3173329552131142,-1.0980605235337122,1.555988227566219,0.0,0.0,23.162939769642296,0.07720623459431342,-0.3028608290746234,-0.16048769828038048
+2.84,1.975848676575367,-0.003599732370921817,2.003462966236383,-0.21807352472478242,-0.0516784918296396,0.9724394720046124,0.06430197962515714,-0.3429092497254102,-0.15304889688648352,-0.28760868912152243,-0.1876617684562123,-1.4982313526308793,1.4296532618046354,0.0,0.0,23.148111049324456,0.09752359929592203,-0.3358574514479608,-0.1703123780099518
+2.86,1.9828398434899595,-0.001689466023065938,2.001952157123667,-0.1998686186198475,-0.05032269069991764,0.9772188182596308,0.050631446068277915,-0.27366307148866587,-0.1167888992376677,-0.2202584377409787,-0.024516818246322514,-1.9366462664314896,1.2957122702276525,0.0,0.0,22.979269407220148,0.11974865306009175,-0.35512684180454523,-0.18063380579475993
+2.88,1.9879984512661804,-0.000176997630954233,2.0007798286621004,-0.17689344687262085,-0.047515938234874044,0.9823289921347144,0.038479803517936485,-0.20923716138338783,-0.08613646136639982,-0.15652824525350423,0.17347083228802476,-2.3945358647151282,1.1539723171205765,0.0,0.0,22.608693036182355,0.13974047895327435,-0.3487143078379126,-0.19144140324563055
+2.9,1.9915582813048527,0.0009423598068465961,1.9999793761366165,-0.14926135293089407,-0.04286803878246876,0.9874723586012881,0.027959269850856613,-0.15092503994538836,-0.060652700323925876,-0.0998462012210484,0.40130392211979793,-2.8379039915789552,1.0043302873459765,0.0,0.0,21.988919941787596,0.15057644754080624,-0.2991296490763341,-0.20275104135140887
+2.92,1.9938029324094697,0.0017006506415428784,1.9995383193344471,-0.11765414830264281,-0.03611955448603639,0.9922124046840809,0.019168285315690866,-0.10017206909381265,-0.03995089743831416,-0.05380518761364229,0.6439926558164626,-3.2113040929325414,0.8467346418796958,0.0,0.0,21.08165906339926,0.14217669649136666,-0.18167397130453686,-0.2147073846613809
+2.94,1.9950499032144968,0.0021571817595063816,1.9993779328305024,-0.08362474657997361,-0.027295975146196978,0.9960488453752359,0.012187252640068142,-0.05861273093353446,-0.023788716168320602,-0.021754910915748445,0.8726262997845966,-3.4306215962395155,0.6810684018114969,0.0,0.0,19.896291716409852,0.09893373389159876,0.03625064896362434,-0.22731449766016648
+2.96,1.9956227048422253,0.0023923716568491006,1.9993476381324697,-0.049945273015515115,-0.01694575966044795,0.998583155409602,0.0070705491398316235,-0.027923409720602862,-0.012144652355120675,-0.0058335605874832545,1.036327449373649,-3.377902944013586,0.5074670562903593,0.0,0.0,18.697940813302925,-0.013445577183156976,0.3767669620700846,-0.23742969144444365
+2.98,1.995813809595637,0.0024954545923018562,1.9992888173063397,-0.020731110965176533,-0.006639047375934434,0.9997558249366919,0.003799288518152399,-0.009264725753505725,-0.005178421440607834,-0.003729169208359941,1.035683468223422,-2.9218631763938996,0.32951662703800544,0.0,0.0,18.174161213549525,-0.23421966791604976,0.8466715146216237,-0.23639221462242882
+3.0,1.9958599205642589,0.0025512469418962733,1.999172728215246,-0.0016837428434832613,0.0006766879495327952,0.9999958101658875,0.002255383365704436,-0.0021580363748036476,-0.0027684259490047174,-0.005871472287899877,0.7356415296752087,-1.9031476159670109,0.15697564448800388,0.0,0.0,18.74430531537498,-0.560702769518466,1.5532105764106605,-0.22064962988222186
+3.02,1.9959231417371093,0.00262408184291302,1.9991808698905666,-0.0016439659322199573,0.0006824797624709441,0.999995879951269,0.0022520386042146467,-0.0031459583496519818,-0.003655409767583661,0.0004015488137856958,0.0005875821915860557,-0.003976153599771104,0.00033815733496064815,0.0,0.0,19.327437243861905,1.440540937842178,0.5846662049315385,-0.22158869647802074
+3.04,1.9959815456998506,0.002678154449175779,1.9993840091159523,-0.0083428248483166,0.019561133810259392,0.9997661909612813,0.003914425676667652,-0.0030055847756676407,-0.0025160979050515434,0.010180867285972468,1.886765058006247,0.6741043838848578,-0.16365436845735964,0.0,0.0,18.807736917421305,0.7551966654377452,0.37116774289734855,-0.23387940372630878
+3.06,1.9959798551528702,0.0028758184391177903,1.9996167047115874,-0.019197562915907913,0.04818010839963193,0.998627177680022,0.007341030685345519,-0.0001375673719190727,-0.009074130705905039,0.01227847445535776,2.8625193142116574,1.0908428348824561,-0.3404369302919102,0.0,0.0,19.213212686827635,0.3090457190372755,0.17663951348177087,-0.23390793239783206
+3.08,1.9958319157881081,0.0034550769686195037,1.999875910608885,-0.03201225817284407,0.08076446312825356,0.9961399162833942,0.012553246958157944,0.0071888130936963186,-0.026889601028777296,0.01694810945558025,3.2672305217668467,1.283562403591262,-0.5216287961607138,0.0,0.0,20.234383013059677,0.005848033573991884,-0.006467591766404435,-0.22299683827954345
+3.1,1.995429900734949,0.004713477805495539,2.0002748133955164,-0.04460094766435352,0.11335772320820921,0.9923605410450252,0.019527893724548773,0.019994917670618256,-0.05709624223956828,0.033211639016691163,3.2844959461542578,1.2509606670595705,-0.696999369169689,0.0,0.0,21.161813433225582,-0.17291482960984483,-0.1180138973765911,-0.2108914446031266
+3.12,1.9946687470207853,0.006967820391286942,2.0009105061173553,-0.05554500055626823,0.14353152182704015,0.9876820791475642,0.028241205462277263,0.038367269803040754,-0.09858609237738236,0.06307282603830486,3.065429800085171,1.0672626236526237,-0.8634543575665186,0.0,0.0,21.836833973269844,-0.26561732826590556,-0.16991989973350818,-0.19951289537760528
+3.14,1.9934658906829748,0.010517110775098894,2.001826051453918,-0.06421282751295937,0.16998758584196963,0.9825917477905123,0.03865734880195791,0.062072813429632806,-0.14931286555767745,0.10521487735905374,2.7198700946981997,0.809713341927912,-1.0207746814418273,0.0,0.0,22.27637964534187,-0.30412730270577804,-0.1847957899986799,-0.18872558509754225
+3.16,1.991769581882639,0.015622225856459011,2.0030205310069906,-0.07048585805944847,0.19215942156914279,0.9775142411954016,0.050718919310390305,0.09100430809286907,-0.20717698374257007,0.15725638659398591,2.3183698067337195,0.5297276214631967,-1.1691872023916328,0.0,0.0,22.520120057857582,-0.3093468615953483,-0.17841211228669807,-0.1784266675752959
+3.18,1.9895596656957741,0.022496855174831503,2.004464260620135,-0.07453990642392112,0.2099347055990227,0.9727438994725762,0.06434693290477182,0.1252866619325582,-0.27036133504488075,0.21634126639746457,1.9048941880140982,0.25821564021490745,-1.3090821490439264,0.0,0.0,22.61087122580693,-0.2948860766823249,-0.1611748327007823,-0.16857054259876475
+3.2,1.986843991509064,0.03130588538293424,2.006112025785828,-0.07669493917845387,0.2234693712349993,0.9684356905343707,0.07944582886132974,0.16522170211104317,-0.3373823039175164,0.2795146815529396,1.5058096690881657,0.011856898419976392,-1.4409165582330585,0.0,0.0,22.58789436630217,-0.269522084605756,-0.13971169838784891,-0.1591269441491662
+3.22,1.9836528687930863,0.04216814067947756,2.0079128663335757,-0.07731872275262583,0.23306705230923494,0.9646257471330021,0.09591002141530491,0.2111816872638898,-0.4070089059922761,0.34398124816687803,1.1360021659054926,-0.20205767042107056,-1.5651485043882387,0.0,0.0,22.484112952216794,-0.23882394263148954,-0.11801595429754003,-0.15006751529785506
+3.24,1.9800331441885293,0.05516123640931081,2.0098165153709178,-0.07676937374820711,0.2391034883516549,0.9612617845143547,0.11362995531397627,0.26351114827313743,-0.4781463298821981,0.4072569050902038,0.8030185774314127,-0.38210275231771007,-1.682199261176238,0.0,0.0,22.325581714425343,-0.20625787269320783,-0.09828927445873631,-0.14136471381211008
+3.2600000000000002,1.9760427537877436,0.07032712948779449,2.0117771485646156,-0.07536439533121587,0.241980470743774,0.9582328947345737,0.13249671370860872,0.32245929881394764,-0.5497347585113429,0.46724084764627105,0.5098107160143414,-0.530255420354942,-1.7924388498405148,0.0,0.0,22.132133090729177,-0.1739268072942699,-0.08155641133243119,-0.13299314662754472
+3.2800000000000002,1.9717461114478252,0.08767758748647567,2.0137552035818747,-0.07336686743416897,0.24209850941790098,0.9553943397199797,0.15240495441930887,0.38814358111494857,-0.6206827207704314,0.5222358338846421,0.25651964307640884,-0.650237928001396,-1.8961862368985232,0.0,0.0,21.918471930351807,-0.14305642470994662,-0.06810170691327727,-0.12493030099847996
+3.3000000000000003,1.9672104105563137,0.10719921422729232,2.0157179347504806,-0.07098222068217513,0.2398411036331386,0.9525863199103063,0.17325435773683864,0.46053606776658956,-0.6898364516801833,0.5709380651573437,0.04162235406210982,-0.7464475713664905,-1.9937163905554485,0.0,0.0,21.695301414424353,-0.11431181324191786,-0.05777049741637169,-0.11715649217608667
+3.3200000000000003,1.9625027792617633,0.1288579055088448,2.0176392095847273,-0.06836117288608486,0.2355661363151283,0.9496472320609552,0.19494994255492187,0.5394632382374756,-0.7559788043947607,0.6124098794027458,-0.13733680449982835,-0.8232993260110596,-2.0852692085670657,0.0,0.0,21.470296140669962,-0.08800336396472952,-0.05017165665169704,-0.10965443183528736
+3.34,1.9576881767385708,0.15260272526800375,2.01949889957862,-0.06560594681153521,0.22960164227738794,0.9464224606297871,0.21740163665885187,0.6246117962698875,-0.8178489809899752,0.6460429034148,-0.28328262896035955,-0.8848502663180438,-2.1710577846906243,0.0,0.0,21.248874301159198,-0.06422077665942072,-0.044809894584818655,-0.10240872446097843
+3.36,1.9528279081560145,0.1783692411708011,2.0212820935410747,-0.06277793825957134,0.2222442795822247,0.9427697944228413,0.24052344040606294,0.715536120289887,-0.8741748161718257,0.6715161656436542,-0.3993179671610074,-0.9346111336431692,-2.2512750519724984,0.0,0.0,21.03478378207714,-0.04292042834901744,-0.04116923666054462,-0.09540543237909892
+3.38,1.9479786441004445,0.20608237323696615,2.022978271726067,-0.05990570020212639,0.21375948306963605,0.9385624139176588,0.2642324462706123,0.8116654421264665,-0.9237108838151624,0.6887521159959775,-0.4885437988280906,-0.9754785134084792,-2.3260986772369825,0.0,0.0,20.830537809672165,-0.023982347034732676,-0.03876315554933368,-0.08863174544137316
+3.4,1.9431918439277052,0.23565880907100462,2.02458051807948,-0.05699256478076409,0.2043826624284112,0.933690198100309,0.2884478962131464,0.9123105450293898,-0.965277453234474,0.6978727412679588,-0.5539513025920698,-1.009741729356815,-2.395694448763764,0.0,0.0,20.637736769872152,-0.007247196859576421,-0.037162272933441765,-0.08207574263215871
+3.42,1.9385134966081814,0.2670090344517553,2.0260848088390215,-0.054023522302810816,0.19432103239813558,0.9280599142232138,0.3130903879356908,1.016670678845864,-0.9977969192515189,0.6991575135287423,-0.5983607041120749,-1.0391332846860113,-2.4602185078627197,0.0,0.0,20.457306607747046,0.00746004994218552,-0.03600722312544152,-0.075726216565426
+3.44,1.9339841053176092,0.30003902274156274,2.0274893918634724,-0.05097117011250198,0.18375579747588708,0.9215947002009933,0.33808128500821255,1.1238416863448335,-1.0203256266781167,0.6930045300182734,-0.6243904845716595,-1.0649019837007743,-2.5198187581225486,0.0,0.0,20.289676730876607,0.02031542936070463,-0.035011903473863515,-0.0695725318824224
+3.46,1.9296388531021778,0.33465162227201534,2.0287942572925717,-0.04780067009874683,0.1728444980271653,0.9142331297952239,0.3633423452127172,1.2328262312649523,-1.0320799830557281,0.6798958206074381,-0.6344460592119701,-1.0878949963577054,-2.5746357199287955,0.0,0.0,20.134913914879405,0.03148742547092098,-0.03396067804289715,-0.06360449617487732
+3.48,1.9255078966905341,0.3707476776921883,2.03000069278996,-0.04447373441569424,0.16172338159481459,0.9059280609250921,0.38879555452855,1.3425467021460367,-1.0324564361059325,0.6603674135739402,-0.6307205763203861,-1.1086400230028637,-2.624803022884201,0.0,0.0,19.992823909901325,0.0411325695399285,-0.03270195467918324,-0.05781223245798471
+3.5,1.921616744269279,0.4082269188909698,2.031110913236396,-0.04095170931404832,0.15050970448007164,0.8966454014392207,0.4143631383695682,1.4518609772094013,-1.0210453397802322,0.6349843905643365,-0.6152028340302363,-1.127422064909922,-2.6704476642198296,0.0,0.0,19.863029131063442,0.04939379380854092,-0.031139776742982572,-0.052186052435846286
+3.52,1.9179866810018977,0.4469886491091476,2.0321277537309963,-0.037197856275395715,0.13930390026133657,0.8863628789974183,0.4399677142653794,1.5595808678214298,-0.9976390050887078,0.6043208561432832,-0.589688946196925,-1.1443516009815113,-2.7116901141456657,0.0,0.0,19.745028624909416,0.05640049640644207,-0.029224552997017872,-0.04671633781192147
+3.54,1.9146352133437947,0.4869322619075963,2.0330544151375483,-0.033178944207022214,0.1281915764653655,0.8750688678425546,0.46553254871961586,1.664492764117775,-0.9622343834606044,0.5689445147680575,-0.5557955855995609,-1.1594225604130486,-2.7486443195255426,0.0,0.0,19.63824506668786,0.06226952658672236,-0.02694371150041608,-0.041393441433727814
+3.56,1.9115765097858126,0.5279576145554308,2.033894252585797,-0.028866270618187237,0.11724531918946934,0.862761301672353,0.4909818834594452,1.7653798080361782,-0.9150309049010494,0.529405393601132,-0.5149735393365373,-1.1725596040102584,-2.781417641920885,0.0,0.0,19.542062544456734,0.0671065006969418,-0.024312842920674926,-0.0362076206848458
+3.58,1.908821821498421,0.5699652830664655,2.0346505989464783,-0.02423622653728094,0.10652629862745051,0.84944668581596,0.5162413011182373,1.861044816909337,-0.8564240219547795,0.4862281670357889,-0.4685209936019384,-1.1836550382690405,-2.8101107601750286,0.0,0.0,19.455858085399242,0.07100704341353303,-0.02136774647025902,-0.031149012635896513
+3.6,1.9063798713963636,0.6128567215494618,2.0353266171135402,-0.01927051168164826,0.09608567677479714,0.8351392113479015,0.541238106351966,1.9503331623562152,-0.7869950133270806,0.4399075143425701,-0.4175964409679598,-1.192596277713374,-2.8348175665501802,0.0,0.0,19.379029121975854,0.07405773236495372,-0.01815767012865782,-0.026207655090571873
+3.62,1.904257204364968,0.6565343458039649,2.0359251767718374,-0.013956094689883868,0.08596582304772328,0.819859966971954,0.565901704579056,2.0321548550342037,-0.7074975888501651,0.390905958566629,-0.3632313675160737,-1.1992851863431295,-2.8556250851044553,0.0,0.0,19.311018314299915,0.07633670757307942,-0.014739918433709087,-0.021373551233223848
+3.64,1.902458494778999,0.7009015582705953,2.0364487530445445,-0.008284999245984564,0.07620134554704516,0.8036362402236945,0.5901639663038046,2.1055051741484787,-0.618841815516928,0.33965367671191343,-0.3063429234139347,-1.2036508786162818,-2.872613438944684,0.0,0.0,19.251336355683577,0.07791407491248525,-0.011175868571130065,-0.016636769558342167
+3.66,1.9009868100267426,0.7458627286244086,2.0368993458512588,-0.002253981537880646,0.06681994692355481,0.7865008968919915,0.6139595698809924,2.1694832908012907,-0.5220758552753012,0.28654982579758903,-0.24774662250234766,-1.2056576335617115,-2.885855888052627,0.0,0.0,19.199582664455008,0.07885235801218116,-0.007528283468320129,-0.011987566359682117
+3.68,1.8998438305770955,0.7913231415776745,2.037278419837563,0.004135851547693326,0.05784311594134308,0.7684918260174878,0.637226319304593,2.2233084491450574,-0.4183659720801786,0.2319649884344935,-0.18816880358012095,-1.2053094553305599,-2.895418951646008,0.0,0.0,19.155463305190562,0.07920730426319311,-0.0038596484487630756,-0.007416516100638813
+3.7,1.8990300282730441,0.8371889209285336,2.03758686527795,0.010879449955633735,0.04928667018631298,0.7496514371721963,0.6599054359953925,2.266333376934042,-0.3089752328162702,0.1762444009416986,-0.12825820973015387,-1.202652493271163,-2.9013626195556754,0.0,0.0,19.118805173494103,0.07902930213767034,-0.00023111066746855924,-0.0029146358362950424
+3.72,1.8985448051057243,0.8833669366423325,2.0378249803914854,0.0179682348049673,0.04116117246089278,0.7300261969760681,0.68194182460698,2.298054691715828,-0.19524130015836968,0.11971168509854987,-0.06859572839324933,-1.197776036589294,-2.9037406475644296,0.0,0.0,19.08956546678048,0.07836550982252832,0.0032984944674357974,0.0015265066546902377
+3.74,1.898386594853773,0.929764699856125,2.037992475103766,0.025390512680099474,0.03347225272887502,0.7096661931041375,0.7032843127272049,2.3181201511473297,-0.0785537013227894,0.06267286086503973,-0.00970122355304883,-1.190812188074706,-2.9026009239877393,0.0,0.0,19.06783573514644,0.0772625460054337,0.006674046698618374,0.005914706051615096
+3.7600000000000002,1.8985529298268984,0.9762902492026247,2.038088495566502,0.03313189098092105,0.026220877372361822,0.6886247165064557,0.723885863383348,2.326332671619155,0.03966904130012888,0.0054204710922715615,0.04796339802453699,-1.1819337113027217,-2.897985890617862,0.0,0.0,19.053840277427597,0.07576930006311355,0.009846365350763886,0.010257053417558285
+3.7800000000000002,1.8990404747163836,1.0228520307746611,2.0381116678741047,0.041175697952036565,0.01940361537456072,0.6669578561707327,0.7437037580251286,2.322651117338548,0.15800201475642076,-0.051762301204859466,0.10399515235074205,-1.1713490862683178,-2.8899330011771336,0.0,0.0,19.047929169199875,0.07393915750404167,0.012775385413633433,0.014559886634473183
+3.8000000000000003,1.8998450294178837,1.0693587733785936,2.0380601585616778,0.049503407213515684,0.013012953748026268,0.644724105205,0.7626997468706914,2.3071879505836606,0.27503582885028655,-0.10859759966156335,0.15805430605423945,-1.1592936581217463,-2.878475203703425,0.0,0.0,19.05056659926992,0.07183080203512737,0.015434048861225236,0.01882875367381997
+3.8200000000000003,1.9009615028160656,1.1157193604138311,2.0379317487569004,0.0580950567222355,0.007037709292112724,0.6219839816922,0.7808401638944278,2.280203936919618,0.38939739595859807,-0.16480898948933556,0.2098702796633018,-1.146016046367364,-2.863641437025039,0.0,0.0,19.062315301721164,0.06950683761703158,0.017812271210164266,0.023068391155451055
+3.84,1.9023838600056004,1.1618426997165798,2.03772391835241,0.06692963784480181,0.0014635690196200797,0.5987996717933837,0.798096005881388,2.2421002129059944,0.49976914607640155,-0.22011770559212618,0.25924631900021206,-1.1317597385850164,-2.8454571315822856,0.0,0.0,19.083817687877776,0.0670298200459789,0.01992000699311225,0.027282731353225752
+3.86,1.9041050462714493,1.2076375929825862,2.0374339362790956,0.07598541506250288,-0.0037262322208520848,0.5752347049597105,0.8144429790360589,2.193408135060934,0.6049063007283525,-0.2742390765187318,0.3060595468523597,-1.1167409429259791,-2.8239446976071476,0.0,0.0,19.115773999450678,0.06445586133338155,0.021788285297301258,0.03147496183148642
+3.88,1.9061168922550897,1.253012606921488,2.0370589529096903,0.0852401254175007,-0.008550118571865127,0.5513536709767297,0.829861522178068,2.1347774164522675,0.7036519372870653,-0.3268791622434826,0.350253164472006,-1.1011251012545136,-2.7991239682448654,0.0,0.0,19.158917767527115,0.0618266265489934,0.02346721487062753,0.035647666405760677
+3.9,1.908410005888542,1.297875949089256,2.036596090878483,0.09467100364059668,-0.013027318066665594,0.5272219853904858,0.8443368215200534,2.0669630968020454,0.7949497309273641,-0.3777316756600955,0.3918185782770094,-1.085005640818781,-2.7710125457010393,0.0,0.0,19.213989318387565,0.05916105928160631,0.02502040896612323,0.039803068034495126
+3.92,1.9109736576263594,1.3421353523658732,2.036042531292643,0.10425458628028908,-0.017177027549654684,0.5029057037102196,0.8578588369538505,1.9908118628991451,0.8778544431435137,-0.4264753016578866,0.43076690258685996,-1.0683891877303158,-2.739625983611573,0.0,0.0,19.281708807845575,0.05644834965056056,0.02651600988537972,0.04394337307499676
+3.94,1.91379566597858,1.385697973214388,2.0353955935093064,0.11396626463388129,-0.021017698728077572,0.47847137641281956,0.8704223623156736,1.907248130000698,0.951540399011849,-0.4727715758837632,0.46709119922519626,-1.0511912473364549,-2.704977737869871,0.0,0.0,19.36275052757119,0.05364344764840956,0.028015364869231063,0.048071189280270016
+3.96,1.9168622900918055,1.4284703100721694,2.0346528082333415,0.12377957698559024,-0.02456655126126312,0.4539859277079347,0.8820271414891732,1.8172601219814053,1.0153083225165402,-0.516263527458002,0.5007224665929652,-1.0332450976335055,-2.6670788355163464,0.0,0.0,19.457719185974,0.050665963728325436,0.029561209140215206,0.05218996753280114
+3.98,1.9201581349177939,1.4703581494457263,2.033811985272804,0.13366524991898082,-0.02783940450814913,0.4295165334352559,0.8926780584435179,1.7218859793251111,1.0685909503371345,-0.5565753155653704,0.5314834837679357,-1.0143244115072858,-2.6259372441649,0.0,0.0,19.567126518427187,0.047402800720109495,0.031167727315579152,0.05630440585911954
+4.0,1.923666072203913,1.5112665486215553,2.032871278410157,0.14359001313246791,-0.03085088285044722,0.4051304698780498,0.9023854129651899,1.6221997275833304,1.1109578045055062,-0.593313092193709,0.5590451297952006,-0.9941772617732566,-2.5815569658464437,0.0,0.0,19.691364172947516,0.04371441132941234,0.03281494484365832,0.060420753610960054
+4.0200000000000005,1.9273671771865055,1.551099865633995,2.031829250284301,0.15351522251281952,-0.03361499034823993,0.38089490801149656,0.9111652857366993,1.5192968092256063,1.142119374464721,-0.6260673028026873,0.5828898112337244,-0.9725661975312462,-2.533936922248691,0.0,0.0,19.830668228603397,0.039444033065552886,0.03444953344168855,0.06454696301701805
+4.04,1.931240676861849,1.5897618495192605,2.030684940168547,0.16339533285900149,-0.03614597401672837,0.3568766382334992,0.9190399876204703,1.414278866163851,1.1619307553793865,-0.6544166001196219,0.6022860016055731,-0.9493065626206572,-2.4830697362174403,0.0,0.0,19.985073246263394,0.034428384957936764,0.03599337012838921,0.0686926556540094
+4.0600000000000005,1.9352639018825224,1.62715580698309,2.029437937732323,0.1731762607288766,-0.03845931204743788,0.33314172895808286,0.9260385804401571,1.3082375854300463,1.1703945443219765,-0.6779334972227959,0.6162761424222124,-0.9242936184774532,-2.4289405385103615,0.0,0.0,20.154359287794982,0.02850708857172579,0.037360069926561934,0.07286889729141169
+4.08,1.9394122316937894,1.6631848650876488,2.0280884671735904,0.18279365912474177,-0.040572589371639295,0.3097551464719248,0.9321974535524243,1.2022376833279897,1.1676625621062922,-0.696191813969687,0.6236767353018539,-0.8975087874962373,-2.371525934606444,0.0,0.0,20.338001906510492,0.021526922927203707,0.038478059142033394,0.07708778861183205
+4.1,1.9436590228540702,1.6977523527823386,2.0266374894144765,0.19217107732129882,-0.042505977153074734,0.28678038808846656,0.9375609462617681,1.097299463942992,1.1540358196386222,-0.7087758577332737,0.623084388948623,-0.868997102218315,-2.310793263272562,0.0,0.0,20.53513888091909,0.013336834052587232,0.03931645895906488,0.08136187007546389
+4.12,1.947975514722397,1.730762326513846,2.0250868360446277,0.2012178987098018,-0.04428203585321441,0.2642791973587515,0.9421820229567749,0.9943817636192345,1.129962161566063,-0.7152911374479216,0.6128761050243834,-0.8388125432602376,-2.2467002852393323,0.0,0.0,20.74456379431066,0.003771432144474285,0.0399075459911471,0.08570333587195349
+4.14,1.9523307152885219,1.7621202669435327,2.023439397359248,0.2098268556697627,-0.04592463018194126,0.24231142837082523,0.9461230314622316,0.8943663597251496,1.0960312395494798,-0.7153763248101771,0.5911888483689683,-0.8069356747732502,-2.1791954520356502,0.0,0.0,20.96474523854291,-0.007374865767768357,0.04035854803268524,0.09012309763841964
+4.16,1.9566912820954347,1.7917339762586602,2.021699397242113,0.2178708567394381,-0.04745687462357882,0.22093509914083026,0.94945659026741,0.7980449388686985,1.0529668493876383,-0.7087162798322166,0.5558659922486013,-0.7731770212960062,-2.1082188811132885,0.0,0.0,21.193851149826457,-0.020375340205063708,0.040847552112474594,0.0946299707750386
+4.18,1.9610214269420398,1.8195147111235936,2.019872796986673,0.225198878684731,-0.0488981882196875,0.20020662584160803,0.9522666324076939,0.7061094011260808,1.001617049395746,-0.6950563886604456,0.5043681838126995,-0.7370862437338086,-2.033703952730188,0.0,0.0,21.429753821895602,-0.03558776131897512,0.04160578868654241,0.09923037548737285
+4.2,1.9652828854031505,1.8453785962942941,2.0179678764189197,0.2316308086367001,-0.050260642509154546,0.18018118314585074,0.9546495574529099,0.6191456687746498,0.9429426367885216,-0.6742190574511671,0.43366251806835854,-0.6978844605522705,-1.9555791258825144,0.0,0.0,21.669982096539798,-0.053449747992589614,0.04289794779221745,0.10392915986511131
+4.22,1.9694350011791537,1.8692483791741086,2.015996038137356,0.2369513379344722,-0.05154475235207299,0.16091312589648607,0.9567153013672608,0.537630523386323,0.8780043226648603,-0.6461238462777087,0.34011834945635844,-0.6544226489542391,-1.8737690645775853,0.0,0.0,21.91159847221541,-0.07444784251423564,0.04501318697905832,0.10873161482663421
+4.24,1.973434982546131,1.8910556040814401,2.013972866541247,0.24090326118690578,-0.05273470315071019,0.14245644176917352,0.9585879365147394,0.46193066448882547,0.8079484022375195,-0.6108129161982507,0.21944502302219004,-0.6051536240070969,-1.7881940638780611,0.0,0.0,22.150936448019994,-0.09906216905998555,0.04827406704976667,0.11364643528756103
+4.26,1.9772383979385042,1.9107433080986653,2.011919434515313,0.24318077155559895,-0.053792812982863016,0.12486526906007789,0.960405180223206,0.3923032852568968,0.7339902092030046,-0.5684835570508692,0.06670496680741914,-0.5480967871253712,-1.698766909722145,0.0,0.0,22.383132089906812,-0.127685271536504,0.05304558630080247,0.11868822446438534
+4.28,1.9807999966082106,1.9282693644917794,2.0098637835171926,0.24342366718960065,-0.054653085845848604,0.10819452326681382,0.9623159063397839,0.32889789507821277,0.6573946226646478,-0.5195291117845705,-0.12356162887689823,-0.4807987503465016,-1.6053873681852662,0.0,0.0,22.60127686658211,-0.16050172843058397,0.059723076010970044,0.12387954140134472
+4.3,1.9840749667864406,1.9436106269157438,2.0078423786458623,0.24121393608407254,-0.05521405139838153,0.09250062880437751,0.9644744056934847,0.2717593411464499,0.579453609323516,-0.46458990282214535,-0.35704548146900206,-0.40032128713068277,-1.5079344946918036,0.0,0.0,22.795187488568473,-0.1972828191381496,0.06865475405598753,0.12925069814801127
+4.32,1.987020770301245,1.9567680260996327,2.0059011761040617,0.23607742766724757,-0.05533197009800324,0.0778421899945068,0.9670296865598017,0.22083208282258465,0.5014622727601918,-0.4046140308356446,-0.6390923549986327,-0.3033456903652655,-1.4062583855315423,0.0,0.0,22.949670110448245,-0.23706894732640815,0.08000308386534283,0.13483923446151755
+4.34,1.989599694040637,1.9677727149071895,2.0040956806294985,0.2274949760032517,-0.05481642731791191,0.06428034960605199,0.9701078454707516,0.1759656707233199,0.42469682960444544,-0.34092701378320556,-0.9736875566740303,-0.1864784874439781,-1.3001716226446132,0.0,0.0,23.042534950394817,-0.2776667181886452,0.09350434910117333,0.14068685176709908
+4.36,1.9917822072499005,1.9766931611430083,2.00248907669432,0.21493020033627466,-0.053431875855886844,0.05187848999521409,0.9737857392166649,0.13692246219978854,0.3504009454483512,-0.2753031945240926,-1.361745842460857,-0.04689449184306793,-1.1894428490654465,0.0,0.0,23.04265475814403,-0.3149270216673352,0.10812656225912642,0.14683408494661948
+4.38,1.9935510611753366,1.9836426745215956,2.0011471988483054,0.19788479457695515,-0.05091042242401162,0.040701004215497945,0.9780558088471716,0.10339027220552924,0.279789771119854,-0.21002505763781454,-1.7984083876422756,0.1165509364884422,-1.0737957096861557,0.0,0.0,22.908821851229863,-0.3417365539827186,0.12159567794003967,0.15331287561413998
+4.4,1.9949057666360457,1.9887861318480826,2.0001289484150515,0.17599685555232916,-0.046983324045729184,0.0308110851072391,0.9827860149248264,0.07500513035468327,0.2140826475033013,-0.14790493344379452,-2.2689649438454937,0.3015843631894807,-0.9529182072258399,0.0,0.0,22.591082291203275,-0.34664465487338203,0.12978800559481085,0.1601373711155359
+4.42,1.9958666041099473,1.9923436058807211,1.9994711597857615,0.14920321798310499,-0.04144057908804521,0.022267936611432636,0.9876868011406261,0.051390326188925854,0.15457069963344017,-0.09222577782051387,-2.7429408720525505,0.5000265975387276,-0.8264901799927011,0.0,0.0,22.037263237456564,-0.31195983777972713,0.12606545632937238,0.16730057583939675
+4.44,1.9964767346197212,1.99458746782068,1.9991684786014448,0.11799236680685372,-0.03422789096689701,0.015124592723574064,0.9923091753834694,0.032214043298620654,0.1027063109971041,-0.04653690224574144,-3.1657427834413894,0.6945893846848327,-0.6942348856125349,0.0,0.0,21.20596813357938,-0.21079128950453352,0.10082613632226178,0.1748276669545097
+4.46,1.9968006189464822,1.995829190693021,1.999151864455269,0.08378163152155858,-0.025584971460723232,0.009428582530266424,0.9961110125314395,0.0172538977296628,0.06015683506975396,-0.01424303664842564,-3.446787830938415,0.8553756859119165,-0.5559738851385823,0.0,0.0,20.082682089984353,-0.0014404593330858972,0.041960524850413905,0.18299278404905622
+4.48,1.9969173761351182,1.996393714695754,1.9992741011782653,0.049463394455286545,-0.01621525662382466,0.005227236792058514,0.9986306194269751,0.006428685498570657,0.028687620396424758,0.002053960647023109,-3.442241728676518,0.9371331126641117,-0.4116626750998658,0.0,0.0,18.792002875268306,0.36497555083061706,-0.06357246083304169,0.19106805645819963
+4.5,1.9969088754821915,1.9965816661004083,1.9993415046422565,0.01985857176567676,-0.007467785917368671,0.00255112101835115,0.9997716544703943,-0.0002785022838532134,0.009612954857845607,0.0027195873338694684,-2.9623137591439335,0.8777782492723295,-0.2631467084182476,0.0,0.0,18.208173772876336,0.8398146106579514,-0.22879927545100892,0.18743185730308415
+4.5200000000000005,1.996846489569091,1.9966315954224705,1.999299214479492,0.0008845702548103091,-0.0015218746796181184,0.001306542784104257,0.9999975971865518,-0.003128806408322322,0.0024644968402238385,-0.0021380044434074694,-1.8969536348824176,0.5970325243738921,-0.12213347772566754,0.0,0.0,18.80181071960565,1.4808495555471888,-0.46399910944220824,0.16860912568120295
+4.54,1.9967809272265173,1.9967034557943861,1.999307880101362,0.000829557222930486,-0.0014956232227179688,0.0013016934528792944,0.9999976902674047,-0.0032674205364779823,0.0036023006506212627,0.0004369898286000532,-0.0054986063487407755,0.0026319122802268557,-0.0004788939507678197,0.0,0.0,19.1412382407069,-0.3032822128419834,-1.553573378463879,0.17018674028254116
+4.5600000000000005,1.9967291491099106,1.9967638522080373,1.9994305557607313,-0.0026834682794380374,-0.020425641644945015,0.002518372152075362,0.9997846017833487,-0.002442406877845522,0.0030180608137059055,0.006194923550100361,-0.3535881359942936,-1.8925885302383412,0.1237987760986273,0.0,0.0,18.522681033582508,-0.20222403116638965,-0.8705890510133236,0.18993210402680466
+4.58,1.9965350579091425,1.9968408554790207,1.9994942576033035,-0.00851351620817091,-0.04984836829459684,0.0051320477709610535,0.9987073256494197,-0.009427251962553776,0.0038840426135578095,0.003901724632506832,-0.5853279866149165,-2.943517766756258,0.26563550008553005,0.0,0.0,19.102027571001145,-0.09743425260697769,-0.371484632864333,0.19332285503777064
+4.6000000000000005,1.995955702301429,1.9969795373111787,1.9995322173254113,-0.015507653184835315,-0.08381862822871428,0.0091950722385489,0.9963179215998693,-0.02834868056292046,0.007253008661097198,0.005909835879309119,-0.6978664923486866,-3.405110475417108,0.41303181793389565,0.0,0.0,20.292188183550135,-0.002752855551881539,-0.005828362545412746,0.18592673540073318
+4.62,1.9946765876229324,1.997237153866804,1.9997029266654325,-0.0224411850635175,-0.11777582172067964,0.014683842132131785,0.9926780111452063,-0.06058682754551306,0.013802860414782412,0.021670682165158668,-0.6818040503520293,-3.4165703418279483,0.5557744657255824,0.0,0.0,21.296509913838808,0.0471972965012909,0.2049924237780021,0.17850942049148114
+4.64,1.9923657314327514,1.9976680255151864,2.0001255957233046,-0.028635238036224836,-0.14899178776859529,0.021570209219963884,0.9881883405476577,-0.10488679455821662,0.02369774858195292,0.05199217475441854,-0.5897838514339061,-3.1585451075622917,0.6932105471981281,0.0,0.0,22.02719243960943,0.06833431491839692,0.31240714899156774,0.1715773279525086
+4.66,1.9887133528553913,1.998313903239932,2.0008587303717316,-0.0338381260785308,-0.1760697334842086,0.02982036157925228,0.9833438748523203,-0.15909562520358783,0.0370301669194022,0.09520345930308907,-0.46356858801639894,-2.760783313090531,0.8258177123073767,0.0,0.0,22.502426707960996,0.07380856076567133,0.35550372747414816,0.16493434971366785
+4.68,1.9834537274451127,1.9992002558735722,2.0019118095865087,-0.03804472331410468,-0.19845739503137125,0.03939186838306774,0.9785783270084243,-0.2211039446198442,0.054027355355459036,0.1486777147299758,-0.328049056780377,-2.3054711392654625,0.9538297561143201,0.0,0.0,22.762247829152095,0.07147789877978006,0.3595074631006987,0.15852372385480468
+4.7,1.9763742771851907,2.0003363878099716,2.0032608877975955,-0.04137379577308445,-0.21611307363870993,0.05023507701084708,0.9741969952034706,-0.2891762484213805,0.07507694768441867,0.2093652429325756,-0.19681707001531357,-1.8429772289876136,1.0773316652033895,0.0,0.0,22.853429944451232,0.065973681945387,0.3408071892990614,0.15232140073579958
+4.72,1.967315986044041,2.0017176989206567,2.0048627649784225,-0.043991206989512976,-0.22928404216012035,0.062295993749855734,0.9703673587260857,-0.36196546149583064,0.10067919634690578,0.27419773064784986,-0.07634868932813423,-1.4027068464325072,1.1963493601516735,0.0,0.0,22.820007326427508,0.059916221461945085,0.30996530884930706,0.146309649591493
+4.74,1.956168936184962,2.003328785601236,2.0066658343122215,-0.0460650610450782,-0.23836438573939311,0.07551903832033079,0.9671387204600834,-0.4383921587546352,0.1313818510670862,0.3403469524447976,0.03117462393872708,-1.0004502724888487,1.3108982767409947,0.0,0.0,22.69901148985131,0.0546660180571269,0.2736585079563024,0.1404728087513657
+4.76,1.942865748899982,2.005146574729033,2.0086174318021484,-0.047741960936341206,-0.24380731588659266,0.0898490186628928,0.9644717993448244,-0.5174997758577407,0.1677235715048886,0.40535743612336705,0.1260116846327885,-0.6433503658158448,1.4210046481064338,0.0,0.0,22.519533971438076,0.05080933792099418,0.23598235610761142,0.13479693622132405
+4.78,1.927374631397543,2.0071430879097387,2.010668271942741,-0.04913658200752112,-0.24607352276918973,0.10523219924658211,0.9622679470616834,-0.5983388834994041,0.21019274717794734,0.46719256548683613,0.20967134473962634,-0.33322638681248584,1.526711600322581,0.0,0.0,22.30342526540423,0.04847424715948079,0.19933583029162594,0.12926976706960733
+4.8,1.9096929035181345,2.009287694648079,2.012774748157931,-0.050329221639419754,-0.24560257781134068,0.12161658751183982,0.9603935385369736,-0.679895915136933,0.25919962342264413,0.5242298096128166,0.2841711380078193,-0.06876671758509917,1.6280779878346339,0.0,0.0,22.06658986252172,0.04753403981093629,0.16501990827423163,0.12388045821298588
+4.82,1.8898413552349898,2.0115488527449616,2.014899806378808,-0.05136782168584935,-0.24279862641543665,0.13895165369766788,0.9586983946167383,-0.7610627516025629,0.31505715239049814,0.575229811125249,0.3515946912298581,0.1530495359588072,1.7251744730797058,0.0,0.0,21.820337402195626,0.047735446966365114,0.13363995700708775,0.1186192495961987
+4.84,1.8678595008204886,2.013895394936292,2.017012941588341,-0.052272247588728886,-0.23802466983035375,0.15718770507969385,0.9570286798502712,-0.840636765119334,0.37796661575993573,0.6192940390438317,0.4138443702915695,0.33605063462816276,1.8180791608985647,0.0,0.0,21.57256122720397,0.04877722478932156,0.10537380064265955,0.11347714517625379
+4.86,1.8438016635894392,2.016297438508596,2.01908970617633,-0.05303946946783751,-0.2316018287826931,0.17627510263992208,0.9552353091109893,-0.9173404353406748,0.44800563447621017,0.6558189451643034,0.4725188694246506,0.4843163744772079,1.906873788984657,0.0,0.0,21.328679008610926,0.05035650052452645,0.08014830732659906,0.10844566088440497
+4.88,1.8177337787677845,2.0187269925243885,2.021110987024154,-0.05364884795782621,-0.22381135663746382,0.1961634622003125,0.9531789831141276,-0.9898516862784243,0.5251176422742223,0.6844506755723058,0.5288686643487565,0.6018497688484927,1.991640778088311,0.0,0.0,21.092345375811664,0.052194488220024926,0.057754084056215294,0.10351664743037521
+4.9,1.7897307975587033,2.021158327997722,2.0230622109478476,-0.05406708022696752,-0.21489804615908104,0.21680093633747816,0.9507328408106732,-1.0568386705456096,0.6091028993587907,0.7050423862680705,0.5837981708892332,0.6923961258626226,2.0724611166719624,0.0,0.0,20.86597428440502,0.05404921784617445,0.0379185950648645,0.09868217822134764
+4.92,1.7598745880222078,2.0235681657836664,2.024932570919954,-0.05425258117406928,-0.2050742202744644,0.2381336340570893,0.9477835163926073,-1.1169949942911834,0.6996116443125376,0.717615238194305,0.6378942231143464,0.7593559092016203,2.1494129292454747,0.0,0.0,20.65111377634629,0.05572026094658045,0.020351556199846188,0.09393448704222132
+4.94,1.7282522453786222,2.025935727330237,2.026714321276016,-0.05415920956455149,-0.19452383277466748,0.2601052044286974,0.9442311905095604,-1.1690730761228592,0.7961401363480228,0.7223236569142687,0.6914677192060202,0.8057558331665859,2.222570553502575,0.0,0.0,20.448711176481375,0.05704868464475421,0.004771990774047552,0.08926593999851382
+4.96,1.6949547403102294,2.028242685310419,2.0284021622994732,-0.05373932994676419,-0.18340640909460607,0.28265658825104756,0.9399890566587319,-1.2119144774145012,0.8980302589063541,0.7194251482037175,0.7446001271818616,0.8342548324893665,2.292003970716829,0.0,0.0,20.259299257602713,0.057914322367599746,-0.009076727681925134,0.08466902834242375
+4.98,1.6600758469728463,2.030473044364517,2.0299927179649324,-0.052946248161685625,-0.17186068108146182,0.30572592734735093,0.934982490986553,-1.2444767372094128,1.004473151650003,0.7092547377190569,0.7971897821743501,0.8471697086753869,2.357778466703891,0.0,0.0,20.08312645770313,0.05823170184679349,-0.021419600517034065,0.0801363716983255
+5.0,1.6237113026390892,2.0326129764403666,2.031484101402645,-0.05173608338705181,-0.16000784465478718,0.32924861282076934,0.9291481142854979,-1.2658566171211436,1.1145170913709233,0.69220391079586,0.8489950667412615,0.846510685761854,2.4199544332676814,0.0,0.0,19.920247952545775,0.05794547762687907,-0.032447657741329516,0.07566072416215083
+5.0200000000000005,1.5859581587099643,2.034650630264965,2.032875558166497,-0.05006915072551226,-0.14795441473715187,0.3531574499686031,0.9224328630513369,-1.2753098203181694,1.2270796029755364,0.6687037640609849,0.8996729920447157,0.8340207056393639,2.4785872472846036,0.0,0.0,19.770589506803823,0.0570258861747176,-0.042318648455439534,0.07123497840901291
+5.04,1.5469142889039476,2.0365759301528157,2.0341671758315143,-0.04791093043098995,-0.13579467934290687,0.3773829171092331,0.9147931360048126,-1.2722672800571702,1.340963575214867,0.6392119512165209,0.9488126205144527,0.8112146662377602,2.533727185456412,0.0,0.0,19.633992418910346,0.055464520358510905,-0.05115951285160247,0.06685216513396962
+5.0600000000000005,1.5066780253232657,2.038380375571236,2.03535964860354,-0.04523269638451766,-0.12361277113056109,0.401853497099657,0.9061940481265508,-1.2563480797176203,1.4548769913296904,0.6042029172669303,0.9959633449054013,0.7794163649367644,2.5854193480123806,0.0,0.0,19.510245294509915,0.053270571485966145,-0.059069808923460365,0.06250544682032641
+5.08,1.465347897212652,2.0400568496083156,2.0364540866988037,-0.04201186974111234,-0.11148438415552987,0.426496062870559,0.8966088016778724,-1.227369010149265,1.567455761486095,0.5641608699686582,1.0406583718206968,0.7397919280778936,2.633703575497193,0.0,0.0,19.399106581777485,0.050467589060048446,-0.06612555490451644,0.05818810600217915
+5.1000000000000005,1.4230224508280842,2.0415994417134566,2.0374518617351276,-0.0382321554213294,-0.09947816849008725,0.45123630120982194,0.8860181695406567,-1.18535071791849,1.6772890562081606,0.5195749309985825,1.082433926039936,0.6933791671195286,2.6786143504279507,0.0,0.0,19.300320565873946,0.04709074631652166,-0.07238314187793682,0.05389352892899342
+5.12,1.3798001320708768,2.043003287793272,2.038354480953857,-0.033883510453221966,-0.08765683735390545,0.47599916185304086,0.8744100779952618,-1.1305203646896667,1.7829464811001394,0.47093593565967223,1.1208447505812997,0.641112723309559,2.720180680985433,0.0,0.0,19.213628682643805,0.04318456328687581,-0.07788310065850856,0.04961518593219737
+5.14,1.335779216488717,2.044264428927852,2.0391634845772137,-0.02896198483867846,-0.07607802142359926,0.5007093214516932,0.8617792719147168,-1.063310710309204,1.8830063968701973,0.41873440077062707,1.1554764666028425,0.5838451257937768,2.7584259676941723,0.0,0.0,19.138777449408007,0.03880102115713513,-0.08265359352419888,0.045346609918390075
+5.16,1.2910577739268794,2.045379688593019,2.039880361901701,-0.02346946808759086,-0.06479490374039072,0.5252916540885171,0.8481270439397087,-0.9843555514855786,1.9760846705707928,0.3634592475175918,1.1859553061983155,0.5223640461626728,2.793367856684612,0.0,0.0,19.07552393453889,0.033997996444309564,-0.08671355852602103,0.041081374341834745
+5.18,1.2457336575338112,2.046346567291777,2.0405064827936896,-0.01741336807928201,-0.053856666585963615,0.5496717016929351,0.8334610095465634,-0.8944814920629844,2.060863141422319,0.3055969420693309,1.2119556605848516,0.4574061212622324,2.8250180848876383,0.0,0.0,19.023639435430148,0.028837946683900717,-0.09007547130751725,0.03681307179770284
+5.2,1.1999045089807792,2.0471631528647043,2.041043042084227,-0.01080624355766566,-0.04330877918708477,0.5737761390312308,0.8177949113463048,-0.7946960854285485,2.1361170964558207,0.24563079549663352,1.2332058103101402,0.38966775968674994,2.8533823235400857,0.0,0.0,18.982911864107354,0.023386787641272888,-0.09274771299338297,0.0325352940713073
+5.22,1.1536677736402476,2.0478280444139494,2.041491014975066,-0.003665407318409133,-0.03319315238145191,0.5975332289876945,0.8011484379811966,-0.6861724690812737,2.2007410773466414,0.18404024115349576,1.2494921321288335,0.31981336201907407,2.878460026762646,0.0,0.0,18.95314722253732,0.01771291307755707,-0.09473654710967438,0.028241614111676847
+5.24,1.1071207210807144,2.0483402876890517,2.041851121998734,0.003987486092648987,-0.02354818359778068,0.6208732647010606,0.7835470452904233,-0.5702306998521411,2.253772380479624,0.121299978563568,1.2606620145434424,0.24848138170450662,2.900244291757431,0.0,0.0,18.934170466666593,0.011886320023953956,-0.09604771665805117,0.023925569999496632
+5.26,1.0603604675804157,2.0486993198987298,2.042123802360951,0.012126856711306986,-0.01440871278131149,0.6437289958546221,0.7650217697806311,-0.4483160897092641,2.294411667673853,0.057878935432373324,1.2666256633061697,0.17628864007724315,2.91872173640097,0.0,0.0,18.925825996663626,0.0059778140446064,-0.09668767644568416,0.01958065057889999
+5.28,1.013483998446879,2.048904922189346,2.042309194675129,0.020723609680518913,-0.005805907296197038,0.6660360371031014,0.7456090267172693,-0.32197493083115303,2.322040174833641,-0.005760948083332232,1.267356939998972,0.10383329075533647,2.9338723987295894,0.0,0.0,18.927977962165777,5.827915767830927e-05,-0.09666447701697867,0.015200282053826435
+5.3,0.9665881887657622,2.048957178431498,2.042407124216687,0.029745531144901072,0.0022329086075847378,0.6877332573022992,0.7253503872587561,-0.19282807955799727,2.3362330891332364,-0.06916606737721086,1.2628933501920214,0.031696808946968635,2.9456696611030044,0.0,0.0,18.94051052952844,-0.005801994764208285,-0.09598831607741298,0.010777814554294716
+5.32,0.9197698218053258,2.0488564394496276,2.042417095917238,0.03915766119543105,0.009684466445480782,0.7087631489203708,0.7042923309335763,-0.06254293753953899,2.3367687604687464,-0.1318925585849337,1.2533352805180855,-0.039554637514180244,2.9540801997996513,0.0,0.0,18.963328218719674,-0.01153392872976955,-0.09467177187990228,0.006306507504580654
+5.34,0.8731256046879892,2.048603292388864,2.042338292418571,0.04892263561874492,0.016529797030925202,0.7290721777713,0.6824859713825961,0.06719557864279618,2.3236335173787657,-0.19350766808409914,1.2388445732849278,-0.10936909498993104,2.959063958597485,0.0,0.0,18.99635637716179,-0.017070214605256595,-0.09272973114221678,0.001779512624416744
+5.36,0.8267521811233117,2.0481985355160117,2.0421695766417267,0.05900098788603061,0.022754495513004215,0.7486111140321333,0.6599867546473863,0.19471445346722635,2.2970219684194864,-0.2535909870594804,1.2196425192324023,-0.17720768169688844,2.9605741427226127,0.0,0.0,19.039541813684192,-0.022345818390737016,-0.09017902188333105,-0.002810146392331256
+5.38,0.780746140991301,2.047643159378873,2.0419094985197765,0.0693514021136024,0.02834900012715859,0.7673353463916356,0.6368541303899161,0.3183817334216543,2.257332783689201,-0.3117354561957698,1.1960073393424382,-0.24254307927847424,2.9585572276332006,0.0,0.0,19.09285356542319,-0.027298431650515376,-0.08703775894660881,-0.007469598031441239
+5.4,0.7352040263816885,2.046938335884741,2.041556305811978,0.0799309066225682,0.033308878081007194,0.7852051821332138,0.6131511973066882,0.4366347979606421,2.2051600643317686,-0.36754805943232555,1.1682712102982515,-0.3048573478181833,2.952952975684599,0.0,0.0,19.15628371184284,-0.031868884896376404,-0.08332440630088345,-0.012206150798816146
+5.42,0.6902223333390551,2.0460854174921645,2.0411079592762076,0.09069499617563713,0.03763511270528821,0.8021861369830048,0.588944324696864,0.5480071312440128,2.141280517068467,-0.42065014784556043,1.1368168598838777,-0.36363875570067616,2.9436944530033404,0.0,0.0,19.229848080986734,-0.03600155001638672,-0.07905655432833991,-0.017027323811472678
+5.44,0.6458975080334739,2.045085949328916,2.0405621529389135,0.10159766890653338,0.04133438602136294,0.8182492196584336,0.564302752725308,0.6511530298728894,2.066636753177132,-0.47067736374549296,1.10207371263607,-0.41837731080518237,2.930708039018837,0.0,0.0,19.31358661136595,-0.03964477646768971,-0.07424940060771268,-0.021940878408268846
+5.46,0.6023259353602554,2.043941697641517,2.0399163407804437,0.1125913612819924,0.04441934999037784,0.8333712172360268,0.539298174476461,0.7448697429367869,1.9823171245237405,-0.5172791697055555,1.0645134966986276,-0.468558663654563,2.9139134220779557,0.0,0.0,19.407563035826488,-0.04275142747041494,-0.06891390738907294,-0.0269548484233431
+5.48,0.5596039170470759,2.042654698532062,2.03916777186373,0.12362676100888896,0.04690887765882368,0.847534988720557,0.5140043035361531,0.8281166158160842,1.8895325916496322,-0.5601180264254231,1.024645124883348,-0.5136560199952427,2.893223576259547,0.0,0.0,19.511863437693805,-0.04527960663912245,-0.06305458672262099,-0.03207756649290881
+5.5,0.5178276351740567,2.041227331422269,2.0383135367944663,0.13465247343796666,0.04882828191921183,0.8607297755371673,0.4884964317020066,0.9000308953720358,1.7895911896793546,-0.5988683062849557,0.9830085327479618,-0.5531196418136844,2.868544716609009,0.0,0.0,19.626593089141274,-0.04719368988293626,-0.056666835380565383,-0.03731768401151545
+5.5200000000000005,0.47709309553584245,2.039662422064362,2.0373506294344246,0.14561451151170565,0.05020948437041197,0.8729515390692123,0.4628509826641379,0.9599399482854384,1.683870716753691,-0.6332150757541183,0.9401669936917569,-0.5863634242501878,2.8397762320800006,0.0,0.0,19.751870808548684,-0.04846579775834425,-0.049733710060780034,-0.042684183232928274
+5.54,0.4374960434155236,2.0379633801335526,2.036276029025951,0.15645557247421135,0.051091109510179304,0.884203336781856,0.4371450692786293,1.0073697432703075,1.5737903170384733,-0.6628529294547028,0.8966972411000053,-0.6127479079922,2.8068105969609487,0.0,0.0,19.887819853365862,-0.04907785371935258,-0.042222008457437676,-0.04818638158260186
+5.5600000000000005,0.3991318419920644,2.0361343763963364,2.0350868093614247,0.16711405636502946,0.051518469991736406,0.894495749784625,0.41145606452229844,1.0420495486022239,1.4607816663303925,-0.6874851121361558,0.8531765245204801,-0.6315589309817126,2.7695332619089332,0.0,0.0,20.03455407458581,-0.04902436708091984,-0.03407752165938349,-0.053833930515043815
+5.58,0.36209530062655876,2.034180564019471,2.033780283388139,0.17752277205689182,0.05154339679043123,0.9038473756118505,0.3858611994592244,1.0639128955501846,1.3462604924881794,-0.7068232233748823,0.8101655350599595,-0.6419809704590863,2.727822524393751,0.0,0.0,20.19215766383374,-0.0483160514131632,-0.025219376361509338,-0.05963681412477881
+5.6000000000000005,0.32648043649154707,2.032108347577936,2.032354193713683,0.1876072671989567,0.05122385385134656,0.9122853999405067,0.3604372055520139,1.073094951790294,1.2315991736775835,-0.7205878655595104,0.768185987123694,-0.643064142742276,2.6815493750194768,0.0,0.0,20.36065627544054,-0.04698434226275028,-0.015533529557241965,-0.06560535594754191
+5.62,0.2923801482064158,2.0299257014797214,2.030806961876912,0.19728371080486728,0.050623260250760374,0.9198462587783036,0.3352600232419656,1.0699265309916144,1.1181011511621963,-0.7285106715196844,0.7276915711101783,-0.6336839278570139,2.6305773108615154,0.0,0.0,20.53997652802045,-0.04508682757578775,-0.004865756075071503,-0.07175024548315612
+5.64,0.259885774071692,2.0276425365128827,2.0291380118987257,0.20645625463861308,0.04980942429247553,0.9265763963486032,0.31040460354492866,1.0549250273210877,1.0069778644773684,-0.7303382367356319,0.6890310103902176,-0.6124931373117065,2.574762100300284,0.0,0.0,20.729889733104127,-0.04271357477343107,0.006985082998201442,-0.07808259852774752
+5.66,0.22908649988742938,2.025271108606324,2.0273481862621696,0.21501380691405947,0.04885297344700438,0.9325331102798743,0.28594483393384373,1.0287825957925836,0.8993288582387484,-0.7258385835739649,0.6524020529262844,-0.5778666614954622,2.513951477210349,0.0,0.0,20.92993383644941,-0.03999434706276017,0.02027206731502673,-0.0846140682133701
+5.68,0.20006857192998947,2.022826457150304,2.0254402744616526,0.22282617590719575,0.04782514191351383,0.937785450063387,0.26195362333996897,0.9923518892246176,0.7961256146165512,-0.7148108991944944,0.6177953724648431,-0.527841358812956,2.4479847370386376,0.0,0.0,21.13930443109181,-0.037106734239650496,0.03530130341711679,-0.09135702968842911
+5.7,0.1729142592216093,2.0203268507112195,2.0234196743241775,0.2297395905579262,0.046794754826817955,0.9424150907051176,0.238503182918466,0.9466296013977282,0.6981995288515499,-0.6970994069457337,0.5849275387321019,-0.46005631046299883,2.3766922029418422,0.0,0.0,21.35670090179721,-0.034285223394837194,0.05242588120042152,-0.09832487102361498
+5.72,0.14770049568519264,2.017794204969387,2.0212952021781896,0.23557169051245846,0.04582422749300774,0.9465170333331453,0.2156655383087271,0.8927379567336458,0.6062342662945551,-0.672612333953737,0.5531625112362023,-0.3717027458767676,2.299894527081041,0.0,0.0,21.580108613381093,-0.031830942557951714,0.07202850357898277,-0.10553241143267444
+5.74,0.1244971176025976,2.015254420411483,2.019080056287272,0.2401062117039739,0.04496438758027576,0.9501998802108411,0.19351330341348694,0.8319041560314347,0.5207625231846071,-0.641346974137237,0.5214219401679777,-0.25949831857117966,2.2174018108163938,0.0,0.0,21.80648606234112,-0.030119995904925867,0.0944882076887597,-0.11299655337015832
+5.76,0.10336459593390626,2.012737564889002,2.0167929128304363,0.24308778470613285,0.044247958888987735,0.9535852881773237,0.1721207286546094,0.7654376998521049,0.4421669696040866,-0.6034218087925647,0.48808801339167873,-0.11970627560323568,2.129012497224006,0.0,0.0,22.031451380696335,-0.029601752642732126,0.1201206544737955,-0.1207350405990617
+5.78,0.08435116158221188,2.0102777981465576,2.014459111985072,0.24421759787696332,0.043681692896926444,0.9568060063469387,0.1515649722438736,0.6947055957876909,0.3706848817608944,-0.5591151378704142,0.4509150980053646,0.051766566531574604,2.034513691089236,0.0,0.0,22.248615823258532,-0.03079107779122059,0.14908524438644027,-0.12876871006458587
+5.8,0.06748920111002012,2.0079129081493834,2.0121117862501,0.24315107360769653,0.04323727885882803,0.9600016777057218,0.13192752526485063,0.6211057789116033,0.3064155850570947,-0.5089121832080591,0.40696214491798277,0.2591506886391277,1.933680524098913,0.0,0.0,22.448836730861263,-0.03423509135295966,0.18123955931700125,-0.13712204307848364
+5.82,0.052790828216775235,2.005683301108188,2.0097926955946956,0.23949940113202653,0.04284157316517555,0.9633113415591502,0.11329561196111496,0.5460398643574069,0.2493298131234022,-0.45355849108586394,0.35258529882653,0.5064062413155793,1.8262762742789167,0.0,0.0,22.619229287365073,-0.04044019791902009,0.2159314638189232,-0.14582456518225315
+5.84,0.040242593598036565,2.0036302731956526,2.007552375259223,0.23283763095917911,0.04236736773168452,0.966861438295891,0.09576326486292351,0.47088767967416134,0.19928009685705347,-0.39411722465630067,0.28355104026661004,0.7965476769611324,1.7120530732147425,0.0,0.0,22.742154662068145,-0.04975265658133088,0.2516851377293202,-0.15490998685699478
+5.86,0.029799441316327235,2.0017934142990383,2.005449035755677,0.22272350494666968,0.04162673109018765,0.9707481984664464,0.07943167363945401,0.3969880425540927,0.156011807746212,-0.3320234327232464,0.19534077146274403,1.1305668039948626,1.5907553686406377,0.0,0.0,22.793921948296013,-0.06209225853862099,0.28585656261074643,-0.16442013993978
+5.88,0.021378252238055406,2.00020706487295,2.003545399366091,0.2087319725126824,0.04037133394014528,0.9750142132534833,0.06440809732129225,0.32563198544414795,0.11917596049128952,-0.2691281843787391,0.0838539632184514,1.505916822620113,1.4621226309632516,0.0,0.0,22.743881691273092,-0.07665795623059624,0.3139889396907984,-0.17440615928917957
+5.9,0.014851721534236549,1.9988959514579288,2.001902550413921,0.1905142781113701,0.03830481390490193,0.9796203894979043,0.05080298758329584,0.25807699943856655,0.08834586644893719,-0.2077161064272749,-0.05344378481503345,1.9141853712839498,1.325892537905797,0.0,0.0,22.554604216117035,-0.09123988326481736,0.3290296405191221,-0.18492759010349147
+5.92,0.010043926041649949,1.9978704229605457,2.0005699765898584,0.16789278268655566,0.035118083308032164,0.9844181764617261,0.03872450901308811,0.19558659346488566,0.06304331769822005,-0.15047286773331942,-0.21539295242877923,2.3377985148598364,1.1818031972040086,0.0,0.0,22.1856972372209,-0.10142307682501106,0.31954271064891004,-0.19602529933771368
+5.94,0.006729695614413436,1.99712222736846,1.999571829742075,0.14101419056187517,0.030560011232449405,0.9891318604096482,0.02827094791831651,0.1394919015071433,0.04277760563931368,-0.10035794085197994,-0.39389416370080177,2.744576947809456,1.0296054503231842,0.0,0.0,21.605752202254802,-0.09955461387159992,0.2693906565984273,-0.20764176752623678
+5.96,0.004640712482444695,1.9966223089423412,1.998891589488236,0.1105626164017985,0.02455862529252359,0.993373887368264,0.019521825554842404,0.09124245275105462,0.02709711093192845,-0.06030143304647963,-0.569394682245811,3.082195100171662,0.8691174326288605,0.0,0.0,20.79239299183078,-0.07347083868008131,0.15352449747944769,-0.21979680865097811
+5.98,0.003479828069386919,1.9963222156572584,1.9984572181240863,0.07807202093768191,0.017402983586605924,0.99671700769327,0.01253404499610986,0.0523901787848524,0.015636672987310488,-0.032837641570781756,-0.7053390251328062,3.268320864524301,0.7001326292021245,0.0,0.0,19.767893089586973,-0.006628627618305849,-0.06363908696639775,-0.2323636231095203
+6.0,0.0029454047149399037,1.9961605855431919,1.9981403429141085,0.04630372681097297,0.009947081741077711,0.9988508613061422,0.007346925631106359,0.024377025221913436,0.00811116587081736,-0.019253632177605354,-0.7463139591028505,3.1833228608207564,0.5227025210296768,0.0,0.0,18.859639173855093,0.09940229662978983,-0.39467972505209825,-0.24119712919628844
+6.0200000000000005,0.002765424143353381,1.9960751857704766,1.9978323095861112,0.019167908292807827,0.0034557213502329075,0.9998024200843382,0.0039711553896272485,0.007923309129780159,0.004155669722973686,-0.016014584455310135,-0.6537281875362669,2.713816080364414,0.3409983461056387,0.0,0.0,18.782387156127587,0.21210093808219788,-0.8173661686586461,-0.23690746824789427
+6.04,0.0027187462652102476,1.9960130255143576,1.9975619630208303,0.001709417558514515,-0.0007187356894446683,0.9999955264665803,0.002346989017082791,0.002031656295942921,0.003084830749777505,-0.013572118867902865,-0.4212726201881396,1.744872284057725,0.1644385300357949,0.0,0.0,19.625641231141206,0.3607931153740418,-1.4418920901564056,-0.22164576978643474
+6.0600000000000005,0.0026554328530712648,1.9959377533912808,1.9975247571510324,0.0016602319389490128,-0.0007389451312452054,0.9999956048248881,0.002342631193352069,0.003141702396159369,0.003781111275874072,-0.0018654214367929498,-0.002031728385216758,0.004913497004097822,0.0004427885978891821,0.0,0.0,10.594326710729092,-1.37727847924261,-0.8501082672700033,-0.1711354472147628
+6.08,0.002575933387592718,1.9959057634491715,1.9962142553422206,0.010572640157522305,-0.01755365540745269,0.9997838403007046,0.003515840654989765,0.003160543952008314,0.002819353185108936,-0.06553229069961815,-1.679688003551399,-0.8951786817697706,-0.11524396891225017,0.0,0.0,9.51659064497822,-0.8488065448193299,-0.47830024606567856,-0.18421470053461825
+6.1000000000000005,0.0025092722473512635,1.9958629824284955,1.9933372181480584,0.024716591661040017,-0.044832634980182785,0.9986710389078143,0.005939779809211699,-0.0018023968661845884,0.01111785501653128,-0.14348776065091148,-2.727284052752327,-1.4208271764309885,-0.23875734094594275,0.0,0.0,9.970145331246004,-0.4713539704680362,-0.1946567907327227,-0.19691338248080403
+6.12,0.0025260119646469434,1.9956780759396011,1.9889362556878687,0.04096994881727714,-0.07791462172590469,0.9960700933299481,0.009754188274127893,-0.015575247004524844,0.036003438940809616,-0.2167732817541586,-3.315999374934077,-1.6325288049320852,-0.37366545897266473,0.0,0.0,10.739825128106176,-0.22156298748320247,-0.021952418344009045,-0.20352396797030284
+6.140000000000001,0.002718686413776057,1.9951619789217871,1.983089063110715,0.05731615591429593,-0.11362097116431545,0.9917547390837336,0.015088760314937343,-0.03924695074113438,0.08034160394949857,-0.2797903956483685,-3.59708973004568,-1.6395756142682263,-0.5167790548798193,0.0,0.0,11.600879958867713,-0.04490513213399272,0.09328140886570555,-0.206955518997323
+6.16,0.0031863083880545283,1.9940867829398403,1.975863141456467,0.07240444818210395,-0.14969509060107195,0.9858306766588729,0.022064738649949966,-0.0723575374372421,0.1438806022465055,-0.3287918536737266,-3.663179616837401,-1.5059174172235332,-0.6656352174542737,0.0,0.0,12.527542972875622,0.08709189264195538,0.1706152339314256,-0.2063344849236709
+6.18,0.004022451514262627,1.9921993799790456,1.9673139258113204,0.08536988669312585,-0.18448326536517098,0.978637215714489,0.03077510787632606,-0.11368764792401204,0.22433953988463418,-0.36085254149281115,-3.572563332404235,-1.276946322467716,-0.816976554675675,0.0,0.0,13.526090490902035,0.19553082831633029,0.22134765968575137,-0.2007237425998143
+6.2,0.00530778056989916,1.9892282284535976,1.9574959625890966,0.09570053579094866,-0.21663790843094582,0.9706736337672938,0.041256766584260245,-0.16184162454740558,0.31822786224032484,-0.3739308560686301,-3.352111017753502,-0.9847836393441572,-0.9665731801922017,0.0,0.0,14.414461793886016,0.27316480387538966,0.2466436979140057,-0.1929000796450307
+6.22,0.007099960228508782,1.9849031701634834,1.9464495982123147,0.10319510750596432,-0.24512987710387707,0.9624969392599908,0.05349537412730255,-0.21560866501301162,0.42136099931334237,-0.36868398156715215,-3.029530085654834,-0.6582627331947949,-1.111595877849576,0.0,0.0,15.111246642626774,0.31663020133747555,0.25157455169875526,-0.18436037877042333
+6.24,0.009426531381034525,1.9789786549152277,1.934191868186672,0.10789622371726294,-0.2693272803977547,0.9546057323860846,0.06744713957411855,-0.27425216012217646,0.5295999076725992,-0.3483193769547017,-2.6416369578618846,-0.32140616252082493,-1.2507807305873144,0.0,0.0,15.604616079386984,0.3320014523582986,0.2409789731462627,-0.1757166772524511
+6.26,0.012283440037862085,1.97124877584085,1.9207186797659308,0.11002861175805138,-0.2889507147711626,0.9473671818498283,0.08304584144762847,-0.3375774216024227,0.6394793957921519,-0.31749330519327873,-2.222393818421021,0.006101629043159917,-1.3837426159606854,0.0,0.0,15.924854582131811,0.3275653702134703,0.2203005967936237,-0.16702558123343508
+6.28,0.015638635859189738,1.9615522501551594,1.9060135875726931,0.10993261865511776,-0.30397634929217404,0.940995529076999,0.10020285766249042,-0.405827360119001,0.7483967942216074,-0.2812326724142669,-1.797330091510294,0.3099881952834179,-1.5103557938266579,0.0,0.0,16.13661970264307,0.3075837257015961,0.19018980676847946,-0.15780965217654536
+6.3,0.019438265417655066,1.9497699515906377,1.8900586628803469,0.10803947254014706,-0.3145796622596411,0.9355502966788394,0.11879709950159824,-0.47947484378783034,0.8544799493018166,-0.2441364327291432,-1.3867840128492979,0.5773033694875715,-1.6302939940017098,0.0,0.0,16.226474939946293,0.28125617653014345,0.16193233473870364,-0.14888938936604212
+6.32,0.023612812993445413,1.9358240833709601,1.8728354965861969,0.10476317740292924,-0.32102861817781386,0.9309820193591172,0.13870033381829405,-0.5590485717975952,0.9563679644486135,-0.21049147107814714,-1.0010742126606913,0.806725113201463,-1.743714123819272,0.0,0.0,16.237709449743164,0.2503784937307304,0.13520613674743476,-0.14006353975004326
+6.34,0.02808315815563785,1.9196728779190193,1.8543298073391252,0.10048986585148005,-0.32365685385942033,0.9271565930904436,0.1597769686153008,-0.6449177234752063,1.0529457499709818,-0.18383052326386773,-0.6476198891381584,0.9980083303366256,-1.8506851295799671,0.0,0.0,16.191409017452514,0.21744523863746001,0.11165386837887706,-0.1314304943057376
+6.36,0.03276563127888043,1.9013059297330233,1.8345327870648647,0.0955555415916126,-0.32283195400153275,0.9238854418251964,0.18189106171042993,-0.7372076908823693,1.1431711320447377,-0.16695852658259214,-0.33048418534753965,1.153636050472468,-1.9513454738275868,0.0,0.0,16.106182202064282,0.1840779965848216,0.09190253802266121,-0.1230549290723171
+6.38,0.03757611115893462,1.8807394861162061,1.8134416185308961,0.09023634979124004,-0.31893487581409335,0.9209503016539552,0.20491092711051476,-0.8357557232341175,1.2259621829157212,-0.16194914876384758,-0.05143422764519423,1.2775486043031898,-2.0458761185491547,0.0,0.0,15.998481839330568,0.15131582130777577,0.07592211580728683,-0.11496789129295881
+6.4,0.04243319965275134,1.8580119147476464,1.7910595740815234,0.08474743426484148,-0.3123468913856655,0.9181222042777443,0.22871141169476786,-0.9401090054801222,1.300151418023285,-0.17017139547365306,0.189302418785689,1.3742059971690206,-2.134474583832632,0.0,0.0,15.882480297564882,0.11983183109933904,0.0633298863914287,-0.10717674229970116
+6.42,0.04726056734667027,1.8331795119952,1.767395911413976,0.07924678095571819,-0.3034411336751331,0.9151754354656116,0.2531745414184345,-1.0495478280760138,1.364493342042882,-0.19233720868124546,0.3925379056450891,1.448010147018265,-2.2173379060379026,0.0,0.0,15.769951111908798,0.09007077745653502,0.05359287055195991,-0.09967525143387874
+6.44,0.05198858812439886,1.806312753358189,1.7424656830966585,0.07384156274688068,-0.29257683078064023,0.9118974518580697,0.27818925032469305,-1.1631208289256922,1.4177091972737652,-0.2285645686060699,0.5598000511343091,1.5029973103529048,-2.2946541998628254,0.0,0.0,15.670288674614802,0.06232786653825084,0.04614452497404892,-0.09245072161601756
+6.46,0.0565553832193662,1.7774930312663884,1.7162895202951929,0.06859550488068929,-0.280095151789342,0.9080955966530451,0.3036507039241285,-1.2796844147233422,1.45855343315968,-0.27845241993793457,0.6931143471757581,1.5427007599476625,-2.366599881471663,0.0,0.0,15.5906571982717,0.03679208426971092,0.04044515333442744,-0.08548806303417306
+6.48,0.060907386169347114,1.7468098841398383,1.6888934155232858,0.06353645444234068,-0.26631613527492826,0.9036012725144753,0.3294595200617875,-1.3979424096029618,1.48588851337817,-0.3411641809150079,0.7948467444344854,1.570110314436772,-2.4333397285540825,0.0,0.0,15.536211845791648,0.01357132163500907,0.036010702667027994,-0.07877186388917076
+6.5,0.06499952596912931,1.7143586949755794,1.6603085055683429,0.058663707213379684,-0.2515364645377675,0.8982720986878389,0.35552104464119805,-1.516484449598961,1.4987573830557668,-0.4155164200371329,0.8675827881208772,1.58768448655003,-2.4950280185217704,0.0,0.0,15.510352544397591,-0.007291032884172877,0.03242451557717008,-0.07228730158561208
+6.5200000000000005,0.06879511105492116,1.6802388229275602,1.6305708428079986,0.05395484204970933,-0.23602798699850067,0.8919924941614576,0.3817447507595204,-1.6338229335281382,1.4964458208312443,-0.5000689095109747,0.9140310565654364,1.5973901026059765,-2.5518098366956807,0.0,0.0,15.51498917179968,-0.025803360541772014,0.02933985201938283,-0.06602041212616291
+6.54,0.0722654836421191,1.6445521232769305,1.5997211393058985,0.04937192116158949,-0.2200369221041412,0.8846730694534242,0.40804378012121023,-1.7484288355203608,1.4785297284198733,-0.5932120797856582,0.9369450687341323,1.6007551198587824,-2.6038221569457916,0.0,0.0,15.550802981221882,-0.042014683462958446,0.02647786618074499,-0.05995807346715987
+6.5600000000000005,0.07538950302103724,1.6074018073958234,1.567804470469187,0.04486697552181615,-0.20378370155643458,0.8762491618435855,0.4343346219862013,-1.858766701197733,1.4449049522156556,-0.6932481254032267,0.9390609530990339,1.5989270304511243,-2.651194609762185,0.0,0.0,15.617497068811112,-0.05600452821471802,0.023623343192872015,-0.05408786290687845
+6.58,0.07815290749693016,1.5688915931896812,1.5348699312982474,0.0403867348974717,-0.18746336807806074,0.8666788028136341,0.4605369149445862,-1.9633289159336293,1.3957993251175638,-0.798462627694587,0.9230491909196463,1.5927326227817455,-2.6940499711593535,0.0,0.0,15.714030383905818,-0.06787382858009627,0.020619427172984484,-0.04839787389001608
+6.6000000000000005,0.08054759726588546,1.5291250974190307,1.5009702458966767,0.03587659222458554,-0.17124643953350097,0.8559403592285162,0.48657335369007676,-2.060669012620683,1.3317681812189506,-0.9071844735547925,0.8914789051385368,1.5827367021859324,-2.7325044428135965,0.0,0.0,15.838830555342003,-0.07773715870033548,0.01736193655194518,-0.042876538197464543
+6.62,0.08257087417050578,1.4882054239123383,1.4661613380234628,0.03128381675513632,-0.1552801323124122,0.8440300418665139,0.5123696826951041,-2.1494334834270625,1.2536756237784161,-1.0178329036100155,0.8467929729006433,1.5692982961715565,-2.7666677928719987,0.0,0.0,15.989981467689864,-0.08571658107944091,0.013793503056486467,-0.03751248061342274
+6.640000000000001,0.08422466862483638,1.4462349056981605,1.4305018758021932,0.02656005102812518,-0.13968983483824465,0.8309594283744067,0.5378547593244168,-2.2283913377865336,1.1626643737396212,-1.1289515566822848,0.7912919919976301,1.552623292509954,-2.796643414020315,0.0,0.0,16.16538261833279,-0.0919372518256063,0.009897585535229791,-0.03229442171640172
+6.66,0.08551477882640704,1.4033149641418674,1.3940528066722429,0.021663143358580432,-0.12458072888706792,0.81675310499473,0.5629606697552327,-2.2964605374141525,1.0601171881988987,-1.2392302608559447,0.7271249913702487,1.5328126331427494,-2.822528341648367,0.0,0.0,16.362878872997904,-0.09652472731967476,0.0056923433972813076,-0.027211134446703034
+6.68,0.08645014258720729,1.3595460538202715,1.3568768992815714,0.016558379965278792,-0.11003947090110808,0.8014464944270909,0.5876228820146953,-2.3527304443029093,0.9476127271176271,-1.3475159750964796,0.6562848231989489,1.5099052754125155,-2.844413262903763,0.0,0.0,16.58036105838654,-0.0996037249892,0.0012243651405514089,-0.022251450523020737
+6.7,0.08704215768585734,1.3150276676360322,1.3190383077042007,0.011219188999814604,-0.09613586438243177,0.7850839070494533,0.611780421613502,-2.396479527865723,0.826878467281136,-1.452814683125362,0.5806064232945933,1.483915232383146,-2.8623825364910647,0.0,0.0,16.81583951548521,-0.10129795348338677,-0.0034377015067034167,-0.017404305089740887
+6.72,0.08730406257363038,1.2698583821528664,1.2806021707482593,0.005627392866438887,-0.0829244744141636,0.7677168296459258,0.6353760567341753,-2.4271877695370847,0.6997428995188191,-1.5542862010555956,0.5017665349887067,1.4548621614565131,-2.876514232702265,0.0,0.0,17.067493976354847,-0.10173056973257233,-0.008209510767947736,-0.012658803871849492
+6.74,0.08725038557349722,1.2241359280310067,1.2416342558486742,-0.00022691410757066601,-0.07044615372984606,0.7494024492412767,0.6583564817071632,-2.444543447944236,0.5680888572524863,-1.6512338278564929,0.42128397951042634,1.422795190386669,-2.886880193933437,0.0,0.0,17.3337037766796,-0.10102484199843231,-0.012997215915440563,-0.008004297088631982
+6.76,0.08689646741485195,1.1779572746037485,1.2022006536453913,-0.006345787680399134,-0.05872946492754861,0.7302023986536947,0.6806724894752467,-2.44844425453671,0.4338094465410452,-1.7430905995714545,0.3405200341071961,1.387809931944069,-2.89354610911944,0.0,0.0,17.613062506002905,-0.09930467851864311,-0.017703321918504433,-0.003430447506237564
+6.78,0.08625805906842667,1.1314187210198374,1.1623675262006237,-0.012724084565922838,-0.047791994801959406,0.7101817038024825,0.7022791257676764,-2.4389929502916203,0.29876769729822394,-1.8294036587813893,0.26067887705165294,1.3500589119589108,-2.896571592111609,0.0,0.0,17.904380869596476,-0.09669479115107715,-0.02223201377271783,0.001072715385126313
+6.8,0.08535099440303562,1.0846159890216225,1.1222009091128002,-0.01935027695089022,-0.03764156447803508,0.6894079095455665,0.7231358196504871,-2.416488006186681,0.1647607387947594,-1.9098179652363942,0.18280833771734037,1.3097558900868944,-2.8960102541531807,0.0,0.0,18.20668096590283,-0.09332037525935866,-0.026493802545656808,0.005514758587804365
+6.82,0.08419093518461665,1.037644314416583,1.0817665656199762,-0.026207371978042286,-0.028277343612172587,0.6679503599289105,0.7432064868951849,-2.381409857807629,0.0334890210294945,-1.9840602854743117,0.1078013440630497,1.2671747655831278,-2.891909763479114,0.0,0.0,18.519184524309047,-0.08930628250128059,-0.030409367672337484,0.009904826321709654
+6.84,0.08279318436963826,0.9905985357263255,1.0411298891460643,-0.033273884546902194,-0.01969087905731116,0.6458796094515841,0.7624596041652166,-2.334403539000229,-0.09346914384559643,-2.0519241313428913,0.036398506166521086,1.2226439183511442,-2.8843118893611144,0.0,0.0,18.841297007223968,-0.08477573358660895,-0.033912518059224905,0.014251636363110932
+6.86,0.0811725624835392,0.9435731794573713,1.0003558495751914,-0.040524818948546225,-0.01186704871793144,0.6232669437062985,0.7808682533481069,-2.276258538391478,-0.21467766042718453,-2.11325608335866,-0.030807758782772296,1.1765369339309115,-2.873252532275698,0.0,0.0,19.17258892186386,-0.0798486623207929,-0.036952240215689214,0.018563489309469168
+6.88,0.0793433410799877,0.8966625420177264,0.9595089777662181,-0.04793262183088218,-0.004784950404634388,0.6001839901075744,0.7984101364791117,-2.2078867483240447,-0.32885747253168973,-2.167943741465588,-0.09336723711877327,1.1292607017501735,-2.8587617452502165,0.0,0.0,19.512775242171106,-0.07463980340422571,-0.039493844645834164,0.022848288108916155
+6.9,0.07731922679805435,0.8499607685761158,0.9186533823601216,-0.0554680755136796,0.0015812662185624203,0.5767024020404828,0.8150675626453872,-2.130299354191743,-0.43489640458227563,-2.215905394449681,-0.15096384703841106,1.0812418648847186,-2.8408637532687413,0.0,0.0,19.861693520488526,-0.06925663914224456,-0.041519259308549494,0.027113565626148944
+6.92,0.07511338931766182,0.8035619291885656,0.8778527927143911,-0.06310110650359319,0.00726161866895084,0.5528936024100916,0.8308274090726188,-2.0445834513973966,-0.5318544195326239,-2.2570813877386815,-0.2034053730152132,1.0329125450500334,-2.8195769777892337,0.0,0.0,20.21928107037264,-0.06379730910854908,-0.043026549486603116,0.0313665185380327
+6.94,0.07273852650641552,0.7575600923499782,0.8371706217632977,-0.07080148899983811,0.012289558083867487,0.5288285750722024,0.8456810593271041,-1.9518790862822581,-0.6189647844948932,-2.2914270928547893,-0.25061098672527987,0.9846961778650611,-2.7949140721827486,0.0,0.0,20.58555151770903,-0.05834856597007121,-0.04402876629042651,0.03561404627189422
+6.96,0.07020696020686132,0.7120493958059881,0.7966700427139974,-0.07853942706264966,0.01670128179786731,0.5045776948633608,0.8596243222648811,-1.8533573056959678,-0.695631701492935,-2.318907334364538,-0.2925966021242322,0.9369941798670995,-2.7668819717119586,0.0,0.0,20.96057103434919,-0.052983833370966016,-0.044552241633037615,0.039862793944137206
+6.98,0.06753075639525774,0.6671241140119876,0.7564140737323485,-0.08628600179297588,0.020535092895574054,0.48021058885379153,0.8726573360758778,-1.7501996762773524,-0.7614250015929007,-2.3394921053680027,-0.32945814778724675,0.8901740386033208,-2.735481959048447,0.0,0.0,21.34443467063296,-0.04776139030958748,-0.044634457313956655,0.04411919833735604
+7.0,0.06472186382471481,0.6228787210707816,0.716465665159449,-0.0940134712051027,0.023830809313793704,0.45579602299287497,0.8847844625575743,-1.643579607095941,-0.8160725184160638,-2.3531533943070686,-0.3613528390855522,0.8445592765091747,-2.700709743776182,0.0,0.0,21.737243369932827,-0.042722672006471174,-0.0443216224400812,0.04838953592757202
+7.0200000000000005,0.061792265731676566,0.5794079473259311,0.6768877843631115,-0.10169541031267951,0.026629226270300645,0.4314018095108797,0.8960141776670275,-1.5346456851770303,-0.8594507545108798,-2.359862948914612,-0.38847849302613413,0.8004215908951288,-2.662555552224922,0.0,0.0,22.139082453167195,-0.03789063959822392,-0.04366610031364748,0.052679971908064185
+7.04,0.05875413973000006,0.5368068270296222,0.6377434941199756,-0.10930667709642096,0.028971636259212348,0.40709473132443186,0.9063589655111771,-1.4245071187154768,-0.8915744344494123,-2.359590813418539,-0.41105084566098027,0.7579753143604469,-2.6210042225688897,0.0,0.0,22.550002569272635,-0.03326812816742164,-0.04272383697870408,0.05699660910553876
+7.0600000000000005,0.055620021643760106,0.4951707336199877,0.5990960215104708,-0.11682318619581455,0.030899410286494823,0.38294048034188855,0.915835224331609,-1.314221282702535,-0.912585505356861,-2.3523044899818277,-0.4292786989560142,0.7173741655899749,-2.5760352995320885,0.0,0.0,22.970004274244125,-0.028836031044777055,-0.04155196196421507,0.061345535666913774
+7.08,0.052402969743688646,0.4545953980868925,0.5610088157663261,-0.12422146595040173,0.03245364202734964,0.3590036070881786,0.9244631948401031,-1.2047832807436176,-0.9227421027358493,-2.3379685936331702,-0.4433365288393074,0.6787100590411651,-2.5276231232888975,0.0,0.0,23.399027471606487,-0.024551113492623733,-0.04020675858460909,0.06573287038482856
+7.1000000000000005,0.04911672665258993,0.4151769045943282,0.5235455954188738,-0.1314779652114566,0.033674852649555484,0.3353474796096905,0.9322669236234405,-1.0971173788887612,-0.9224079510651599,-2.3165448920695613,-0.4533339103789979,0.6420135012815221,-2.475736908222483,0.0,0.0,23.836946855597063,-0.02034316696813243,-0.03874223439858393,0.0701648044566145
+7.12,0.045775877053202574,0.3770116558236555,0.48677038753674573,-0.1385680633474109,0.03460274753101152,0.31203425034658633,0.9392742774579196,-0.9920701354333523,-0.9120426191132504,-2.28799264989466,-0.45928073566493255,0.6072548122658596,-2.4203408090995904,0.0,0.0,24.28357416850919,-0.016111116145406168,-0.037209556027590907,0.07464763823964145
+7.140000000000001,0.04239600021132925,0.34019629818511754,0.4507475648938202,-0.14546471908497072,0.03527600630721522,0.28912483074744744,0.9455170284695055,-0.8904050467933093,-0.8921930034934097,-2.2522692352386615,-0.46104669041761487,0.5743450704387203,-2.3613939749711714,0.0,0.0,24.738667425898736,-0.011717583387809372,-0.03565763636810848,0.07918781111002376
+7.16,0.03899381709902338,0.30482759377695534,0.41554189065264413,-0.15213667020440313,0.035732073932638865,0.2666788749987513,0.9510310352985277,-0.792798553680468,-0.8634863740672162,-2.209331001575699,-0.4583128031319693,0.5431353040761085,-2.2988505947500295,0.0,0.0,25.201946171117218,-0.006981323905868919,-0.03413516295135188,0.08379192192022486
+7.18,0.03558733235663311,0.2710022212466584,0.38121858466538244,-0.15854606581366523,0.03600690218115102,0.24475477642617752,0.9558565516995203,-0.699837300879707,-0.8266252821782569,-2.1591345374522124,-0.4505121048801155,0.5134120699308378,-2.2326599428391347,0.0,0.0,25.67311016749108,-0.0016669197635188482,-0.032694318635738906,0.08846673714466219
+7.2,0.032195971131966594,0.23881648079404708,0.3478434308572096,-0.16464537595595827,0.03613456799221372,0.22340968284175855,0.9600387006703855,-0.6120166108953359,-0.782384602831966,-2.1016384922813636,-0.43675562630599746,0.48488722962926456,-2.1627664378063653,0.0,0.0,26.151856577750202,0.004528748149920325,-0.03139635371432847,0.09321918443465882
+7.22,0.02884070948004772,0.2083658684203789,0.3154829514046568,-0.1703733815331016,0.03614666756447568,0.20269954011014654,0.963628157396874,-0.5297402047312813,-0.7316109409402016,-2.0368063486566146,-0.41573932152965476,0.45717953730403793,-2.0891097303238557,0.0,0.0,26.63788746169556,0.011993297220316057,-0.03031900251330788,0.09805633240411316
+7.24,0.02554419379617087,0.17974446977836014,0.2842046804806223,-0.17565000646801104,0.03607135428379985,0.18267917597949085,0.9666820838783423,-0.45332126535104356,-0.675224551429793,-1.9646107299139104,-0.38562746079649457,0.4297857061830629,-2.011624835032235,0.0,0.0,27.130895192777196,0.021217991269687708,-0.029565467402974388,0.10298536497335459
+7.26,0.022330838748098863,0.15304410282805875,0.2540775778452098,-0.18036972563772363,0.035931857221371176,0.1634024379462852,0.9692653439503579,-0.38298497010736204,-0.614223756049982,-1.885040106793974,-0.3439093072857197,0.40203909743266,-1.9302423086236384,0.0,0.0,27.630508004716297,0.03281648260268133,-0.02927426209801176,0.1080135724607052
+7.28,0.019226883171777016,0.12835310883659223,0.22517262949129924,-0.18439328909388578,0.03574429069059168,0.1449223984372455,0.9714519849438579,-0.31887259451453775,-0.5496915171453229,-1.7981090810013638,-0.28722970135871745,0.3730553325249174,-1.8448884438910933,0.0,0.0,28.136171151773418,0.04753717557483195,-0.029628509430914884,0.11314840269919069
+7.3,0.01626036797773351,0.10575465090243959,0.19756368650003,-0.1875375854706562,0.035514554920435815,0.12729163643198624,0.9733268771173981,-0.26104718845802166,-0.4828032635325443,-1.7038737229957293,-0.21120243540709266,0.3416663878654833,-1.7554853840004447,0.0,0.0,28.646930778056067,0.06625793255504964,-0.030862160085625145,0.11839764184669722
+7.32,0.013460977820069645,0.08532432660703418,0.17132858915990945,-0.18956369247582122,0.035234152447580096,0.11056259529149995,0.9749872171052505,-0.20950062909665837,-0.41483416655615696,-1.6024536168080705,-0.11023093820677701,0.30634873968258647,-1.6619509479451808,0.0,0.0,29.161076831940896,0.08994346164126732,-0.0332588366471579,0.12376981927950079
+7.34,0.010859658103909348,0.06712683721683495,0.14655060143898196,-0.1901636392525086,0.034874836261022175,0.09478800115355748,0.9765432765317275,-0.16416155580919428,-0.3471627786907573,-1.4940621294938308,0.02261189445052203,0.26515781168503894,-1.5641977839514087,0.0,0.0,29.675586677724134,0.11953299974155462,-0.037136453213837184,0.1292749377754447
+7.36,0.008487881392965862,0.05121138586477306,0.1233201234554594,-0.18894730774828086,0.03438221304836533,0.080021306321592,0.9781172572116081,-0.12490331100671878,-0.28126631069367614,-1.3790457103529332,0.195547797769294,0.21569144903158932,-1.4621312311664987,0.0,0.0,30.185289277874173,0.1557074235525986,-0.04280700489863343,0.13492557677250863
+7.38,0.006376396374868791,0.037605417995795046,0.10173652759343209,-0.1854324975461197,0.033668822429188755,0.06631710311064945,0.9798383749808187,-0.0915506168563724,-0.2187010593271698,-1.257931438167146,0.41773332414236103,0.15512097300163355,-1.3556450119862669,0.0,0.0,30.681635294214942,0.19845999492870314,-0.05049175101837455,0.14073817041563977
+7.4,0.004553257659095049,0.026306306981120964,0.08190973433459264,-0.1790438717929125,0.03260795850621367,0.05373143607058676,0.9818314243256082,-0.0638834524008726,-0.16106011737281442,-1.1314794091171179,0.6983025747396674,0.08035650759473473,-1.2446137757406075,0.0,0.0,31.15090624772478,0.2463664069450049,-0.060135643331768907,0.14673352888917127
+7.42,0.0030409219366444172,0.017270703327813733,0.06396075387811843,-0.16913082256368803,0.031031119756319173,0.04232186661369148,0.9841954552180843,-0.04163655965025201,-0.10990037742796584,-1.0007331855505253,1.044393715163259,-0.01151094745493321,-1.1288820634971297,0.0,0.0,31.57165681478071,0.29542256648397247,-0.07097111808088441,0.15293467091015833
+7.44,0.0018522436913296879,0.010401651061489606,0.04801983372726073,-0.15502082436941558,0.02873574843663843,0.03214686003404414,0.9869697969864957,-0.024492693591281423,-0.06663321359475635,-0.8670584004841498,1.4575586906199884,-0.12215034310361189,-1.0082509876715249,0.0,0.0,31.911378125076965,0.3372628317864961,-0.0806944993663734,0.15936130702211337
+7.46,0.0009854542324235398,0.005534466020767472,0.034220151114019506,-0.13613441146598584,0.025515702226768022,0.02326383873687466,0.9900884630990856,-0.012065753229312665,-0.03237929532715097,-0.7321574096717962,1.9276987298961525,-0.2497980870979821,-0.8824658299355848,0.0,0.0,32.12348338909857,0.35643157558606364,-0.08458326810668138,0.1660239805821349
+7.48,0.00041891967950733226,0.0024240926235593934,0.022684622590646755,-0.11220214904872222,0.021228444203215242,0.015726362596093946,0.9933341393635152,-0.003869497315732549,-0.0077970861706913065,-0.5980432408688401,2.4232150523822904,-0.3858650316072227,-0.7512094371042786,0.0,0.0,32.14875386121323,0.32634356109616824,-0.07512063240450484,0.17291961038361758
+7.5,0.000107599543358684,0.0007394937803025972,0.013504567661559979,-0.08364266861489082,0.015906427905240394,0.009583067165034334,0.9963227661566232,0.0007197147052641299,0.007097434508565469,-0.46693858693088486,2.8755422492985363,-0.5110588809514813,-0.6141125252964661,0.0,0.0,31.92977526965268,0.2038459682312732,-0.04214149517287891,0.1800308823937404
+7.5200000000000005,-1.5865370229633407e-05,7.418337530198168e-05,0.006713580620871101,-0.05217776311495121,0.009911221052816539,0.004881165354838409,0.9985766985857125,0.0024796531555402106,0.013097491291318689,-0.341029033641626,3.156060457945725,-0.5918302643488129,-0.4707998669600896,0.0,0.0,31.448645474327186,-0.07776553719173873,0.026240217259710485,0.18734332058430725
+7.54,-3.231287106677665e-05,-1.583458595723645e-05,0.002269336442746955,-0.02177314993565547,0.00411190483191998,0.0016742417767014477,0.9997530790625283,0.0023154611040718635,0.011932777921510854,-0.2219622720900245,3.043111480054994,-0.5784736237906646,-0.3209868579517091,0.0,0.0,30.79332731819349,-0.6192137519227473,0.13896838837481607,0.1949308349353997
+7.5600000000000005,-1.7778054681255422e-05,6.276378069349673e-05,6.81847947490953e-05,-2.9101743477082726e-05,5.248989948503081e-06,2.8442939555858607e-05,0.9999999991582679,0.0011878576877215187,0.006327468028326785,-0.10995079009844967,2.1745731959410657,-0.4107564829246999,-0.16459463253847403,0.0,0.0,30.258807463117797,-1.5596038365875737,0.3263175708326902,0.20383953892364842
+7.58,-1.8464380976886724e-05,6.155953276224476e-05,3.178314290691738e-05,-2.145215392413631e-05,-3.2064688274261667e-06,1.285953355003455e-05,0.999999999682078,-3.431508225130802e-05,-6.011896685547205e-05,-0.0018200857038479373,0.0007649430861267348,-0.0008455222844616084,-0.0015583611924547824,0.0,0.0,19.533005621610283,-0.000548900290262555,0.000649674610254003,0.001990017664952188
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
deleted file mode 100644
index 16bd8d88a91abebcf2c00cdabb7bd163f44e2046..0000000000000000000000000000000000000000
--- a/test/gtest_factor_force_torque.cpp
+++ /dev/null
@@ -1,856 +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>
-
-// 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<StateBlock>(c); KF->addStateBlock('C', sbc, problem);
- StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
- StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
- StateBlockPtr sbbimu = make_shared<StateBlock>(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<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateBlock>(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_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 45034d63e1e90ab4c86172a8ba81a88317cd6bb4..0f63fd9eeefb78accd65fb03b42eeffc64d21230 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -58,6 +58,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#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"
@@ -72,6 +73,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include <core/state_block/state_block_derived.h>
#include <Eigen/Eigenvalues>
@@ -126,11 +128,11 @@ class FactorInertialKinematics_1KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = ZERO3;
bias_imu_ = ZERO6;
- StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
- StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
- StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
- StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
- sbbimu_ = make_shared<StateBlock>(bias_imu_);
+ StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
+ StateBlockPtr sbL = make_shared<StateVector3d>(ZERO3);
+ StateBlockPtr sbd = make_shared<StateVector3d>(ZERO3);
+ StateBlockPtr sbb = make_shared<StateParams3>(bias_p_);
+ sbbimu_ = make_shared<StateParams6>(bias_imu_);
KF0_->addStateBlock('C', sbc, problem_);
KF0_->addStateBlock('D', sbd, problem_);
@@ -328,7 +330,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
// Vector3d bias_p_ = ZERO3;
// Vector6d bias_imu_ = ZERO6;
-// StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
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..6e3ee1a73fdbcc8c4f8d39b79175ffd677b3ab31
--- /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().front()->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().front()->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_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64ab43627674118f196b43bccc4b05c15fed4ba8
--- /dev/null
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,560 @@
+//--------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/factor/factor_angular_momentum.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 <core/math/rotations.h>
+#include <core/tree_manager/tree_manager_sliding_window.h>
+#include <core/capture/capture_pose.h>
+
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <string>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SolverCeresPtr solver;
+
+ // FTI sensor
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ Vector6d bias_true;
+ Vector3d cdm_true;
+ Vector3d inertia_true;
+ double mass_true;
+ Matrix<double, 12, 1> data_fti;
+
+ // Pose sensor
+ SensorBasePtr SP;
+ Vector7d data_pose;
+
+ double dt;
+
+ // Reading csv data_fti
+ std::vector<double> time_stamp;
+ std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
+ std::vector<Quaterniond> quaternion;
+
+ protected:
+ void extractAndCompleteData(const std::string& file_path_name)
+ {
+ Vector3d position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i;
+ std::ifstream data_simulation;
+ string data_file = file_path_name;
+ data_simulation.open(data_file, std::ios::in);
+ string line_data;
+ char delimiter = ',';
+ std::getline(data_simulation, line_data);
+
+ int counter = 0;
+
+ while (std::getline(data_simulation, line_data))
+ {
+ std::stringstream data_str(line_data);
+
+ // time stamp
+ string string_time_stamp;
+ std::getline(data_str, string_time_stamp, delimiter);
+ time_stamp.push_back(std::stod(string_time_stamp));
+
+ // position
+ string string_pos_x;
+ string string_pos_y;
+ string string_pos_z;
+
+ std::getline(data_str, string_pos_x, delimiter);
+ std::getline(data_str, string_pos_y, delimiter);
+ std::getline(data_str, string_pos_z, delimiter);
+
+ position_i(0) = std::stod(string_pos_x);
+ position_i(1) = std::stod(string_pos_y);
+ position_i(2) = std::stod(string_pos_z);
+
+ position.push_back(position_i);
+
+ // quaternion
+ string string_quat_x;
+ string string_quat_y;
+ string string_quat_z;
+ string string_quat_w;
+
+ std::getline(data_str, string_quat_x, delimiter);
+ std::getline(data_str, string_quat_y, delimiter);
+ std::getline(data_str, string_quat_z, delimiter);
+ std::getline(data_str, string_quat_w, delimiter);
+
+ Quaterniond quaternion_i(std::stod(string_quat_w),
+ std::stod(string_quat_x),
+ std::stod(string_quat_y),
+ std::stod(string_quat_z));
+
+ quaternion.push_back(quaternion_i);
+
+ // linear velocity
+ string string_vlin_x;
+ string string_vlin_y;
+ string string_vlin_z;
+
+ std::getline(data_str, string_vlin_x, delimiter);
+ std::getline(data_str, string_vlin_y, delimiter);
+ std::getline(data_str, string_vlin_z, delimiter);
+
+ vlin_i(0) = std::stod(string_vlin_x);
+ vlin_i(1) = std::stod(string_vlin_y);
+ vlin_i(2) = std::stod(string_vlin_z);
+
+ vlin.push_back(vlin_i);
+
+ // angular velocity
+ string string_vang_x;
+ string string_vang_y;
+ string string_vang_z;
+
+ std::getline(data_str, string_vang_x, delimiter);
+ std::getline(data_str, string_vang_y, delimiter);
+ std::getline(data_str, string_vang_z, delimiter);
+
+ vang_i(0) = std::stod(string_vang_x);
+ vang_i(1) = std::stod(string_vang_y);
+ vang_i(2) = std::stod(string_vang_z);
+
+ vang.push_back(vang_i);
+
+ // force
+ string string_force_x;
+ string string_force_y;
+ string string_force_z;
+
+ std::getline(data_str, string_force_x, delimiter);
+ std::getline(data_str, string_force_y, delimiter);
+ std::getline(data_str, string_force_z, delimiter);
+
+ force_i(0) = std::stod(string_force_x);
+ force_i(1) = std::stod(string_force_y);
+ force_i(2) = std::stod(string_force_z);
+
+ force.push_back(force_i);
+
+ // torque
+ string string_torque_x;
+ string string_torque_y;
+ string string_torque_z;
+
+ std::getline(data_str, string_torque_x, delimiter);
+ std::getline(data_str, string_torque_y, delimiter);
+ std::getline(data_str, string_torque_z, delimiter);
+
+ torque_i(0) = std::stod(string_torque_x);
+ torque_i(1) = std::stod(string_torque_y);
+ torque_i(2) = std::stod(string_torque_z);
+
+ torque.push_back(torque_i);
+
+ // acceleration -- need to compute from other data
+ a_meas_i = force_i/mass_true;
+
+ a_meas.push_back(a_meas_i);
+
+ counter++;
+ }
+ }
+
+ void SetUp() override
+ {
+ bias_true = Vector6d::Zero();
+ cdm_true = {0, 0, 0};
+ inertia_true = {0.0134943, 0.0141622, 0.0237319};
+ mass_true = 1.9828;
+
+ std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv");
+
+
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml",
+ wolf_bodydynamics_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ solver = std::make_shared<SolverCeres>(P);
+ auto options = solver->getSolverOptions();
+ // solver->getSolverOptions().max_num_iterations = 1e4;
+ // solver->getSolverOptions().function_tolerance = 1e-15;
+ // solver->getSolverOptions().gradient_tolerance = 1e-15;
+ // solver->getSolverOptions().parameter_tolerance = 1e-15;
+
+ // FTI sensor and processor
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->findSensor("sensor FTI"));
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+
+ // Pose sensor
+ SP = P->findSensor("sensor Pose");
+ }
+};
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registration)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+//this test checks the pre-integration evolution along the time
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_and_csv)
+{
+ std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ // creem un nou arxiu CSV per imprimir els estats que estem rebent ara
+ std::fstream fout;
+ fout.open(wolf_bodydynamics_root + "/test/dades_simulacio_pep_estimador.csv",
+ std::fstream::out | std::fstream::trunc);
+
+ S->getStateBlock('I')->setState(bias_true);
+ S->getStateBlock('C')->setState(cdm_true);
+ S->getStateBlock('i')->setState(inertia_true);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ S->setStateBlockDynamic('I', false);
+
+ //Writing the first lines to know how the data will be distributed in the csv
+ fout << "time stamp"
+ << ","
+ << "position_x_true"
+ << ","
+ << "position_y_true"
+ << ","
+ << "position_z_true"
+ << ","
+ << "quaternion_x_true"
+ << ","
+ << "quaternion_y_true"
+ << ","
+ << "quaternion_z_true"
+ << ","
+ << "quaternion_w_true"
+ << ","
+ << "position_x_est"
+ << ","
+ << "position_y_est"
+ << ","
+ << "position_z_est"
+ << ","
+ << "quaternion_x_est"
+ << ","
+ << "quaternion_y_est"
+ << ","
+ << "quaternion_z_est"
+ << ","
+ << "quaternion_w_est"
+ << "\n";
+
+ // Force processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+ C0->process();
+
+ int i_init = 0;
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+ p->getOrigin()->getFrame()->getStateBlock('V')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i_init].conjugate() * vlin[i_init]);
+ Vector3d ang_mom_init(
+ vang[i_init](0) * inertia_true(0), vang[i_init](1) * inertia_true(1), vang[i_init](2) * inertia_true(2));
+ p->getOrigin()->getFrame()->getStateBlock('L')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(ang_mom_init);
+
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data_fti
+ {
+ // SIMULATOR
+
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ // results printed in the csv in the order established before
+ Vector3d position_est(p->getLast()->getFrame()->getStateBlock('P')->getState());
+ Vector4d quaternion_coeffs_est(p->getLast()->getFrame()->getStateBlock('O')->getState());
+
+ fout << time_stamp[i] << "," << position[i](0) << "," << position[i](1) << "," << position[i](2) << ","
+ << quaternion[i].coeffs()(0) << "," << quaternion[i].coeffs()(1) << "," << quaternion[i].coeffs()(2)
+ << "," << quaternion[i].coeffs()(3) << "," << position_est(0) << "," << position_est(1) << ","
+ << position_est(2) << "," << quaternion_coeffs_est(0) << "," << quaternion_coeffs_est(1) << ","
+ << quaternion_coeffs_est(2) << "," << quaternion_coeffs_est(3)
+ << "\n";
+
+ }
+ fout.close();
+}
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online)
+{
+ // Calibration params
+ Vector6d bias_guess = S->getStateBlock('I')->getState();
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+
+ // add regularization, uncomment if the parameter is not fixed
+
+ S->addPriorParameter('I', // cdm
+ bias_guess, // cdm
+ 0.1*0.1*Matrix6d::Identity()); // cov
+
+ S->addPriorParameter('C', // cdm
+ cdm_guess, // cdm
+ 0.1*0.1*Matrix3d::Identity()); // cov
+
+ S->addPriorParameter('i', // inertia
+ inertia_guess, // inertia
+ 0.01*0.01*Matrix3d::Identity()); // cov
+
+ S->addPriorParameter('m', // mass
+ Vector1d(mass_guess), // mass
+ 0.1 * 0.1 * Matrix1d::Identity()); // cov
+
+ std::string report;
+
+ // Force processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+ C0->process();
+
+ int i_init = 0;
+
+ // Pose
+ data_pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
+ {
+ // SIMULATOR
+
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i] , quaternion[i].coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ // check if new KF and solve
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+ // results of this iteration
+ WOLF_INFO("Time : ", t, " s.");
+ WOLF_INFO(report);
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ }
+ }
+
+ // FINAL RESULTS
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.11);
+ ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-2);
+ ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+ ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 2e-2);
+}
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
+{
+ // Calibration params
+ Vector6d bias_guess = S->getStateBlock('I')->getState();
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+
+ // // add regularization, uncomment if the parameter is not fixed
+
+ // S->addPriorParameter('I', // bias
+ // bias_guess, // bias
+ // 0.1 * 0.1 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('C', // cdm
+ // cdm_guess, // cdm
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('i', // inertia
+ // inertia_guess, // inertia
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('m', // mass
+ // Vector1d(mass_guess), // mass
+ // 0.01 * 0.01 * Matrix1d::Identity()); // cov
+
+ int i_init = 0;
+
+ // Force FTI processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), S->getNoiseCov(), nullptr);
+ C0->process();
+
+ // Pose
+ data_pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
+ {
+ // SIMULATOR
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i] , quaternion[i].coeffs();
+
+ // ESTIMATOR
+
+ // fti
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ // Pose
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
+ CP->process();
+ }
+
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+
+ // FINAL RESULTS
+ WOLF_INFO("-----------------------------");
+ WOLF_INFO(report);
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.21);
+ ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 2e-3);
+ ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+ ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 4e-2);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) =
+ // "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
+ ::testing::GTEST_FLAG(filter) =
+ "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation*";
+
+ return RUN_ALL_TESTS();
+}
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..43629f400a4f763490ea6bd6210deaae1104ae80
--- /dev/null
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,1166 @@
+//--------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/factor/factor_angular_momentum.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 <core/math/rotations.h>
+#include <core/tree_manager/tree_manager_sliding_window.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;
+ Vector6d bias_true;
+ 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());
+
+ bias_true = Vector6d::Zero();
+ cdm_true = {0, 0, 0.0341};
+ inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {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)
+{
+ Vector3d cdm_guess(0.01, 0.01, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ // 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("-----------------------------");
+
+ double dt = 0.2;
+ double time_last = 5.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ 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)
+{
+ Vector3d cdm_guess(0.005, 0.005, 0.05);
+ double mass_guess = 1.9;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ // 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.
+
+ double dt = 0.2;
+ double time_last = 5.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ 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
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
+{
+ Vector3d cdm_guess(0.02, 0.02, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ // 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_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ 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("-----------------------------");
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+ 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_and_ang_mom_hovering)
+{
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ // 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', false);
+
+ 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_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ 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.
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
+ 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("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+
+ 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
+}
+
+// Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
+{
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ // 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', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 0.01, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ 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()->unfix();
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF1->getFrame()->unfix();
+ C_KF1->getFrame()->getStateBlock('P')->fix();
+ C_KF1->getFrame()->getStateBlock('O')->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("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.
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process(); // En aquesta lÃnia el programa s'aborta
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->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("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+
+ 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
+}
+
+// Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
+{
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ Vector3d ang_vel_measure_initial(0, 0, 0);
+ data.segment<3>(3) = ang_vel_measure_initial;
+ data.segment<3>(6) = -mass_true * gravity();
+ Vector3d torque_measure_constant(0, 0, 1);
+ data.segment<3>(9) = torque_measure_constant;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->fix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->unfix();
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState((Quaterniond(1, 0, 0, 0)).coeffs());
+
+ // initialiation of all the variables needed in the following iteration process
+ Vector3d torque_true(0, 0, 1);
+ Vector3d dL;
+ Vector3d L_true(0, 0, 0);
+ Vector3d ang_vel_true(0, 0, 0);
+ Vector3d ang_acc_true;
+ Vector3d angle_true;
+ Quaterniond quat_true(1, 0, 0, 0);
+ double dt = 0.2; // time increment
+ Vector3d position_data(0, 0, 0);
+ Quaterniond q_data;
+ std::string report;
+
+ for (TimeStamp t = dt; t <= 5.0; t += dt)
+ {
+ // SIMULATOR
+
+ // calculate dL = torque*dt
+ dL = torque_true * dt;
+ // actualize L = L + dt
+ L_true = L_true + dL; // L = L + dt
+ // w = L*i_inv we actualize the data
+ ang_vel_true(0) = L_true(0) / inertia_true(0);
+ ang_vel_true(1) = L_true(1) / inertia_true(1);
+ ang_vel_true(2) = L_true(2) / inertia_true(2);
+ data.segment<3>(3) = ang_vel_true;
+
+ ang_acc_true(0) = torque_true(0) / inertia_true(0);
+ ang_acc_true(1) = torque_true(1) / inertia_true(1);
+ ang_acc_true(2) = torque_true(2) / inertia_true(2);
+
+ angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
+ quat_true = quat_true * exp_q(angle_true);
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ q_data = quat_true;
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
+
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("-----------------------------");
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__zero_bias)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true(0, 0, 0.01);
+ Vector3d L_true(0, 0, 0);
+ Vector3d position_true(0, 0, 0);
+ Quaterniond quat_true(1, 0, 0, 0);
+ Vector3d
+ ang_acc_true; // = inertia_true.asDiagonal().inverse() * torque_true; // alpha = I.inv * torque is constant
+ // Vector3d ang_acc_true = (torque_true.array() / inertia_true.array()).matrix();
+ ang_acc_true(0) = torque_true(0) / inertia_true(0);
+ ang_acc_true(1) = torque_true(1) / inertia_true(1);
+ ang_acc_true(2) = torque_true(2) / inertia_true(2);
+ Vector3d angle_true(0, 0, 0);
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(3) = ang_vel_true;
+ data.segment<3>(6) = -mass_true * gravity();
+ data.segment<3>(9) = torque_true;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess;
+ bias_guess << 0, 0, 0, 0, 0, 0;
+ S->getStateBlock('I')->setState(bias_guess);
+ Vector3d cdm_guess(0.005, 0.005, 0.1);
+ S->getStateBlock('C')->setState(cdm_guess);
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
+ double mass_guess = 2.0;
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ // Vector3d dL;
+ double dt = 0.1; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= 10.0; t += dt)
+ {
+ // SIMULATOR
+
+ // angular momentum
+ // L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ // FIXME JS: this should be on top but it works better here, weird...
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // // w = L*i_inv we actualize the data
+ // ang_vel_true(0) = L_true(0) / inertia_true(0);
+ // ang_vel_true(1) = L_true(1) / inertia_true(1);
+ // ang_vel_true(2) = L_true(2) / inertia_true(2);
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true; // TODO: add acc bias
+ data.segment<3>(3) = ang_vel_true; // TODO: add gyro bias
+ data.segment<3>(6) = force_true;
+ data.segment<3>(9) = torque_true;
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ WOLF_INFO("True IMU bias : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6); // Ix and Iy not observable in this test
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__nonzero_bias)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true = Vector3d(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true = Vector3d(0, 0, 0.01);
+ Vector3d position_true = Vector3d(0, 0, 0);
+ Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
+ Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
+ Vector3d angle_true = Vector3d(0, 0, 0);
+ Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
+ Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
+ bias_true << acc_bias_true, gyro_bias_true;
+ // bias_true.setZero();
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(6) = -mass_true * gravity();
+ data.segment<3>(9) = torque_true;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess = Vector6d::Zero();
+ // Vector6d bias_guess = bias_true;
+ Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
+ Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
+ double mass_guess = 2.0;
+ S->getStateBlock('I')->setState(bias_guess);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('i')->setState(inertia_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 0.01, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ // C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ double t_final = 8.0; // total run time
+ double dt = 0.1; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= t_final; t += dt)
+ {
+ // SIMULATOR
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(6) = force_true;
+ data.segment<3>(9) = torque_true;
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ // results of this iteration
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ // Final results
+ WOLF_INFO("True IMU bias * : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6); // Ix and Iy not observable in this test
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
+ torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true = Vector3d(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true = Vector3d(0, 0, 0.1);
+ Vector3d position_true = Vector3d(0, 0, 0);
+ Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
+ Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
+ Vector3d angle_true = Vector3d(0, 0, 0);
+ Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
+ Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
+ bias_true << acc_bias_true, gyro_bias_true;
+ // bias_true.setZero();
+
+ // Noise
+ Vector3d acc_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ Vector3d gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ Vector3d force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ Vector3d torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = -mass_true * gravity() + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess = Vector6d::Zero();
+ // Vector6d bias_guess = bias_true;
+ Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
+ Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
+ double mass_guess = 2.0;
+ S->getStateBlock('I')->setState(bias_guess);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('i')->setState(inertia_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * .001, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ // C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ double t_final = 2.40; // total run time
+ double dt = 0.01; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= t_final; t += dt)
+ {
+ // SIMULATOR
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // noises
+ acc_noise = Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = force_true + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+ data_cov = S->getNoiseCov();
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ // WOLF_INFO(report);
+ // P->print(4,1,1,1);
+
+ // results of this iteration
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ // Final results
+ WOLF_INFO("True IMU bias * : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ EXPECT_MATRIX_APPROX(bias_estimated, bias_true, 1e-3);
+ EXPECT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-3); // cdm_z is not observable while hovering
+ EXPECT_NEAR(inertia_estimated(2), inertia_true(2), 1e-3); // Ix and Iy not observable in this test
+ EXPECT_NEAR(mass_estimated, mass_true, 1e-3);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) =
+ // "Test_SolveProblemForceTorqueInertialDynamics_yaml.torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise";
+ 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..2df498f9c582fcad2918a0ae5fc714951ebb6c26
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -0,0 +1,64 @@
+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
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ 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]
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
+ 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_simulation_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f3e25c03075b12b8df8c8f0026df1a88aa055e9e
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
@@ -0,0 +1,97 @@
+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: [100,100,100]
+ L: [100,100,100]
+ time_tolerance: 0.01
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ plugin: "core"
+ n_frames: 1000
+ n_fix_first_frames: 0
+ 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
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
+
+ # Dynamics
+ com: [0.005,0.005,0.01] # center of mass [m]
+ inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ mass: 2.00 # mass [kg]
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
+ -
+ name: sensor Pose
+ type: SensorPose
+ plugin: core
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ std_px: 0.001
+ std_py: 0.001
+ std_pz: 0.001
+ std_ox: 0.001
+ std_oy: 0.001
+ std_oz: 0.001
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.01
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.001
+ state_getter: true
+ state_priority: 1
+ # n_propellers: 6
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.0995 # De moment el que funciona millor és 1 s o 2s
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ -
+ name: prc Pose
+ type: ProcessorPose
+ plugin: core
+ sensor_name: sensor Pose
+ time_tolerance: 0.01
+ keyframe_vote:
+ voting_active: true
+ apply_loss_function: false
+
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..79aaa00d0706000e43bd4641d1658d8ab1b77236
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -0,0 +1,73 @@
+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: 6
+ n_fix_first_frames: 0
+ 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
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
+
+ # 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.952 # mass [kg]
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
+ 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.0995
+ 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..84aca5a4b193784435a6c073d95770f1a255deea
--- /dev/null
+++ b/test/yaml/processor_force_torque_inertial.yaml
@@ -0,0 +1,14 @@
+ 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..21d169c79bfe81bc609e966bf3dfbd57275e7e97
--- /dev/null
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -0,0 +1,16 @@
+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
+accb_initial_std: 0 # m/s2 - initial bias
+gyrob_initial_std: 0 # rad/sec - initial bias
+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]
+imu_bias_fix: true
+com_fix: true
+inertia_fix: true
+mass_fix: true
\ 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