diff --git a/include/bodydynamics/factor/factor_force_torque_inertial.h b/include/bodydynamics/factor/factor_force_torque_inertial.h
index 67055159d4ea85ad4b4602d7d5867873c29253f1..51c60184de4a56d64da4f20992a7ade895baea72 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial.h
@@ -191,7 +191,7 @@ inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>& _p1,
fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
- Matrix<T, 18, 1> delta_step = J_delta_bias_ * (_b1 - bias_preint_);
+ Matrix<T, 18, 1> delta_step = J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
_res = sqrt_info_upper_ * delta_err;
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 0d02421a33b5b4c0a1654c047ca4b1a1d59a8244..3e8754838f35f52c72f6d7e8f9e3aaffcae3d57e 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -35,10 +35,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorqueInertial);
struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
{
// noise std dev
- double acc_noise_std;
- double gyro_noise_std;
- double force_noise_std;
- double torque_noise_std;
+ double acc_noise_std;
+ double gyro_noise_std;
+ double force_noise_std;
+ double torque_noise_std;
+ Vector3d com;
+ Vector3d inertia;
+ double mass;
ParamsSensorForceTorqueInertial() = default;
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -48,6 +51,9 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
+ com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+ inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+ mass = _server.getParam<double>(prefix + _unique_name + "/mass");
}
~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override
@@ -56,7 +62,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
+ "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
- + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n"; //
+ + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ + "com: print not implemented." + "\n" //
+ + "inertia: print not implemented. \n" //
+ + "mass: " + std::to_string(mass) + "\n"; //
}
};
@@ -81,7 +90,7 @@ class SensorForceTorqueInertial : public SensorBase
double getMass() const; // Total mass
Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
Vector3d getCom() const; // centre of mass
- Vector7d getModel() const; // dynamic model [com, inertia, mass]
+ Vector7d getModel() const; // dynamic model [com, inertia, mass]
};
inline double SensorForceTorqueInertial::getMass() const
@@ -111,13 +120,13 @@ inline Vector3d SensorForceTorqueInertial::getInertia() const
inline Vector3d SensorForceTorqueInertial::getCom() const
{
- return getStateBlock('c')->getState();
+ return getStateBlock('C')->getState();
}
inline Vector7d SensorForceTorqueInertial::getModel() const
{
Vector7d model;
- model << getCom() , getInertia() , getMass();
+ model << getCom(), getInertia(), getMass();
return model;
}
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index a183868cdc5201a4e41c5f220388cf1be068c532..886ef10e9f015805d52da0dd49f12065068bfdb4 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -41,9 +41,12 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
false),
params_fti_(_params)
{
- addStateBlock('c', FactoryStateBlock::create("StateParams3", Vector3d::Ones(), true)); // centre of mass
- addStateBlock('i', FactoryStateBlock::create("StateParams3", Vector3d::Ones(), true)); // inertial vector
- addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d::Ones(), true)); // total mass
+ 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
+ setStateBlockDynamic('C', false);
+ setStateBlockDynamic('i', false);
+ setStateBlockDynamic('m', false);
}
// TODO: Adapt this API to that of MR !448
@@ -59,10 +62,10 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&
false),
params_fti_(_params)
{
- auto sbc = emplaceStateBlock<StateParams3>('c', getProblem(), _states.at('c'), true); // centre of mass
+ auto sbc = emplaceStateBlock<StateParams3>('C', getProblem(), _states.at('C'), true); // centre of mass
auto sbi = emplaceStateBlock<StateParams3>('i', getProblem(), _states.at('i'), true); // inertial vector
auto sbm = emplaceStateBlock<StateParams1>('m', getProblem(), _states.at('m'), true); // total mass
- setStateBlockDynamic('c', false);
+ setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 7277dfd7e58e71c6f2997fa98fe44b894e61670c..7ca4c35a861ceac10989f6c0b0c746b6f1888f3e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -20,6 +20,8 @@ wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_inertial.cpp)
+wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp)
+
wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inertial_delta_tools.cpp)
diff --git a/test/yaml/sensor_force_torque_inertial.yaml b/test/yaml/sensor_force_torque_inertial.yaml
index b819837c8a68ea2cd892692bfca9737dc9dabf7e..140459814a7b017a720dc0a9f72aec6aa30b11ea 100644
--- a/test/yaml/sensor_force_torque_inertial.yaml
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -1,4 +1,8 @@
-force_noise_std: 0.1 # std dev of force noise in N
-torque_noise_std: 0.1 # std dev of torque noise in N/m
-acc_noise_std: 0.01 # std dev of acc noise in m/s2
-gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
\ No newline at end of file
+force_noise_std: 0.1 # std dev of force noise in N
+torque_noise_std: 0.1 # std dev of torque noise in N/m
+acc_noise_std: 0.01 # std dev of acc noise in m/s2
+gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+
+com: [0,0,0.0341] # center of mass [m]
+inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+mass: 1.952 # mass [kg]
\ No newline at end of file