From e915d4cfef32b0dc76027b4215cc7bb894e194c2 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Fri, 5 Aug 2022 14:58:54 +0200
Subject: [PATCH] new data2tangent function in the processor ftipd
---
...or_force_torque_inertial_preint_dynamics.h | 15 +++-
..._force_torque_inertial_preint_dynamics.cpp | 80 +++++++++----------
...problem_force_torque_inertial_dynamics.cpp | 12 +--
3 files changed, 57 insertions(+), 50 deletions(-)
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 9bf6d34..345a4c2 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -50,8 +50,8 @@ struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessor
std::string print() const override
{
- return ParamsProcessorMotion::print() + "\n"; //
- // + "n_propellers: " + std::to_string(n_propellers); //
+ return ParamsProcessorMotion::print() + "\n"; //
+ // + "n_propellers: " + std::to_string(n_propellers); //
}
// int n_propellers;
@@ -65,11 +65,12 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
~ProcessorForceTorqueInertialPreintDynamics() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics, ParamsProcessorForceTorqueInertialPreintDynamics);
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics,
+ ParamsProcessorForceTorqueInertialPreintDynamics);
protected:
ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
- SensorForceTorqueInertialPtr sensor_fti_;
+ SensorForceTorqueInertialPtr sensor_fti_;
public:
/** \brief convert raw CaptureMotion data to the delta-state format.
@@ -132,6 +133,12 @@ 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
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 3ad28d7..684c679 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -111,6 +111,38 @@ 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
+
+{
+ 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)
+ _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.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();
+}
+
void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
@@ -290,60 +322,28 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
* 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);
+ VectorXd tangent(12);
+ MatrixXd J_tangent_data(12, 12);
+ MatrixXd J_tangent_bias(12, 6);
- // 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);
+ data2tangent(_data, _calib, tangent, J_tangent_data, J_tangent_bias);
// 2. go from tangent to delta. We need again the dynamic model for this
- // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+ Vector7d model = _calib.segment<7>(6);
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?
- 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;
+ // !!!!!! 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]
const auto& J_delta_data = J_delta_tangent * J_tangent_data;
- 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 01fa2ad..d6da02a 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -67,9 +67,9 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
// 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;
+ 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());
@@ -192,8 +192,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.1, 0.1, 0.2));
- S->getStateBlock('m')->setState(Vector1d(1.9));
+ S->getStateBlock('C')->setState(Vector3d(0.0, 0.0, 0.02));
+ S->getStateBlock('m')->setState(Vector1d(1.91));
Vector3d cdm_guess = S->getStateBlock('C')->getState();
double mass_guess = S->getStateBlock('m')->getState()(0);
@@ -250,6 +250,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
+ ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.mass_and_cdm_hovering";
return RUN_ALL_TESTS();
}
\ No newline at end of file
--
GitLab