From f981d59338f609f64ce5d2e93e24ad80c626862f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 8 Aug 2022 12:30:50 +0200
Subject: [PATCH] New class CaptureForceTorqueInertial
---
CMakeLists.txt | 2 ++
.../capture/capture_force_torque_inertial.h | 23 +++++++++++++++
src/capture/capture_force_torque_inertial.cpp | 29 +++++++++++++++++++
3 files changed, 54 insertions(+)
create mode 100644 include/bodydynamics/capture/capture_force_torque_inertial.h
create mode 100644 src/capture/capture_force_torque_inertial.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 20dd422..5208055 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 0000000..9bef398
--- /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 0000000..6a15c13
--- /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
--
GitLab