Skip to content
Snippets Groups Projects
Commit 722aa368 authored by Amanda Sanjuan Sánchez's avatar Amanda Sanjuan Sánchez
Browse files

gtestfactor

parent b071279c
No related branches found
No related tags found
3 merge requests!31devel->main,!29Add functionality for UAV,!27Fix test with acc_x = 2 m/s2
Hide whitespace changes
Inline Side-by-side
.clang-format copy deleted 100644 → 0
+ 0
− 115
View file @ b071279c
---
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
+ 2
− 0
View file @ 722aa368
...@@ -31,6 +31,8 @@ wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inerti ...@@ -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_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_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_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp) wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
......
test/gtest_factor_force_torque_inertial_dynamics.cpp
+ 182
− 80
View file @ 722aa368
...@@ -29,6 +29,7 @@ ...@@ -29,6 +29,7 @@
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include <core/utils/logging.h> #include <core/utils/logging.h>
#include <core/utils/params_server.h>
#include <core/yaml/parser_yaml.h> #include <core/yaml/parser_yaml.h>
#include <core/state_block/state_block_derived.h> #include <core/state_block/state_block_derived.h>
#include <core/state_block/factory_state_block.h> #include <core/state_block/factory_state_block.h>
...@@ -62,9 +63,6 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test ...@@ -62,9 +63,6 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
protected: protected:
void SetUp() override 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; std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics.yaml", wolf_root + "/test/yaml/"); ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics.yaml", wolf_root + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams()); ParamsServer server = ParamsServer(parser.getParams());
...@@ -73,13 +71,12 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test ...@@ -73,13 +71,12 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().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 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
C->process(); C->process();
P->print(4, 1, 1, 1);
C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin()); C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
F0 = C0->getFrame(); F0 = C0->getFrame();
...@@ -91,6 +88,28 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test ...@@ -91,6 +88,28 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
C1 = std::static_pointer_cast<CaptureMotion>(p->getLast()); C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
F1 = C1->getFrame(); 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 ...@@ -153,11 +172,11 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
// crear un feature // crear un feature
VectorXd delta_preint(VectorXd::Zero(19)); VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = 0.5*gravity(); delta_preint.head<3>() = -0.5*gravity();
delta_preint.segment<3>(3) = gravity(); delta_preint.segment<3>(3) = -gravity();
delta_preint(15) = 1; delta_preint.segment<3>(6) = -0.5*gravity();
delta_preint.segment<3>(6) = 0.5*gravity(); delta_preint.segment<3>(9) = -gravity();
delta_preint.segment<3>(9) = gravity(); delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13)); VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
...@@ -169,87 +188,170 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test ...@@ -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) TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
{ {
//FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
ASSERT_EQ(c1->getCapture(), C1); ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13); ASSERT_EQ(c1->getCalibPreint().size(), 13);
} }
//Test en el que no hi ha moviment //Test en el que no hi ha moviment (not yaml files)
// TEST_F(Test_FactorForceTorqueInertialDynamics, residual) TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
// { {
// VectorXd res_exp = VectorXd::Zero(18); VectorXd res_exp = VectorXd::Zero(18);
// Matrix<double, 18, 1> res; Matrix<double, 18, 1> res;
// VectorXd bias = VectorXd::Zero(6); VectorXd bias = VectorXd::Zero(6);
// c1->residual(F0->getStateBlock('P')->getState(), // p0 P->print(4,1,1,1);
// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
// F0->getStateBlock('V')->getState(), // v0 c1->residual(F0->getStateBlock('P')->getState(), // p0
// F0->getStateBlock('L')->getState(), // L0 Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
// bias, // I F0->getStateBlock('V')->getState(), // v0
// F1->getStateBlock('P')->getState(), // p1 F0->getStateBlock('L')->getState(), // L0
// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 bias, // I
// F1->getStateBlock('V')->getState(), // v1 F1->getStateBlock('P')->getState(), // p1
// F1->getStateBlock('L')->getState(), // L1 Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
// S->getCom(), // C F1->getStateBlock('V')->getState(), // v1
// S->getInertia(), // i F1->getStateBlock('L')->getState(), // L1
// S->getMass(), // m S->getCom(), // C
// res); S->getInertia(), // i
S->getMass(), // m
// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); 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)
// { //Test en el que s'avança 1m en direcció x (not yaml files)
// VectorXd res_exp = VectorXd::Zero(18); TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
// Matrix<double, 18, 1> res; {
// VectorXd bias = VectorXd::Zero(6); VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
// //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual VectorXd bias = VectorXd::Zero(6);
// F1->getStateBlock('P')->setState(Vector3d (1,0,0));
//provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
// //Hem de crear un nou feature i un nou factor ja que la delta preint canvia. F1->getStateBlock('P')->setState(Vector3d (1,0,0));
// VectorXd delta_preint(VectorXd::Zero(19));
// delta_preint.head<3>() = 0.5*gravity(); //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
// delta_preint(0) = 1; VectorXd delta_preint(VectorXd::Zero(19));
// delta_preint.segment<3>(3) = gravity(); delta_preint.head<3>() = -0.5*gravity();
// delta_preint(15) = 1; delta_preint(0) = 1;
// delta_preint.segment<3>(6) = 0.5*gravity(); delta_preint.segment<3>(3) = -gravity();
// delta_preint(6) = 1; delta_preint.segment<3>(6) = -0.5*gravity();
// delta_preint.segment<3>(9) = gravity(); delta_preint(6) = 1;
// MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); delta_preint.segment<3>(9) = -gravity();
// VectorXd calib_preint(VectorXd::Zero(13)); delta_preint(18) = 1;
// MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
// FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib); VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
// FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false); 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 c2->residual(F0->getStateBlock('P')->getState(), // p0
// F0->getStateBlock('L')->getState(), // L0 Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
// bias, // I F0->getStateBlock('V')->getState(), // v0
// F1->getStateBlock('P')->getState(), // p1 F0->getStateBlock('L')->getState(), // L0
// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 bias, // I
// F1->getStateBlock('V')->getState(), // v1 F1->getStateBlock('P')->getState(), // p1
// F1->getStateBlock('L')->getState(), // L1 Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
// S->getCom(), // C F1->getStateBlock('V')->getState(), // v1
// S->getInertia(), // i F1->getStateBlock('L')->getState(), // L1
// S->getMass(), // m S->getCom(), // C
// res); S->getInertia(), // i
S->getMass(), // m
// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); 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) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
\ No newline at end of file
test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
+ 4
− 4
View file @ 722aa368
...@@ -6,8 +6,8 @@ ...@@ -6,8 +6,8 @@
n_propellers: 3 n_propellers: 3
keyframe_vote: keyframe_vote:
voting_active: false voting_active: false
max_time_span: 1 max_time_span: 999
max_buff_length: 5 # motion deltas max_buff_length: 999 # motion deltas
dist_traveled: 1 # meters dist_traveled: 999 # meters
angle_turned: 1 # radians (1 rad approx 57 deg, approx 60 deg) angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment