diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7c1cf5c8443a510097caf0682105612c6aa11bec..60296c1c281d4e9de35a772b0090f10a6a01a624 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -124,7 +124,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_preint.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
)
@@ -150,7 +150,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_preint.cpp
+src/processor/processor_force_torque.cpp
src/processor/processor_inertial_kinematics.cpp
src/processor/processor_point_feet_nomove.cpp
)
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 7b60949d554264ebb5ed946f9ff19db4e3c09bdb..a2a1b32f1da4fe3c0fb58db6f6405cc3ae45fab1 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -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", "PFTPreint", sen_ft, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
+ ProcessorForceTorquePtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 9f4db7c21fd32f833b47aa1ca4ebaf32181e454b..b9ff1b4c45b3e4108c0ed1e755c7a326044a3f76 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -24,7 +24,7 @@
//Wolf includes
#include "bodydynamics/capture/capture_force_torque.h"
-#include "bodydynamics/feature/feature_force_torque_preint.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"
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque.h
similarity index 84%
rename from include/bodydynamics/processor/processor_force_torque_preint.h
rename to include/bodydynamics/processor/processor_force_torque.h
index 6cd089107cfebd55fad2a51ff1b6cf5eddb0b036..c34fe8e43854af461aebd67ed9fda24d4dd7bfaf 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_preint);
+ ~ProcessorForceTorque() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorque, ParamsProcessorForceTorque);
protected:
void computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -109,7 +109,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
CaptureBasePtr _capture_origin) override;
private:
- ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
+ ParamsProcessorForceTorquePtr params_motion_force_torque_preint_;
SensorBasePtr sensor_ikin_;
SensorBasePtr sensor_angvel_;
int nbc_;
@@ -124,17 +124,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);
};
-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/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque.cpp
similarity index 87%
rename from src/processor/processor_force_torque_preint.cpp
rename to src/processor/processor_force_torque.cpp
index 33bb2ef1df8191b72fb3f04c1574847c3fc7e318..7fe666b0452356d8a93e046b89010e4898cc54c1 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque.cpp
@@ -22,8 +22,8 @@
// wolf
#include "bodydynamics/math/force_torque_delta_tools.h"
#include "bodydynamics/capture/capture_force_torque.h"
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/feature/feature_force_torque.h"
+#include "bodydynamics/processor/processor_force_torque.h"
#include "bodydynamics/factor/factor_force_torque.h"
namespace wolf {
@@ -37,11 +37,11 @@ int compute_data_size(int nbc, int dimc){
using namespace Eigen;
-ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) :
- ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12,
+ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque_preint) :
+ ProcessorMotion("ProcessorForceTorque", "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)),
+ params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque_preint)),
nbc_(_params_motion_force_torque_preint->nbc),
dimc_(_params_motion_force_torque_preint->dimc)
@@ -49,12 +49,12 @@ ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorqu
//
}
-ProcessorForceTorquePreint::~ProcessorForceTorquePreint()
+ProcessorForceTorque::~ProcessorForceTorque()
{
//
}
-bool ProcessorForceTorquePreint::voteForKeyFrame() const
+bool ProcessorForceTorque::voteForKeyFrame() const
{
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span)
@@ -76,7 +76,7 @@ bool ProcessorForceTorquePreint::voteForKeyFrame() const
}
-CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
@@ -119,7 +119,7 @@ CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr&
return cap;
}
-FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
+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,
@@ -130,13 +130,13 @@ FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capt
return feature;
}
-Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
+Eigen::VectorXd ProcessorForceTorque::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 ProcessorForceTorque::getCalibration (const CaptureBaseConstPtr _capture) const
{
VectorXd bias_vec(6);
@@ -162,7 +162,7 @@ VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _
return bias_vec;
}
-void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorForceTorque::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
{
CaptureForceTorquePtr cap_ft(std::static_pointer_cast<CaptureForceTorque>(_capture));
@@ -176,7 +176,7 @@ void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture,
cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
}
-FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorForceTorque::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
CaptureForceTorquePtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
FeatureForceTorquePtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
@@ -194,7 +194,7 @@ FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_
return fac_ftpreint;
}
-void ProcessorForceTorquePreint::computeCurrentDelta(
+void ProcessorForceTorque::computeCurrentDelta(
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
@@ -226,7 +226,7 @@ void ProcessorForceTorquePreint::computeCurrentDelta(
_jac_delta_calib = jac_delta_body * jac_body_bias;
}
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta) const
@@ -234,7 +234,7 @@ void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_pr
_delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
}
-void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
+void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
VectorComposite& _x_plus_delta) const
@@ -251,7 +251,7 @@ void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
_x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
}
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta,
@@ -268,5 +268,5 @@ void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_pr
namespace wolf
{
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorquePreint)
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorque)
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 93fddc94887955cdc786b13b1e1b8dccc5879e81..86c5ad5fcd023a4cc300cf314557ef1fe67ca026 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -25,9 +25,9 @@ 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_preint 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_preint gtest_processor_force_torque.cpp)
wolf_add_gtest(gtest_processor_force_torque_inertial_preint_dynamics gtest_processor_force_torque_inertial_preint_dynamics.cpp)
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque.cpp
similarity index 98%
rename from test/gtest_processor_force_torque_preint.cpp
rename to test/gtest_processor_force_torque.cpp
index bc5ce81200e5f5ec1e303665dea973364c448b15..e59acaaea92acb04d7228d8192c0746fb6873367 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque.cpp
@@ -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_ftpreint_;
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", "PFTPreint", sen_ft_, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ proc_ftpreint_ = 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);