From 748a9ff8db1673ba70379972170d802d7ce328d4 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 6 Feb 2025 13:47:59 +0100
Subject: [PATCH] [skip ci] pragma once and primary work
---
.../capture/capture_force_torque_preint.h | 11 ++--
.../capture/capture_inertial_kinematics.h | 11 ++--
.../bodydynamics/capture/capture_leg_odom.h | 11 ++--
.../capture/capture_point_feet_nomove.h | 9 ++--
.../bodydynamics/factor/factor_force_torque.h | 7 +--
.../factor/factor_force_torque_preint.h | 11 ++--
.../factor/factor_inertial_kinematics.h | 9 ++--
.../factor/factor_point_feet_altitude.h | 9 ++--
.../factor/factor_point_feet_nomove.h | 11 ++--
.../factor/factor_point_feet_zero_velocity.h | 9 ++--
.../feature/feature_force_torque.h | 9 ++--
.../feature/feature_force_torque_preint.h | 7 +--
.../feature/feature_inertial_kinematics.h | 9 ++--
.../math/force_torque_delta_tools.h | 17 ++----
.../processor/processor_force_torque_preint.h | 7 +--
.../processor/processor_inertial_kinematics.h | 25 +++------
.../processor/processor_point_feet_nomove.h | 19 ++-----
.../bodydynamics/sensor/sensor_force_torque.h | 42 +++------------
.../sensor/sensor_inertial_kinematics.h | 48 +++++------------
.../sensor/sensor_point_feet_nomove.h | 15 ++----
schema/sensor/SensorForceTorque.schema | 16 ++++++
schema/sensor/SensorInertialKinematics.schema | 41 +++++++++++++++
src/sensor/sensor_force_torque.cpp | 52 ++++++++-----------
src/sensor/sensor_inertial_kinematics.cpp | 44 ++++++----------
24 files changed, 180 insertions(+), 269 deletions(-)
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index 98e5b23..35c9805 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -18,15 +18,14 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef CAPTURE_FORCE_TORQUE_PREINT_H
-#define CAPTURE_FORCE_TORQUE_PREINT_H
+#pragma once
// Wolf includes
#include "bodydynamics/math/force_torque_delta_tools.h"
-#include "core/capture/capture_base.h"
-#include "core/capture/capture_motion.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_motion.h>
#include <core/state_block/vector_composite.h>
namespace wolf
@@ -82,6 +81,4 @@ class CaptureForceTorquePreint : public CaptureMotion
CaptureBasePtr cap_gyro_other_;
};
-} // namespace wolf
-
-#endif // CAPTURE_FORCE_TORQUE_PREINT_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/capture/capture_inertial_kinematics.h b/include/bodydynamics/capture/capture_inertial_kinematics.h
index 2e21e91..4b968fd 100644
--- a/include/bodydynamics/capture/capture_inertial_kinematics.h
+++ b/include/bodydynamics/capture/capture_inertial_kinematics.h
@@ -18,13 +18,12 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef CAPTURE_INERTIAL_KINEMATICS_H
-#define CAPTURE_INERTIAL_KINEMATICS_H
+#pragma once
// Wolf includes
#include "bodydynamics/math/force_torque_delta_tools.h"
-#include "core/capture/capture_motion.h"
-#include "core/capture/capture_base.h"
+#include <core/capture/capture_motion.h>
+#include <core/capture/capture_base.h>
namespace wolf
{
@@ -66,6 +65,4 @@ class CaptureInertialKinematics : public CaptureBase
Eigen::Matrix3d B_I_q_;
Eigen::Vector3d B_Lc_m_;
};
-} // namespace wolf
-
-#endif // CAPTURE_INERTIAL_KINEMATICS_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/capture/capture_leg_odom.h b/include/bodydynamics/capture/capture_leg_odom.h
index 9ca8a3d..9d89d41 100644
--- a/include/bodydynamics/capture/capture_leg_odom.h
+++ b/include/bodydynamics/capture/capture_leg_odom.h
@@ -18,12 +18,11 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef CAPTURE_LEG_ODOM_H
-#define CAPTURE_LEG_ODOM_H
+#pragma once
// Wolf includes
-#include "core/capture/capture_base.h"
-#include "core/capture/capture_motion.h"
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_motion.h>
namespace wolf
{
@@ -59,6 +58,4 @@ class CaptureLegOdom : public CaptureMotion
// return
// }
-} // namespace wolf
-
-#endif // CAPTURE_LEG_ODOM_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/capture/capture_point_feet_nomove.h b/include/bodydynamics/capture/capture_point_feet_nomove.h
index 52a0186..817e34a 100644
--- a/include/bodydynamics/capture/capture_point_feet_nomove.h
+++ b/include/bodydynamics/capture/capture_point_feet_nomove.h
@@ -18,11 +18,10 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef CAPTURE_POINT_FEET_NOMOVE_H
-#define CAPTURE_POINT_FEET_NOMOVE_H
+#pragma once
// Wolf includes
-#include "core/capture/capture_base.h"
+#include <core/capture/capture_base.h>
namespace wolf
{
@@ -41,6 +40,4 @@ class CapturePointFeetNomove : public CaptureBase
std::unordered_map<int, Eigen::Vector7d> kin_incontact_;
};
-} // namespace wolf
-
-#endif // CAPTURE_POINT_FEET_NOMOVE_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 93b02ed..91c2c33 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -18,8 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_FORCE_TORQUE_H_
-#define FACTOR_FORCE_TORQUE_H_
+#pragma once
#include <core/math/rotations.h>
#include <core/math/covariance.h>
@@ -259,6 +258,4 @@ bool FactorForceTorque::operator()(const T* const _ck,
return true;
}
-} // namespace wolf
-
-#endif /* FACTOR_FORCE_TORQUE_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
index c0ff79c..c0143ef 100644
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -18,15 +18,14 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_
+#pragma once
// Wolf includes
#include "bodydynamics/capture/capture_force_torque_preint.h"
#include "bodydynamics/feature/feature_force_torque_preint.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/math/rotations.h"
+#include <core/factor/factor_autodiff.h>
+#include <core/math/rotations.h>
// Eigen include
@@ -349,6 +348,4 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1>& _c1,
// Matrix<double,12,1> toto = residual();
// }
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index 7dfb400..ba56ce9 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -18,10 +18,9 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_INERTIAL_KINEMATICS_H_
-#define FACTOR_INERTIAL_KINEMATICS_H_
+#pragma once
-#include "core/factor/factor_autodiff.h"
+#include <core/factor/factor_autodiff.h>
#include "bodydynamics/feature/feature_inertial_kinematics.h"
namespace wolf
@@ -169,6 +168,4 @@ bool FactorInertialKinematics::operator()(const T* const _pb,
return true;
}
-} // namespace wolf
-
-#endif /* FACTOR__INERTIAL_KINEMATICS_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_point_feet_altitude.h b/include/bodydynamics/factor/factor_point_feet_altitude.h
index 6708eb2..8b8649e 100644
--- a/include/bodydynamics/factor/factor_point_feet_altitude.h
+++ b/include/bodydynamics/factor/factor_point_feet_altitude.h
@@ -18,10 +18,9 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_POINT_FEET_ALTITUDE_H_
-#define FACTOR_POINT_FEET_ALTITUDE_H_
+#pragma once
-#include "core/factor/factor_autodiff.h"
+#include <core/factor/factor_autodiff.h>
namespace wolf
{
@@ -120,6 +119,4 @@ bool FactorPointFeetAltitude::operator()(const T* const _pb, const T* const _qb,
return true;
}
-} // namespace wolf
-
-#endif /* FACTOR__POINT_FEET_ALTITUDE_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index 889c509..52ececc 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -18,11 +18,10 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_POINT_FEET_NOMOVE_H_
-#define FACTOR_POINT_FEET_NOMOVE_H_
+#pragma once
-#include "core/math/rotations.h"
-#include "core/factor/factor_autodiff.h"
+#include <core/math/rotations.h>
+#include <core/factor/factor_autodiff.h>
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
namespace wolf
@@ -173,6 +172,4 @@ bool FactorPointFeetNomove::operator()(const T* const _pb,
return true;
}
-} // namespace wolf
-
-#endif /* FACTOR__POINT_FEET_NOMOVE_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
index da26c4f..8665676 100644
--- a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
+++ b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
@@ -18,10 +18,9 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FACTOR_POINT_FEET_ZERO_VELOCITY_H_
-#define FACTOR_POINT_FEET_ZERO_VELOCITY_H_
+#pragma once
-#include "core/factor/factor_autodiff.h"
+#include <core/factor/factor_autodiff.h>
namespace wolf
{
@@ -103,6 +102,4 @@ bool FactorPointFeetNomove::operator()(const T* const _vb, const T* const _qb, c
return true;
}
-} // namespace wolf
-
-#endif /* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index 4bd9a52..952f38e 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -18,11 +18,10 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FEATURE_FORCE_TORQUE_H_
-#define FEATURE_FORCE_TORQUE_H_
+#pragma once
// Wolf includes
-#include "core/feature/feature_base.h"
+#include <core/feature/feature_base.h>
// std includes
namespace wolf
@@ -142,6 +141,4 @@ class FeatureForceTorque : public FeatureBase
Eigen::Matrix<double, 14, 14> cov_kin_meas_;
};
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h
index ffe5e2a..cbfc43f 100644
--- a/include/bodydynamics/feature/feature_force_torque_preint.h
+++ b/include/bodydynamics/feature/feature_force_torque_preint.h
@@ -18,8 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FEATURE_FORCE_TORQUE_PREINT_H_
-#define FEATURE_FORCE_TORQUE_PREINT_H_
+#pragma once
// Wolf includes
#include <core/feature/feature_base.h>
@@ -87,6 +86,4 @@ inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobian
return J_delta_bias_;
}
-} // namespace wolf
-
-#endif // FEATURE_FORCE_TORQUE_PREINT_H_
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/feature/feature_inertial_kinematics.h b/include/bodydynamics/feature/feature_inertial_kinematics.h
index 7069549..9c7ca1e 100644
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
@@ -18,11 +18,10 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef FEATURE_INERTIAL_KINEMATICS_H_
-#define FEATURE_INERTIAL_KINEMATICS_H_
+#pragma once
// Wolf includes
-#include "core/feature/feature_base.h"
+#include <core/feature/feature_base.h>
// std includes
namespace wolf
@@ -83,6 +82,4 @@ void recomputeIKinCov(const Eigen::Matrix3d& Qp,
const Eigen::Vector3d& w_unb,
const Eigen::Matrix3d& Iq);
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/math/force_torque_delta_tools.h b/include/bodydynamics/math/force_torque_delta_tools.h
index 3ca1ed4..c6479a2 100644
--- a/include/bodydynamics/math/force_torque_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_delta_tools.h
@@ -17,18 +17,11 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * imu_3d_tools.h
- *
- * Created on: Feb 17, 2020
- * Author: mfourmy
- */
-#ifndef FORCE_TORQUE_DELTA_TOOLS_H_
-#define FORCE_TORQUE_DELTA_TOOLS_H_
+#pragma once
-#include "core/common/wolf.h"
-#include "core/math/rotations.h"
+#include <core/common/wolf.h>
+#include <core/math/rotations.h>
/*
* Most functions in this file are explained in the document:
@@ -834,6 +827,4 @@ inline void debiasData(const MatrixBase<D1>& _data,
}
} // namespace bodydynamics
-} // namespace wolf
-
-#endif /* FORCE_TORQUE_DELTA_TOOLS_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index 6f81bf9..a7222ed 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -18,8 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef PROCESSOR_FORCE_TORQUE_PREINT_H
-#define PROCESSOR_FORCE_TORQUE_PREINT_H
+#pragma once
// Wolf core
#include <core/processor/processor_motion.h>
@@ -133,6 +132,4 @@ inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const
.finished(); // com, com vel, ang momentum, orientation
}
-} // namespace wolf
-
-#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
index f872f34..60c7220 100644
--- a/include/bodydynamics/processor/processor_inertial_kinematics.h
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -18,16 +18,15 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef PROCESSOR_INERTIAL_KINEMATICS_H
-#define PROCESSOR_INERTIAL_KINEMATICS_H
+#pragma once
// Wolf
-#include "core/sensor/sensor_base.h"
-#include "core/processor/processor_base.h"
+#include <core/sensor/sensor_base.h>
+#include <core/processor/processor_base.h>
+#include <core/processor/processor_base.h>
+#include <core/factor/factor_base.h>
+#include <core/factor/factor_block_difference.h>
-#include "core/processor/processor_base.h"
-#include "core/factor/factor_base.h"
-#include "core/factor/factor_block_difference.h"
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/feature/feature_inertial_kinematics.h"
@@ -126,14 +125,4 @@ Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
const Eigen::Vector3d& w_unb,
const Eigen::Matrix3d& Iq);
-} /* namespace wolf */
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-namespace wolf
-{
-} // namespace wolf
-
-#endif // PROCESSOR_INERTIAL_KINEMATICS_H
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
index 4e8b8a4..c10a6d3 100644
--- a/include/bodydynamics/processor/processor_point_feet_nomove.h
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -18,12 +18,11 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef PROCESSOR_POINT_FEET_NOMOVE_H
-#define PROCESSOR_POINT_FEET_NOMOVE_H
+#pragma once
// Wolf
-#include "core/sensor/sensor_base.h"
-#include "core/processor/processor_base.h"
+#include <core/sensor/sensor_base.h>
+#include <core/processor/processor_base.h>
#include "bodydynamics/capture/capture_point_feet_nomove.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
@@ -107,14 +106,4 @@ inline bool ProcessorPointFeetNomove::storeCapture(CaptureBasePtr)
return true;
}
-} /* namespace wolf */
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-namespace wolf
-{
-} // namespace wolf
-
-#endif // PROCESSOR_POINT_FEET_NOMOVE_H
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
index 2778976..5b00008 100644
--- a/include/bodydynamics/sensor/sensor_force_torque.h
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -18,40 +18,13 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef SENSOR_FORCE_TORQUE_H
-#define SENSOR_FORCE_TORQUE_H
+#pragma once
// wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
-#include <iostream>
+#include <core/sensor/sensor_base.h>
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorque);
-
-struct ParamsSensorForceTorque : public ParamsSensorBase
-{
- // noise std dev
- double mass = 10; // total mass of the robot (kg)
- double std_f = 0.001; // standard deviation of the force sensors (N)
- double std_tau = 0.001; // standard deviation of the torque sensors (N.m)
-
- ParamsSensorForceTorque() = default;
- ParamsSensorForceTorque(std::string _unique_name, const ParamsServer& _server)
- : ParamsSensorBase(_unique_name, _server)
- {
- mass = _server.getParam<double>(_unique_name + "/mass");
- std_f = _server.getParam<double>(_unique_name + "/std_f");
- std_tau = _server.getParam<double>(_unique_name + "/std_tau");
- }
- ~ParamsSensorForceTorque() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" + "mass: " + toString(mass) + "\n" + "std_f: " + toString(std_f) +
- "\n" + "std_tau: " + toString(std_tau) + "\n";
- }
-};
WOLF_PTR_TYPEDEFS(SensorForceTorque);
@@ -64,14 +37,15 @@ class SensorForceTorque : public SensorBase
double std_tau_; // standard deviation of the torque sensors (N.m)
public:
- SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params);
- ~SensorForceTorque() override = default;
+ SensorForceTorque(const YAML::Node &_params);
- WOLF_SENSOR_CREATE(SensorForceTorque, ParamsSensorForceTorque, 0);
+ WOLF_SENSOR_CREATE(SensorForceTorque);
double getMass() const;
double getForceNoiseStd() const;
double getTorqueNoiseStd() const;
+
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
inline double SensorForceTorque::getMass() const
@@ -89,6 +63,4 @@ inline double SensorForceTorque::getTorqueNoiseStd() const
return std_tau_;
}
-} // namespace wolf
-
-#endif // SENSOR_FORCE_TORQUE_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
index 4ba9958..08d9aea 100644
--- a/include/bodydynamics/sensor/sensor_inertial_kinematics.h
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -18,66 +18,42 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef SENSOR_INERTIAL_KINEMATICS_H
-#define SENSOR_INERTIAL_KINEMATICS_H
+#pragma once
// wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
-#include <iostream>
+#include <core/sensor/sensor_base.h>
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorInertialKinematics);
-
-struct ParamsSensorInertialKinematics : public ParamsSensorBase
-{
- // noise std dev
- double std_pbc; // standard deviation of the com position measurement relative to the base frame (m)
- double std_vbc; // standard deviation of the com velocity measurement relative to the base frame (m/s)
-
- ParamsSensorInertialKinematics() = default;
- ParamsSensorInertialKinematics(std::string _unique_name, const ParamsServer& _server)
- : ParamsSensorBase(_unique_name, _server)
- {
- std_pbc = _server.getParam<double>(_unique_name + "/std_pbc");
- std_vbc = _server.getParam<double>(_unique_name + "/std_vbc");
- }
- ~ParamsSensorInertialKinematics() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" + "std_pbc: " + toString(std_pbc) + "\n" +
- "std_vbc: " + toString(std_vbc) + "\n";
- }
-};
WOLF_PTR_TYPEDEFS(SensorInertialKinematics);
class SensorInertialKinematics : public SensorBase
{
protected:
- ParamsSensorInertialKinematicsPtr intr_ikin_;
+ // noise std dev
+ double std_pbc_; // standard deviation of the com position measurement relative to the base frame (m)
+ double std_vbc_; // standard deviation of the com velocity measurement relative to the base frame (m/s)
public:
- SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _intr_ikin);
- ~SensorInertialKinematics() override;
+ SensorInertialKinematics(const YAML::Node &_params);
- WOLF_SENSOR_CREATE(SensorInertialKinematics, ParamsSensorInertialKinematics, 0);
+ WOLF_SENSOR_CREATE(SensorInertialKinematics);
double getPbcNoiseStd() const;
double getVbcNoiseStd() const;
+
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
inline double SensorInertialKinematics::getPbcNoiseStd() const
{
- return intr_ikin_->std_pbc;
+ return std_pbc_;
}
inline double SensorInertialKinematics::getVbcNoiseStd() const
{
- return intr_ikin_->std_vbc;
+ return std_vbc_;
}
-} // namespace wolf
-
-#endif // SENSOR_INERTIAL_KINEMATICS_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/bodydynamics/sensor/sensor_point_feet_nomove.h b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
index 5cae493..3dc89cb 100644
--- a/include/bodydynamics/sensor/sensor_point_feet_nomove.h
+++ b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
@@ -18,13 +18,10 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#ifndef SENSOR_POINT_FEET_NOMOVE_H
-#define SENSOR_POINT_FEET_NOMOVE_H
+#pragma once
// wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
-#include <iostream>
+#include <core/sensor/sensor_base.h>
namespace wolf
{
@@ -34,7 +31,7 @@ struct ParamsSensorPointFeetNomove : public ParamsSensorBase
{
// standard deviation on the assumption that the feet are not moving when in contact
// the noise represents a random walk on the foot position, hence the unit
- double std_foot_nomove_; // m/(s^2 sqrt(dt))
+ double std_foot_nomove_; // m/s^2/sqrt(Hz)
// certainty on current ground altitude
double std_altitude_ = 1000; // m, deactivated by default
double foot_radius_ = 0; // m
@@ -63,7 +60,7 @@ class SensorPointFeetNomove : public SensorBase
protected:
Matrix3d cov_foot_nomove_; // random walk covariance in (m/s^2/sqrt(Hz))^2
Matrix1d cov_altitude_; // m^2
- double foot_radius_;
+ double foot_radius_; // m
public:
SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics, const ParamsSensorPointFeetNomovePtr& _intr_pfnm);
@@ -91,6 +88,4 @@ inline double SensorPointFeetNomove::getFootRadius() const
return foot_radius_;
}
-} // namespace wolf
-
-#endif // SENSOR_POINT_FEET_NOMOVE_H
+} // namespace wolf
\ No newline at end of file
diff --git a/schema/sensor/SensorForceTorque.schema b/schema/sensor/SensorForceTorque.schema
index e69de29..30e201a 100644
--- a/schema/sensor/SensorForceTorque.schema
+++ b/schema/sensor/SensorForceTorque.schema
@@ -0,0 +1,16 @@
+follow: SensorBase.schema
+
+mass:
+ _type: double
+ _mandatory: true
+ _doc: "total mass of the robot (kg)"
+
+std_f:
+ _type: double
+ _mandatory: true
+ _doc: "standard deviation of the force sensors (N)"
+
+std_tau:
+ _type: double
+ _mandatory: true
+ _doc: "standard deviation of the torque sensors (N.m)"
\ No newline at end of file
diff --git a/schema/sensor/SensorInertialKinematics.schema b/schema/sensor/SensorInertialKinematics.schema
index e69de29..ea62252 100644
--- a/schema/sensor/SensorInertialKinematics.schema
+++ b/schema/sensor/SensorInertialKinematics.schema
@@ -0,0 +1,41 @@
+follow: SensorBase.schema
+
+states:
+ I:
+ dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the bias are dynamic, i.e. they change along time.
+
+ value:
+ _type: Vector3d
+ _mandatory: false
+ _default: [0,0,0]
+ _doc: "A vector containing the IMU bias values. Default: zeros"
+
+ prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: "Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values."
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: "A vector containing the stdev values of the noise of the prior factor."
+
+ drift_std:
+ _type: Vector3d
+ _mandatory: false
+ _doc: "A vector containing the stdev rate values of the noise of the drift factors (only if dynamic==true)."
+
+std_pbc:
+ _type: double
+ _mandatory: true
+ _doc: "standard deviation of the com position measurement relative to the base frame (m)"
+
+std_vbc:
+ _type: double
+ _mandatory: true
+ _doc: "standard deviation of the com velocity measurement relative to the base frame (m/s)"
\ No newline at end of file
diff --git a/src/sensor/sensor_force_torque.cpp b/src/sensor/sensor_force_torque.cpp
index f9fa17f..4b3cc4d 100644
--- a/src/sensor/sensor_force_torque.cpp
+++ b/src/sensor/sensor_force_torque.cpp
@@ -25,39 +25,33 @@
namespace wolf
{
-SensorForceTorque::SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params)
- : SensorBase("SensorForceTorque",
- nullptr,
- nullptr,
- nullptr,
- (Eigen::Matrix<double, 12, 1>() << _params->std_f,
- _params->std_f,
- _params->std_f,
- _params->std_f,
- _params->std_f,
- _params->std_f,
- _params->std_tau,
- _params->std_tau,
- _params->std_tau,
- _params->std_tau,
- _params->std_tau,
- _params->std_tau)
- .finished(),
- false,
- false,
- true),
- mass_(_params->mass),
- std_f_(_params->std_f),
- std_tau_(_params->std_tau)
+SensorForceTorque::SensorForceTorque(const YAML::Node &_params)
+ : SensorBase("SensorForceTorque", TypeComposite{}, _params),
+ mass_(_params["mass"].as<double>()),
+ std_f_(_params["std_f"].as<double>()),
+ std_tau_(_params["std_tau"].as<double>())
{
- assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0");
}
-} // namespace wolf
+Eigen::MatrixXd SensorForceTorque::computeNoiseCov(const Eigen::VectorXd& _data) const
+{
+ return (Eigen::Matrix<double, 12, 1>() << _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau)
+ .finished()
+ .cwiseAbs2()
+ .asDiagonal();
+}
// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf
-{
WOLF_REGISTER_SENSOR(SensorForceTorque)
} // namespace wolf
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index ef4f5e0..668cd66 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -20,32 +20,13 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
namespace wolf
{
-SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extrinsics,
- ParamsSensorInertialKinematicsPtr _params)
- : SensorBase("SensorInertialKinematics",
- nullptr, // no position
- nullptr, // no orientation
- std::make_shared<StateBlock>(Eigen::Vector3d(0, 0, 0), false), // bias; false = unfixed
- (Eigen::Vector6d() << _params->std_pbc,
- _params->std_pbc,
- _params->std_pbc,
- _params->std_vbc,
- _params->std_vbc,
- _params->std_vbc)
- .finished(),
- false,
- false,
- true),
- intr_ikin_(_params)
+SensorInertialKinematics::SensorInertialKinematics(const YAML::Node &_params)
+ : SensorBase("SensorInertialKinematics", TypeComposite{'I', "StateParams3"}, _params),
+ std_pbc_(_params["std_pbc"].as<double>()),
+ std_vbc_(_params["std_vbc"].as<double>())
{
- assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0.");
- assert(getIntrinsic()->getState().size() == 3 && "Wrong extrinsics size! Should be 3.");
- setStateBlockDynamic('I', true);
}
SensorInertialKinematics::~SensorInertialKinematics()
@@ -53,11 +34,20 @@ SensorInertialKinematics::~SensorInertialKinematics()
//
}
-} // namespace wolf
+Eigen::MatrixXd SensorInertialKinematics::computeNoiseCov(const Eigen::VectorXd& _data) const
+{
+ return (Eigen::Vector6d() << _params->std_pbc,
+ _params->std_pbc,
+ _params->std_pbc,
+ _params->std_vbc,
+ _params->std_vbc,
+ _params->std_vbc)
+ .finished()
+ .cwiseAbs2()
+ .asDiagonal();
+}
// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf
-{
WOLF_REGISTER_SENSOR(SensorInertialKinematics)
+
} // namespace wolf
--
GitLab