gtestfactor

.clang-format copy

----
-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

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)

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

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)