Skip to content
Snippets Groups Projects
Commit bb10d90b authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Rename ProcessorForceTorqueInertialPreint --> ProcessorForceTorqueInertial

parent ef18716f
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
CMakeLists.txt
+ 2
− 2
View file @ bb10d90b
......@@ -122,7 +122,7 @@ include/${PROJECT_NAME}/feature/feature_force_torque.h
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.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
......@@ -148,7 +148,7 @@ src/feature/feature_force_torque.cpp
src/feature/feature_inertial_kinematics.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_force_torque_inertial_preint.cpp
src/processor/processor_force_torque_inertial.cpp
src/processor/processor_force_torque_inertial_dynamics.cpp
src/processor/processor_force_torque.cpp
src/processor/processor_inertial_kinematics.cpp
......
include/bodydynamics/processor/processor_force_torque_inertial_preint.h → include/bodydynamics/processor/processor_force_torque_inertial.h
+ 15
− 15
View file @ bb10d90b
......@@ -26,8 +26,8 @@
* Author: jsola
*/
#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_
#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_
#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_
#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_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(ParamsProcessorForceTorqueInertialPreint);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertial);
struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion
struct ParamsProcessorForceTorqueInertial : public ParamsProcessorMotion
{
ParamsProcessorForceTorqueInertialPreint() = default;
ParamsProcessorForceTorqueInertialPreint(std::string _unique_name, const ParamsServer& _server)
ParamsProcessorForceTorqueInertial() = default;
ParamsProcessorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
: ParamsProcessorMotion(_unique_name, _server)
{
n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
......@@ -57,18 +57,18 @@ struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion
int n_propellers;
};
WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreint);
class ProcessorForceTorqueInertialPreint : public ProcessorMotion
WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertial);
class ProcessorForceTorqueInertial : public ProcessorMotion
{
public:
ProcessorForceTorqueInertialPreint(
ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint);
~ProcessorForceTorqueInertialPreint() override;
ProcessorForceTorqueInertial(
ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial_preint);
~ProcessorForceTorqueInertial() override;
void configure(SensorBasePtr _sensor) override;
WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreint, ParamsProcessorForceTorqueInertialPreint);
WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertial, ParamsProcessorForceTorqueInertial);
protected:
ParamsProcessorForceTorqueInertialPreintPtr params_force_torque_inertial_preint_;
ParamsProcessorForceTorqueInertialPtr params_force_torque_inertial_preint_;
SensorForceTorqueInertialPtr sensor_fti_;
public:
......@@ -242,11 +242,11 @@ class ProcessorForceTorqueInertialPreint : public ProcessorMotion
virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
};
inline Eigen::VectorXd ProcessorForceTorqueInertialPreint::deltaZero() const
inline Eigen::VectorXd ProcessorForceTorqueInertial::deltaZero() const
{
return bodydynamics::fti::identity();
}
} /* namespace wolf */
#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_ */
#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ */
src/processor/processor_force_torque_inertial_preint.cpp → src/processor/processor_force_torque_inertial.cpp
+ 18
− 18
View file @ bb10d90b
......@@ -27,7 +27,7 @@
*/
// bodydynamics
#include "bodydynamics/processor/processor_force_torque_inertial_preint.h"
#include "bodydynamics/processor/processor_force_torque_inertial.h"
#include "bodydynamics/factor/factor_force_torque_inertial.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
......@@ -38,9 +38,9 @@
namespace wolf
{
ProcessorForceTorqueInertialPreint::ProcessorForceTorqueInertialPreint(
ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint)
: ProcessorMotion("ProcessorForceTorqueInertialPreint",
ProcessorForceTorqueInertial::ProcessorForceTorqueInertial(
ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial_preint)
: ProcessorMotion("ProcessorForceTorqueInertial",
"POVL",
3, // dim
13, // state size [p, q, v, L]
......@@ -54,12 +54,12 @@ ProcessorForceTorqueInertialPreint::ProcessorForceTorqueInertialPreint(
// TODO Auto-generated constructor stub
}
ProcessorForceTorqueInertialPreint::~ProcessorForceTorqueInertialPreint()
ProcessorForceTorqueInertial::~ProcessorForceTorqueInertial()
{
// TODO Auto-generated destructor stub
}
CaptureMotionPtr ProcessorForceTorqueInertialPreint::emplaceCapture(const FrameBasePtr& _frame_own,
CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
......@@ -75,30 +75,30 @@ CaptureMotionPtr ProcessorForceTorqueInertialPreint::emplaceCapture(const FrameB
return capture;
}
FeatureBasePtr ProcessorForceTorqueInertialPreint::emplaceFeature(CaptureMotionPtr _capture_own)
FeatureBasePtr ProcessorForceTorqueInertial::emplaceFeature(CaptureMotionPtr _capture_own)
{
FeatureBasePtr returnValue;
return returnValue;
}
FactorBasePtr ProcessorForceTorqueInertialPreint::emplaceFactor(FeatureBasePtr _feature_motion,
FactorBasePtr ProcessorForceTorqueInertial::emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin)
{
FactorBasePtr returnValue;
return returnValue;
}
void ProcessorForceTorqueInertialPreint::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
}
void ProcessorForceTorqueInertialPreint::configure(SensorBasePtr _sensor)
void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
{
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
}
void ProcessorForceTorqueInertialPreint::computeCurrentDelta(const Eigen::VectorXd& _data,
void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
......@@ -125,7 +125,7 @@ void ProcessorForceTorqueInertialPreint::computeCurrentDelta(const Eigen::Vector
_jacobian_calib = J_delta_tangent * J_tangent_calib;
}
void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1,
void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2) const
......@@ -133,7 +133,7 @@ void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1,
void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
const Eigen::VectorXd& _delta2,
const double _dt2,
Eigen::VectorXd& _delta1_plus_delta2,
......@@ -143,7 +143,7 @@ void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
void ProcessorForceTorqueInertialPreint::statePlusDelta(const VectorComposite& _x,
void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
VectorComposite& _x_plus_delta) const
......@@ -153,13 +153,13 @@ void ProcessorForceTorqueInertialPreint::statePlusDelta(const VectorComposite& _
_x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
}
Eigen::VectorXd ProcessorForceTorqueInertialPreint::correctDelta(const Eigen::VectorXd& _delta_preint,
Eigen::VectorXd ProcessorForceTorqueInertial::correctDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta_step) const
{
return fti::plus(_delta_preint, _delta_step);
}
VectorXd ProcessorForceTorqueInertialPreint::getCalibration(const CaptureBaseConstPtr _capture) const
VectorXd ProcessorForceTorqueInertial::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState(); // IMU bias
......@@ -171,6 +171,6 @@ VectorXd ProcessorForceTorqueInertialPreint::getCalibration(const CaptureBaseCon
#include <core/processor/factory_processor.h>
namespace wolf
{
WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreint);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreint);
WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertial);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertial);
} // namespace wolf
\ No newline at end of file
test/gtest_factor_force_torque_inertial.cpp
+ 3
− 3
View file @ bb10d90b
......@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial.h"
#include "bodydynamics/processor/processor_force_torque_inertial_preint.h"
#include "bodydynamics/processor/processor_force_torque_inertial.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
......@@ -46,7 +46,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
public:
ProblemPtr problem;
SensorForceTorqueInertialPtr sensor;
ProcessorForceTorqueInertialPreintPtr processor;
ProcessorForceTorqueInertialPtr processor;
FrameBasePtr frame_origin, frame_last, frame;
CaptureMotionPtr capture_origin, capture_last, capture;
FeatureMotionPtr feature;
......@@ -71,7 +71,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
problem = Problem::autoSetup(server);
sensor = std::static_pointer_cast<SensorForceTorqueInertial>(problem->getHardware()->getSensorList().front());
processor = std::static_pointer_cast<ProcessorForceTorqueInertialPreint>(sensor->getProcessorList().front());
processor = std::static_pointer_cast<ProcessorForceTorqueInertial>(sensor->getProcessorList().front());
problem->print(4, 1, 1, 1);
......
test/yaml/problem_force_torque_inertial.yaml
+ 1
− 1
View file @ bb10d90b
......@@ -32,7 +32,7 @@ config:
processors:
-
name: "proc FTI"
type: "ProcessorForceTorqueInertialPreint"
type: "ProcessorForceTorqueInertial"
sensor_name: "sensor FTI"
plugin: "bodydynamics"
follow: "processor_force_torque_inertial_preint.yaml"
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