diff --git a/.clang-format copy b/.clang-format copy
deleted file mode 100644
index bc951c7539c78c4fa00efde0030dcf12b126686a..0000000000000000000000000000000000000000
--- a/.clang-format copy
+++ /dev/null
@@ -1,115 +0,0 @@
----
-Language: Cpp
-BasedOnStyle: Google
-IndentAccessModifiers: false
-AccessModifierOffset: -2
-AlignAfterOpenBracket: Align
-AlignConsecutiveAssignments: true
-AlignConsecutiveDeclarations: true
-AlignEscapedNewlines: Left
-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: true
-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/test/CMakeLists.txt b/test/CMakeLists.txt
index 7ca4c35a861ceac10989f6c0b0c746b6f1888f3e..e2ad0bed4bde3a03cc06886a00cbe5c69986b143 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -31,6 +31,8 @@ wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inerti
# 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_inertial_preint_dynamics gtest_processor_force_torque_inertial_preint_dynamics.cpp)
+
wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
index af4888344ba6e6ca4312e03475020a226ba4129c..a0a659fa636a0337a570c93f57d819b22f5c9ce5 100644
--- a/test/gtest_factor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -29,6 +29,7 @@
#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>
@@ -62,9 +63,6 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
protected:
void SetUp() override
{
- data = VectorXd::Zero(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_dynamics.yaml", wolf_root + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams());
@@ -73,13 +71,12 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
- //P->print(4, 1, 1, 1);
-
+ 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();
- P->print(4, 1, 1, 1);
-
C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
F0 = C0->getFrame();
@@ -91,6 +88,28 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
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));
+
}
};
@@ -153,11 +172,11 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
// crear un feature
VectorXd delta_preint(VectorXd::Zero(19));
- delta_preint.head<3>() = 0.5*gravity();
- delta_preint.segment<3>(3) = gravity();
- delta_preint(15) = 1;
- delta_preint.segment<3>(6) = 0.5*gravity();
- delta_preint.segment<3>(9) = gravity();
+ 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));
@@ -169,87 +188,170 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
}
};
+// 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)
{
- //FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13);
}
-//Test en el que no hi ha moviment
-// TEST_F(Test_FactorForceTorqueInertialDynamics, 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 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 s'avança 1m en direcció x
+//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_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
-// {
-// 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(15) = 1;
-// delta_preint.segment<3>(6) = 0.5*gravity();
-// delta_preint(6) = 1;
-// delta_preint.segment<3>(9) = gravity();
-// 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 (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/yaml/processor_force_torque_inertial_preint_dynamics.yaml b/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
index beafed9ea44a627f2c5b2ea30490bfd11db7868b..a7160e79986f6252a1b8355adad7d8853fecb9c7 100644
--- a/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
+++ b/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
@@ -6,8 +6,8 @@
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)
+ 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)