diff --git a/CMakeLists.txt b/CMakeLists.txt index 20dd4225a5854284cf96ed0cfb27126d285349f5..520805516aff79c903e1d431833c20db1e1d8a25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,7 @@ include/${PROJECT_NAME}/math/force_torque_inertial_delta_tools.h ) SET(HDRS_CAPTURE include/${PROJECT_NAME}/capture/capture_force_torque_preint.h +include/${PROJECT_NAME}/capture/capture_force_torque_inertial.h include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h include/${PROJECT_NAME}/capture/capture_leg_odom.h include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h @@ -139,6 +140,7 @@ include/${PROJECT_NAME}/sensor/sensor_point_feet_nomove.h # ============ SOURCES ============ SET(SRCS_CAPTURE src/capture/capture_force_torque_preint.cpp +src/capture/capture_force_torque_inertial.cpp src/capture/capture_inertial_kinematics.cpp src/capture/capture_leg_odom.cpp src/capture/capture_point_feet_nomove.cpp diff --git a/include/bodydynamics/capture/capture_force_torque_inertial.h b/include/bodydynamics/capture/capture_force_torque_inertial.h new file mode 100644 index 0000000000000000000000000000000000000000..9bef3980c4850136192e2af8612b28d1f0a0bd5c --- /dev/null +++ b/include/bodydynamics/capture/capture_force_torque_inertial.h @@ -0,0 +1,23 @@ +#ifndef CAPTURE_FORCE_TORQUE_INERTIAL_H +#define CAPTURE_FORCE_TORQUE_INERTIAL_H + +#include <core/capture/capture_motion.h> + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial); + +class CaptureForceTorqueInertial : public CaptureMotion +{ + CaptureForceTorqueInertial(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr = nullptr); + virtual ~CaptureForceTorqueInertial(); +}; + +} // namespace wolf + +#endif // CAPTURE_FORCE_TORQUE_INERTIAL_H \ No newline at end of file diff --git a/src/capture/capture_force_torque_inertial.cpp b/src/capture/capture_force_torque_inertial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6a15c13a51ae57e098aec6c903844ce4ec9814e6 --- /dev/null +++ b/src/capture/capture_force_torque_inertial.cpp @@ -0,0 +1,29 @@ +#include "bodydynamics/capture/capture_force_torque_inertial.h" + +#include <core/state_block/state_block_derived.h> + +namespace wolf +{ +CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureForceTorqueInertial", + _init_ts, + _sensor_ptr, + _data, + _data_cov, + _capture_origin_ptr, + nullptr, // position is static + nullptr, // orientation is static + (_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias + ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlock('I')->getState(), false) + : nullptr)) +{ + // +} + +CaptureForceTorqueInertial::~CaptureForceTorqueInertial() {} + +} // namespace wolf \ No newline at end of file