New class CaptureForceTorqueInertial

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

include/bodydynamics/capture/capture_force_torque_inertial.h

+#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

src/capture/capture_force_torque_inertial.cpp

+#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