From 287b28e19287acbdb96ff9d5dd64d9a2abf24133 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 7 Aug 2022 22:00:20 +0200
Subject: [PATCH] Add IMU drift params to SensorFTI
---
include/bodydynamics/sensor/sensor_force_torque_inertial.h | 7 +++++++
...blem_force_torque_inertial_dynamics_processor_test.yaml | 2 ++
.../problem_force_torque_inertial_dynamics_solve_test.yaml | 7 ++++++-
test/yaml/sensor_force_torque_inertial.yaml | 2 ++
4 files changed, 17 insertions(+), 1 deletion(-)
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 3e87548..2d509f8 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
// noise std dev
double acc_noise_std;
double gyro_noise_std;
+ double acc_drift_std;
+ double gyro_drift_std;
double force_noise_std;
double torque_noise_std;
Vector3d com;
@@ -49,6 +51,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
{
acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
+ acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
+ gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_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");
@@ -61,6 +65,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
return ParamsSensorBase::print() + "\n" //
+ "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
+ + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
+ "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
+ "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ "com: print not implemented." + "\n" //
@@ -91,6 +97,7 @@ class SensorForceTorqueInertial : public SensorBase
Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
Vector3d getCom() const; // centre of mass
Vector7d getModel() const; // dynamic model [com, inertia, mass]
+ ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;}
};
inline double SensorForceTorqueInertial::getMass() const
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
index 2eef41c..5ef4ecb 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -31,6 +31,8 @@ config:
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
+ acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(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]
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 312976d..182966b 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -31,11 +31,16 @@ config:
plugin: "bodydynamics"
extrinsic:
pose: [0,0,0, 0,0,0,1]
+
+ # IMU
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
-
+ acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+
+ # Dynamics
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.9 # mass [kg]
diff --git a/test/yaml/sensor_force_torque_inertial.yaml b/test/yaml/sensor_force_torque_inertial.yaml
index 1404598..e6d323b 100644
--- a/test/yaml/sensor_force_torque_inertial.yaml
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -2,6 +2,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
+acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
+gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(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]
--
GitLab