diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
index 345a4c24c024304b1cff2d390fed3d47a93bf0cc..a5e92bb4a9f3651b006decc49acba2a6ec33e3e8 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -133,12 +133,6 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override;
- void data2tangent(const Eigen::VectorXd& _data,
- const Eigen::VectorXd& _calib,
- Eigen::VectorXd& _tangent,
- Eigen::MatrixXd& _J_tangent_data,
- Eigen::MatrixXd& _J_tangent_bias) const;
-
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
* \param _delta2 the second delta-state
@@ -249,6 +243,14 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
public:
virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ private:
+ void data2tangent(const Eigen::VectorXd& _data, ///< [a, w, f, t] raw measures
+ const Eigen::VectorXd& _bias, ///< [ab, wb]
+ const Eigen::VectorXd& _model, ///< [com, inertia, mass]
+ Eigen::VectorXd& _tangent, ///< [a, w, f, t] calibrated
+ Eigen::MatrixXd& _J_tangent_data,
+ Eigen::MatrixXd& _J_tangent_bias,
+ Eigen::MatrixXd& _J_tangent_model) const;
};
inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 684c679bd7e0dff3d54a34fcd17b4e66eb99a826..992a18499ace494bed94928ac693ca69ba9c9e44 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -111,36 +111,40 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
}
-void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
- const Eigen::VectorXd& _calib,
- Eigen::VectorXd& _tangent,
- Eigen::MatrixXd& _J_tangent_data,
- Eigen::MatrixXd& _J_tangent_bias) const
+void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
+ const Eigen::VectorXd& _bias,
+ const Eigen::VectorXd& _model,
+ Eigen::VectorXd& _tangent,
+ Eigen::MatrixXd& _J_tangent_data,
+ Eigen::MatrixXd& _J_tangent_bias,
+ Eigen::MatrixXd& _J_tangent_model) const
{
- Vector6d bias = _calib.segment<6>(0);
- Vector7d model = _calib.segment<7>(6);
- const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector
- const Vector3d& fd = _data.segment<3>(6);
- const Vector3d& td = _data.segment<3>(9);
- const Vector3d& c = model.segment<3>(0);
- // const Matrix3d& fd_cross = skew(fd);
+ const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector
+ const Vector3d& fd = _data.segment<3>(6);
+ const Vector3d& td = _data.segment<3>(9);
+ const Vector3d& c = _model.segment<3>(0);
+ const Matrix3d& fd_cross = skew(fd);
const Matrix3d& c_cross = skew(c);
// tangent(a,w) = data(a,w) - bias(a,w)
// tangent(f) = data(f)
// tangent(t) = data(t) - model(c) x data(f)
- _tangent.segment<6>(0) = awd - bias;
+ _tangent.segment<6>(0) = awd - _bias;
_tangent.segment<3>(6) = fd;
_tangent.segment<3>(9) = td - c.cross(fd); // c x fd
// J_tangent_data
- _J_tangent_data = MatrixXd::Identity(12, 12);
+ _J_tangent_data.setIdentity(12, 12);
_J_tangent_data.block<3, 3>(9, 6) = -c_cross; // J_tt_fd = -c_cross
// J_tangent_bias = [-Identity(6,6) ; Zero(6,6)]
_J_tangent_bias.topRows<6>() = -Matrix6d::Identity();
_J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
+
+ // J_tangent_model
+ _J_tangent_model.setZero(12,7);
+ _J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross
}
void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -274,23 +278,21 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
*
* ---------------------------------------------------------------
*
- * Notes:
- *
- * cross product and Jacobians
+ * Notes: Jacobians of cross product
*
- * a, b in R3
+ * a, b in R^3
*
- * a_x, b_x, skew-symmetric matrices
+ * a_x, b_x, skew-symmetric matrices, in R^3x3
*
- * a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
+ * a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
*
- * a x b = a_x b
+ * a x b = a_x b
*
- * a x b = - b x a = - b_x a
+ * a x b = - b x a = - b_x a
*
- * J_(axb)_a = -b_x
+ * J_(axb)_a = -b_x
*
- * J_(axb)_b = a_x
+ * J_(axb)_b = a_x
*/
// Matrix<double, 12, 1> tangent = _data;
@@ -322,14 +324,16 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
* tangent = [at,wt,ft,tt]
*/
- VectorXd tangent(12);
+ Vector6d bias = _calib.segment<6>(0);
+ Vector7d model = _calib.segment<7>(6);
+ VectorXd tangent(12);
MatrixXd J_tangent_data(12, 12);
- MatrixXd J_tangent_bias(12, 6);
+ MatrixXd J_tangent_bias(12, 6);
+ MatrixXd J_tangent_model(12, 7);
- data2tangent(_data, _calib, tangent, J_tangent_data, J_tangent_bias);
+ data2tangent(_data, bias, model, tangent, J_tangent_data, J_tangent_bias, J_tangent_model);
// 2. go from tangent to delta. We need again the dynamic model for this
- Vector7d model = _calib.segment<7>(6);
Matrix<double, 18, 12> J_delta_tangent;
Matrix<double, 18, 7> J_delta_model;
fti::tangent2delta(
@@ -337,11 +341,11 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
// 3. Compose jacobians
Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
- // !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model !!!!!!!!!!!
- // TODO: Check the calculation od the J_delta_model
- // J_delta_model += J_delta_tangent * J_tangent_model;
- _jacobian_calib << J_delta_bias,
- J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_bias ; J_delta_model]
+ // !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model
+ // !!!!!!!!!!!
+ // TODO: Check the calculation od the J_delta_model in fti::tangent2delta()
+ // J_delta_model += J_delta_tangent * J_tangent_model;
+ _jacobian_calib << J_delta_bias, J_delta_model; // J_delta_calib = [J_delta_bias ; J_delta_model]
const auto& J_delta_data = J_delta_tangent * J_tangent_data;
// 4. compute covariance