diff --git a/CMakeLists.txt b/CMakeLists.txt
index 60296c1c281d4e9de35a772b0090f10a6a01a624..71ccfc6d2fc73a310bd1d1c6a07dcf786e92e8eb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -112,7 +112,7 @@ include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h
SET(HDRS_FACTOR
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_preint.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
@@ -123,7 +123,7 @@ include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
)
SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint.h
-include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint_dynamics.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
@@ -149,7 +149,7 @@ src/feature/feature_inertial_kinematics.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_force_torque_inertial_preint.cpp
-src/processor/processor_force_torque_inertial_preint_dynamics.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
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
similarity index 89%
rename from include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
rename to include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
index 8bcc1608659f2a373282c323099bdd38dc8d0666..f7b73c56f6cdcbbb5ac8bc70c1461bdff8cc5295 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -26,8 +26,8 @@
* Author: jsola
*/
-#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
-#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+#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"
@@ -37,12 +37,12 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreintDynamics);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialDynamics);
-struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessorMotion
+struct ParamsProcessorForceTorqueInertialDynamics : public ParamsProcessorMotion
{
- ParamsProcessorForceTorqueInertialPreintDynamics() = default;
- ParamsProcessorForceTorqueInertialPreintDynamics(std::string _unique_name, const ParamsServer& _server)
+ ParamsProcessorForceTorqueInertialDynamics() = default;
+ ParamsProcessorForceTorqueInertialDynamics(std::string _unique_name, const ParamsServer& _server)
: ParamsProcessorMotion(_unique_name, _server)
{
// n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
@@ -57,20 +57,20 @@ struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessor
// int n_propellers;
};
-WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreintDynamics);
-class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialDynamics);
+class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
{
public:
- ProcessorForceTorqueInertialPreintDynamics(
- ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
- ~ProcessorForceTorqueInertialPreintDynamics() override;
+ ProcessorForceTorqueInertialDynamics(
+ ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics_);
+ ~ProcessorForceTorqueInertialDynamics() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics,
- ParamsProcessorForceTorqueInertialPreintDynamics);
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics,
+ ParamsProcessorForceTorqueInertialDynamics);
protected:
- ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
- SensorForceTorqueInertialPtr sensor_fti_;
+ ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
+ SensorForceTorqueInertialPtr sensor_fti_;
Matrix6d imu_drift_cov_;
Matrix3d gyro_noise_cov_;
@@ -268,11 +268,11 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
Eigen::MatrixXd& _J_tangent_model) const;
};
-inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const
+inline Eigen::VectorXd ProcessorForceTorqueInertialDynamics::deltaZero() const
{
return bodydynamics::fti::identity();
}
} /* namespace wolf */
-#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ */
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ */
diff --git a/src/capture/capture_force_torque.cpp b/src/capture/capture_force_torque.cpp
index 0211ef89153eae87cd19ca7c6d44295f88af959b..3b09bdda9e821497e4e7cc77300a8e06cb656e71 100644
--- a/src/capture/capture_force_torque.cpp
+++ b/src/capture/capture_force_torque.cpp
@@ -20,7 +20,7 @@
//
//--------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"
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 44d1be590b582d7e07fe0fe85ef538d38419c63c..52a9421da7e2df46d9c4b02d2f9f8cafa08666bf 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "bodydynamics/feature/feature_force_torque_preint.h"
+#include "bodydynamics/feature/feature_force_torque.h"
namespace wolf {
FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
similarity index 90%
rename from src/processor/processor_force_torque_inertial_preint_dynamics.cpp
rename to src/processor/processor_force_torque_inertial_dynamics.cpp
index 4e8d1b278e302ebe7db8867296432754e893ece5..d057966275793cfc7ed350e4dbb097c21727e2a5 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -27,7 +27,7 @@
*/
// bodydynamics
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
@@ -39,9 +39,9 @@
namespace wolf
{
-ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDynamics(
- ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics)
- : ProcessorMotion("ProcessorForceTorqueInertialPreintDynamics",
+ProcessorForceTorqueInertialDynamics::ProcessorForceTorqueInertialDynamics(
+ ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics)
+ : ProcessorMotion("ProcessorForceTorqueInertialDynamics",
"POVL",
3, // dim
13, // state size [p, q, v, L]
@@ -49,18 +49,18 @@ ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDy
18, // delta cov size
12, // data size [a, w, f, t]
13, // calib size [ab, wb, c, i, m]
- _params_force_torque_inertial_preint_dynamics),
- params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
+ _params_force_torque_inertial_dynamics),
+ params_force_torque_inertial_dynamics_(_params_force_torque_inertial_dynamics)
{
//
}
-ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics()
+ProcessorForceTorqueInertialDynamics::~ProcessorForceTorqueInertialDynamics()
{
//
}
-CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
@@ -77,7 +77,7 @@ CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(cons
}
-FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
+FeatureBasePtr ProcessorForceTorqueInertialDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
{
auto motion = _capture_own->getBuffer().back();
@@ -90,7 +90,7 @@ FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(Captur
return ftr;
}
-FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
+FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin)
{
FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
@@ -100,7 +100,7 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB
return fac_ftipd;
}
-void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
+void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
CaptureMotionPtr _capture_own)
{
// 1. Feature and factor FTI -- force-torque-inertial
@@ -160,7 +160,7 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu
}
}
-void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
+void ProcessorForceTorqueInertialDynamics::setCalibration(const CaptureBasePtr _capture,
const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias
@@ -169,7 +169,7 @@ void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBas
_capture->getStateBlock('m')->setState(_calibration.segment<1>(12)); // Set m
}
-void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor)
+void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
{
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
@@ -183,7 +183,7 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
}
-void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
+void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
const Eigen::VectorXd& _bias,
const Eigen::VectorXd& _model,
Eigen::VectorXd& _tangent,
@@ -219,7 +219,7 @@ void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::Vecto
_J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross
}
-void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
+void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
@@ -424,7 +424,7 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
-void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2) const
@@ -432,7 +432,7 @@ void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::Vec
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
-void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2,
@@ -442,7 +442,7 @@ void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::Vec
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
-void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComposite& _x,
+void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
VectorComposite& _x_plus_delta) const
@@ -452,13 +452,13 @@ void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComp
_x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
}
-Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
+Eigen::VectorXd ProcessorForceTorqueInertialDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta_step) const
{
return fti::plus(_delta_preint, _delta_step);
}
-VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
{
@@ -482,17 +482,17 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
}
}
-bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
+bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
{
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ >
- params_force_torque_inertial_preint_dynamics_->max_time_span)
+ params_force_torque_inertial_dynamics_->max_time_span)
{
WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
- if (getBuffer().size() > params_force_torque_inertial_preint_dynamics_->max_buff_length)
+ if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
{
WOLF_DEBUG("PM: vote: buffer length");
return true;
@@ -503,14 +503,14 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
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_preint_dynamics_->dist_traveled)
+ if (dist > params_force_torque_inertial_dynamics_->dist_traveled)
{
WOLF_DEBUG("PM: vote: distance traveled");
return true;
}
// angle turned
double angle = 2.0 * acos(getMotion().delta_integr_(15));
- if (angle > params_force_torque_inertial_preint_dynamics_->angle_turned)
+ if (angle > params_force_torque_inertial_dynamics_->angle_turned)
{
WOLF_DEBUG("PM: vote: angle turned");
return true;
@@ -524,6 +524,6 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
#include <core/processor/factory_processor.h>
namespace wolf
{
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreintDynamics);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreintDynamics);
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialDynamics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialDynamics);
} // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 86c5ad5fcd023a4cc300cf314557ef1fe67ca026..52e1fd976f3619fc9b303b7c9ef601c740d10677 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -25,11 +25,11 @@ 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.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.cpp)
+# wolf_add_gtest(gtest_processor_force_torque gtest_processor_force_torque.cpp)
-wolf_add_gtest(gtest_processor_force_torque_inertial_preint_dynamics gtest_processor_force_torque_inertial_preint_dynamics.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)
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
index a0a659fa636a0337a570c93f57d819b22f5c9ce5..846807390641df274b1b0d9cf8542183e6948067 100644
--- a/test/gtest_factor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
@@ -47,7 +47,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
public:
ProblemPtr P;
SensorForceTorqueInertialPtr S;
- ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ ProcessorForceTorqueInertialDynamicsPtr p;
FrameBasePtr F0, F1, F;
CaptureMotionPtr C0, C1, C;
FeatureMotionPtr f1;
@@ -69,7 +69,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
P = Problem::autoSetup(server);
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
- p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
data = VectorXd::Zero(12); // [ a, w, f, t ]
data_cov = MatrixXd::Identity(12, 12);
@@ -120,7 +120,7 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
public:
ProblemPtr P;
SensorForceTorqueInertialPtr S;
- ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ ProcessorForceTorqueInertialDynamicsPtr p;
FrameBasePtr F0, F1;
CaptureMotionPtr C0, C1;
FeatureMotionPtr f1;
@@ -150,8 +150,8 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
S->setStateBlockDynamic('I', true);
// crear processador
- auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
- p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
+ auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>();
+ p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
// crear dos frame
VectorXd state(13);
diff --git a/test/gtest_processor_force_torque.cpp b/test/gtest_processor_force_torque.cpp
index e59acaaea92acb04d7228d8192c0746fb6873367..ee8d7b941eb8c9fd2f5b8a39adf21b17efbcca77 100644
--- a/test/gtest_processor_force_torque.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>
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp
similarity index 94%
rename from test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
rename to test/gtest_processor_force_torque_inertial_dynamics.cpp
index daa10f8db883230ad842e915b7f2882e02f59dc0..25df6c6d1d00683e5a77bc16f1a90511f41d991a 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
@@ -42,12 +42,12 @@ using namespace bodydynamics::fti;
WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
-class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Test
+class Test_ProcessorForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
ProblemPtr P;
SensorForceTorqueInertialPtr S;
- ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ ProcessorForceTorqueInertialDynamicsPtr p;
protected:
void SetUp() override
@@ -58,17 +58,17 @@ class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Tes
P = Problem::autoSetup(server);
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
- p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
}
};
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion)
+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_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = - gravity();
@@ -137,7 +137,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
}
//Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__com_zero)
+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();
@@ -205,7 +205,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__c
}
//Test to see if the voteForKeyFrame works (distance traveled)
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dist)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_dist)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
@@ -231,7 +231,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dis
}
//Test to see if the voteForKeyFrame works (buffer)
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buffer)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_buffer)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
@@ -260,6 +260,6 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buf
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.not_moving_test";
+ // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialDynamics_yaml.not_moving_test";
return RUN_ALL_TESTS();
}
\ No newline at end of file
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 3c9ef6531c8608f91cca6818deed2fed6e64ad9b..099a536aa1fb1a4617241af6e0bb6adb2ddeddd9 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
@@ -43,16 +43,16 @@ using namespace bodydynamics::fti;
WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
-class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::Test
+class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
- ProblemPtr P;
- SensorForceTorqueInertialPtr S;
- ProcessorForceTorqueInertialPreintDynamicsPtr p;
- SolverCeresPtr solver;
- Vector3d cdm_true;
- Vector3d inertia_true;
- double mass_true;
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ SolverCeresPtr solver;
+ Vector3d cdm_true;
+ Vector3d inertia_true;
+ double mass_true;
protected:
void SetUp() override
@@ -70,7 +70,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
// solver->getSolverOptions().parameter_tolerance = 1e-15;
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
- p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
cdm_true = {0, 0, 0.0341};
inertia_true = {0.017598, 0.017957, 0.029599};
@@ -78,13 +78,13 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
}
};
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, force_registraion)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion)
{
FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
}
// Hover test.
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
{
double mass_guess = S->getStateBlock('m')->getState()(0);
@@ -152,7 +152,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
}
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
{
S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
S->getStateBlock('m')->setState(Vector1d(mass_true));
@@ -220,7 +220,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
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_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
{
S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
S->getStateBlock('m')->setState(Vector1d(1.900));
@@ -307,6 +307,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
+ // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.cdm_only_hovering";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics.yaml b/test/yaml/problem_force_torque_inertial_dynamics.yaml
index 2d9b19cc19d0519737336367906b1ee1e2621f88..029d216cce1c0a3f1b9de1f49aa2824134e03009 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics.yaml
@@ -32,7 +32,7 @@ config:
processors:
-
name: "proc FTID"
- type: "ProcessorForceTorqueInertialPreintDynamics"
+ type: "ProcessorForceTorqueInertialDynamics"
sensor_name: "sensor FTI"
plugin: "bodydynamics"
follow: "processor_force_torque_inertial_preint_dynamics.yaml"
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
index 5ef4ecbe3e2c57c493d962ef0d80a72b5d94ff2e..233368f38accb87f4bbd4500e37bede35dd1378a 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -41,7 +41,7 @@ config:
processors:
-
name: "proc FTID"
- type: "ProcessorForceTorqueInertialPreintDynamics"
+ type: "ProcessorForceTorqueInertialDynamics"
sensor_name: "sensor FTI"
plugin: "bodydynamics"
time_tolerance: 0.1
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 182966b1bea83384e4f0fd498e98f5970944cade..8eb8204126f94bc008bcb01587ae2fa5482fec6b 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -48,7 +48,7 @@ config:
processors:
-
name: "proc FTID"
- type: "ProcessorForceTorqueInertialPreintDynamics"
+ type: "ProcessorForceTorqueInertialDynamics"
sensor_name: "sensor FTI"
plugin: "bodydynamics"
time_tolerance: 0.1