diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index 98e5b237e336b7a4285876f0e3ce7e16cff6844c..35c98059dd7e79e48a7ed58933d2da8df57c687a 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 2e21e919dce2e2c5d9dd0eade1c8dff3cbe30bae..4b968fdae51ae304870cc20738b349b68aa552fa 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 9ca8a3d9c1f06fb8fe4284a37d19616e9c3692d1..9d89d4106fe37dab4125d7fd4ebfe1906b39507f 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 52a01860a376c9ba8e635fff29dd92470f6bb673..817e34a2b48b8aa9d8cf5a634c1e4ef505ab3921 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 93b02ed1799ad7f301763c8dd529a246c94f1c60..91c2c3395725f94f7cdb0e9dbf48d7895e504769 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 c0ff79cd0f2b1ff950a457e720f1b27289acd5e1..c0143ef83fc05b0a7a1560b553a7a1be56fa6fb4 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 7dfb400ba64e17f79f92ada4272a647c9edb6de5..ba56ce9ed307bde4f37994b4eea3efa4ab91895b 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 6708eb2ea8d14c51fca4302723db21db2c49af35..8b8649eb5099e0a45a777e4ff571499a762bbd9e 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 889c5093f0670dba7aded9dd394526a87c9fd82b..52ececc849b513df2261ba618d77ea79b68fdadd 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 da26c4f475ce6121ace298297baa4977fe637331..8665676995a24678b543d01df3de8cef4511358e 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 4bd9a523675ed632626e9f61cb606afa8633cef8..952f38eb6579ad456eed68fcb0bc1eab5aa73d72 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 ffe5e2aa44b78d6ed397cbca13518f5bdbb7a484..cbfc43fe0d2cbdd4e8b8a6b4e804340200b3cfff 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 7069549ff49ccccf1985492fe258ef7f21dcfce2..9c7ca1e4905b8e43df7e65f918ed26350ec4e314 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 3ca1ed40d50f3d81091e22358351adbb70e4689b..c6479a2190c85b92ec58646e4d2589a6130162d1 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 6f81bf938af8aa622a98fd3e6672063642a3d7e8..a7222ed304669c3c5fb09b6361f3a71298ee2671 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 f872f3496981f3dfe113db12740a4e5ef812395b..60c7220d69e04336a75f76b352b5d8cc1e338ea5 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 4e8b8a4893dd4bf083a6a8fbc07a770f19e07e31..c10a6d3adb1b55b3d4caf3381e382da8a37f14dd 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 27789760c1f59b6ad37fbbc94ac560703c0a3162..5b00008fc4294ced016e533baa8e688817b04639 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 4ba99580c5c339a69c290090fc9b56cb4500d595..08d9aea9d565e09602a396e9456744833a256ad7 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 5cae4934acba6a2be21e0491fa75189826999ac9..3dc89cb5e60e760e1f61e33075eade90774d587d 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 e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..30e201a47edc7cd7f112fbaaaa1ce9c0128f65b4 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 e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..ea62252dba2c983f28d822080e008bff73d41819 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 f9fa17f711c615f53205ae8507a089f60f3556f6..4b3cc4d62e8cf4aaaafacf8ef93f9a804f74a2ac 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 ef4f5e0c7b1239d6f39f21a95f154c7418d8086a..668cd667f10fbf7f1c7597226a3f3ca85a98f486 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