From 460667de86a30581a2f737ede75aa75d1dbf02cb Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Fri, 23 Sep 2022 17:03:27 +0200
Subject: [PATCH] Add yanml params to fix calibration states
---
.../bodydynamics/sensor/sensor_force_torque_inertial.h | 5 +++++
src/sensor/sensor_force_torque_inertial.cpp | 8 ++++----
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index d9cff87..21f4295 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -46,6 +46,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
Vector3d com;
Vector3d inertia;
double mass;
+ bool imu_bias_fix, com_fix, inertia_fix, mass_fix;
ParamsSensorForceTorqueInertial() = default;
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -62,6 +63,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ imu_bias_fix = _server.getParam<bool> (prefix + _unique_name + "/imu_bias_fix");
+ com_fix = _server.getParam<bool> (prefix + _unique_name + "/com_fix");
+ inertia_fix = _server.getParam<bool> (prefix + _unique_name + "/inertia_fix");
+ mass_fix = _server.getParam<bool> (prefix + _unique_name + "/mass_fix");
}
~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 2540ece..0c3a35d 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -34,16 +34,16 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
: SensorBase("SensorForceTorqueInertial",
FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), true),
FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), true),
- FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false), // IMU bias
+ FactoryStateBlock::create("StateParams6", Vector6d::Zero(), _params->imu_bias_fix), // IMU bias
12,
false,
false,
false),
params_fti_(_params)
{
- addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass
- addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true)); // inertial vector
- addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true)); // total mass
+ addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix)); // centre of mass
+ addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix)); // inertial vector
+ addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix)); // total mass
setStateBlockDynamic('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
--
GitLab