diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 3a73a0250d837c43e5a8d4ea246d8429ccdc0914..3ad28d73854ef0d783e4b476a93df86d9ceb1229 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -47,7 +47,7 @@ ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDy
19, // delta size [pi, vi, pd, vd, L, q]
18, // delta cov size
12, // data size [a, w, f, t]
- 13, // calib size [ab, wb, C, i, m]
+ 13, // calib size [ab, wb, c, i, m]
_params_force_torque_inertial_preint_dynamics),
params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
{
@@ -122,17 +122,127 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
// compute tangent by removing IMU bias
/**
- * @brief tangent and data
+ * Notes on the computation of `tangent` and `delta` from `data` and `calib`, and its Jacobians:
+ * =============================================================================================
*
- * tangent = [at,wt,ft,tt] wrt CoM
- * data = [ad,wd,fd,td] measured
- * calib = [I,C,i,m]
- * I = [ab,wb]
+ * Overview: from sensor data and system calibration parameters, we want to compute a delta.
+ * We proceed in two steps:
+ * - tangent = f ( data, bias, model )
+ * - delta = g (tangent, model, dt )
*
- * at = ad - ab
- * wt = wd - wb
- * ft = fd
- * tt = td +- C x fd
+ * Notice that the `model` part appears in both stages of the computation, f() and g().
+ * This means that the chain rule for `J_delta_model` will have a double path.
+ *
+ * 1) On a first stage, we want to obtain `tangent`
+ * ------------------------------------------------
+ * We have the following partitions of our variables:
+ *
+ * tangent = [at,wt,ft,tt] -- tangent to the delta manifold. (a,w) at BASE, (f,t) at COM
+ * data = [ad,wd,fd,td] -- sensor data, all measured at BASE
+ * bias = [ab,wb] -- IMU bias. This we called `I` in previous versions
+ * model = [c,i,m] -- system dynamic model, where c = r_base_com (CoM wrt BASE)
+ * calib = [bias,model] -- system calibration parameters
+ *
+ * we have the function:
+ *
+ * tangent = f (data, bias, model)
+ *
+ * we need to obtain:
+ * - tangent
+ * - J_tangent_data
+ * - J_tangent_bias
+ * - J_tangent_model
+ *
+ * We express each one of the blocks of `tangent` wrt the blocks of `data`, `bias` and `model`:
+ *
+ * at = ad - ab --> J_at_ad = I33 ; J_at_ab = -I33
+ * wt = wd - wb --> J_wt_wd = I33 ; J_wt_wb = -I33
+ * ft = fd --> J_ft_fd = I33
+ * tt = td - c x fd --> J_tt_td = I33 ; J_tt_c = skew(fd) = fd_x ; J_tt_fd = -skew(c) = -c_x
+ *
+ * note: to obtain `tt` (at COM) we need to account for the torque measurement `td` (at BASE)
+ * and add the torque produced by the force `fd` applied at the lever arm of BASE wrt COM, which is `-c`,
+ * obtaining `tt = td - c x fd`
+ *
+ * So we can assemble the tangent vector as
+ *
+ * tangent = [at,wt,ft,tt]
+ *
+ * and all the Jacobian blocks as:
+ *
+ * ad wd fd td
+ * J_tangent_data = [ I33 033 033 033 at
+ * 033 I33 033 033 wt
+ * 033 033 I33 033 ft
+ * 033 033 -c_x I33 ] tt
+ *
+ * ab wb
+ * J_tangent_bias = [ -I33 033 at
+ * 033 -I33 wt
+ * 033 033 ft
+ * 033 033 ] tt
+ *
+ * c i m
+ * J_tangent_model = [ 033 033 031 at
+ * 033 033 031 wt
+ * 033 033 031 ft
+ * fd_x 033 031 ] tt
+ *
+ * and from here on, we just need to fill in the matrices.
+ *
+ * Notations:
+ * I33 : 3x3 Identity matrix
+ * 033 : 3x3 zero matrix
+ * 031 : 3x1 zero vector
+ *
+ *
+ * 2) On a second stage, we need to obtain the `delta` from the previous results.
+ * ------------------------------------------------------------------------------
+ *
+ * We have:
+ *
+ * delta = g (tangent, model, dt)
+ *
+ * we need to obtain:
+ *
+ * delta
+ * J_delta_tangent
+ * J_delta_model
+ *
+ * these are obtained directly by the function g() = tangent2delta().
+ *
+ * Note 1: It is unclear to me (JS, 4-aug-2022) if this function tangent2delta() is completely accurate
+ * with regard to the two different reference frames: BASE and COM. It is possible that
+ * we have to revise the math.
+ *
+ * Note 2: It is even possible that the function tangent2delta() is OK, but then that the function
+ * betweenStates() does not account for the difference in reference frames. So we also need to revise
+ * the math inside betweenStates() with regard to the two reference frames BASE and COM.
+ *
+ * 3) On a third stage, we need to apply the chain rule for the functions f() and g(),
+ * that is delta = g( f( data, bias, model), model) -- yes, it's a mess!!
+ *
+ * J_delta_data = J_delta_tangent * J_tangent_data
+ * J_delta_model = J_delta_model + J_delta_tangent * J_tangent_model <-- remember all Jacobians are PARTIAL
+ * derivatives. J_delta_bias = J_delta_tangent * J_tangent_bias
+ *
+ * Finally we can assemble the Jacobian `J_delta_calib` as the concatenation of bias and model:
+ *
+ * J_delta_calib = [ J_delta_bias | J_delta_model ]
+ *
+ * 4) On a fourth stage, we compute the covariance of the delta:
+ *
+ * Q_delta = J_delta_data * Q_data * J_delta_data.transpose()
+ *
+ * 5) The function returns the following quantities, which relate to the variables used above:
+ *
+ * - _delta = delta
+ * - _delta_cov = Q_delta
+ * - _jacobian_calib = J_delta_calib
+ *
+ * ---------------------------------------------------------------
+ *
+ * Notes:
*
* cross product and Jacobians
*
@@ -171,34 +281,71 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
////////////////////// NOU CODI QUE HAURE DE REVISAR I CANVIAR PEL QUE HI HA ADALT //////////////////////////
- Vector3d C = _calib.segment<3>(6);
- Matrix3d C_cross = skew(C);
- Vector3d fd = _data.segment<3>(6);
- Matrix3d fd_cross = skew(fd);
-
- Matrix<double, 12, 1> tangent = _data;
- tangent.segment<3>(9) += _calib.segment<3>(6).cross(_data.segment<3>(6)); // C x fd
- Matrix<double, 12, 12> J_tangent_data = MatrixXd::Identity(12, 12);
- J_tangent_data.bottomRows(3).middleCols(6, 3) = C_cross;
- Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
- J_tangent_I.topRows<6>() = -Matrix6d::Identity();
- J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
- Matrix<double, 12, 7> J_tangent_model = MatrixXd::Zero(12, 7);
- J_tangent_model.bottomRows(3).leftCols(3) = -fd_cross; // J_tangent_model = [Zero(9,7); -fd_cross, Zero(3,3), 0]
-
- // go from tangent to delta. We need to dynamic model for this
- const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
- Matrix<double, 18, 12> J_delta_tangent;
- Matrix<double, 18, 7> J_delta_model;
+ /*
+ * 1. tangent = f(data, bias, model) --> J_tangent_data, J_tangent_bias, J_tangent_model
+ *
+ * data = [ad,wd,fd,td]
+ * bias = [ab,wb]
+ * model = [c,i,m]
+ * tangent = [at,wt,ft,tt]
+ */
+
+ 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 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)
+ Matrix<double, 12, 1> tangent;
+ 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
+ Matrix<double, 12, 12> J_tangent_data = MatrixXd::Identity(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)]
+ Matrix<double, 12, 6> J_tangent_bias;
+ J_tangent_bias.topRows<6>() = -Matrix6d::Identity();
+ J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
+
+ // J_tangent_model
+ Matrix<double, 12, 7> J_tangent_model = MatrixXd::Zero(12, 7);
+ J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross
+
+ WOLF_INFO("J_tangent_data\n", J_tangent_data);
+ WOLF_INFO("J_tangent_bias\n", J_tangent_bias);
+ WOLF_INFO("J_tangent_model\n", J_tangent_model);
+
+ // 2. go from tangent to delta. We need again the dynamic model for this
+ // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+ Matrix<double, 18, 12> J_delta_tangent;
+ Matrix<double, 18, 7> J_delta_model;
fti::tangent2delta(
tangent, model, _dt, _delta, J_delta_tangent, J_delta_model); // Aquà ja farà bé ell sol el J_delta_model?
- // Compute cov and compose jacobians
- Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
- J_delta_model += J_delta_tangent * J_tangent_model;
- _jacobian_calib << J_delta_I, J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
+ WOLF_INFO("J_delta_tangent\n", J_delta_tangent);
+ WOLF_INFO("J_delta_model\n", J_delta_model);
+
+ // 3. Compose jacobians
+ Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
+ // 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]
const auto& J_delta_data = J_delta_tangent * J_tangent_data;
- _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+
+ WOLF_INFO("J_delta_data\n", J_delta_data);
+ WOLF_INFO("J_delta_calib\n", _jacobian_calib);
+
+ // 4. compute covariance
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 619dd4b5273f2325e71053994a3dbce4a1adb608..01fa2ad8fb7ae672c5a54e679cb9fe9f57c818d0 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -63,17 +63,20 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server);
// CERES WRAPPER
- solver_ = std::make_shared<SolverCeres>(P);
- solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;ceres::LINE_SEARCH
- solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
- solver_->getSolverOptions().max_num_iterations = 1e4;
+ solver_ = std::make_shared<SolverCeres>(P);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
+ // solver_->getSolverOptions().function_tolerance = 1e-15;
+ // solver_->getSolverOptions().gradient_tolerance = 1e-15;
+ // solver_->getSolverOptions().parameter_tolerance = 1e-15;
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
- cdm_true = {0,0,0.0341};
- inertia_true = {0.017598,0.017957,0.029599};
- mass_true = 1.952;
+ cdm_true = {0, 0, 0.0341};
+ inertia_true = {0.017598, 0.017957, 0.029599};
+ mass_true = 1.952;
}
};
@@ -88,10 +91,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
double mass_guess = S->getStateBlock('m')->getState()(0);
// Data
- VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
- data.segment<3>(0) = -gravity();
- data.segment<3>(6) = -mass_true * gravity();
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity(); // accelerometer
+ data.segment<3>(6) = -mass_true * gravity(); // force
+ MatrixXd data_cov = MatrixXd::Identity(12, 12) * 1e-6;
// We fix the parameters of the sensor except for the mass
S->getStateBlock('P')->fix();
@@ -125,7 +128,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
WOLF_INFO("======== SOLVE PROBLEM =======")
std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+ WOLF_INFO(report);
P->print(4, 1, 1, 1);
@@ -134,10 +137,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
}
-
TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.005,0.005,0.33));
+ S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
S->getStateBlock('m')->setState(Vector1d(mass_true));
Vector3d cdm_guess = S->getStateBlock('C')->getState();
@@ -188,13 +190,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
}
-
TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.1,0.1,0.2));
+ S->getStateBlock('C')->setState(Vector3d(0.1, 0.1, 0.2));
S->getStateBlock('m')->setState(Vector1d(1.9));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
- double mass_guess = S->getStateBlock('m')->getState()(0);
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -246,7 +247,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);