From 8597a987515f35299a4ba520da1433b7e5995a4c Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Sat, 1 Jan 2022 23:03:39 +0100 Subject: [PATCH] Follow core 434 --- .../bodydynamics/processor/processor_force_torque_preint.h | 2 +- src/processor/processor_force_torque_preint.cpp | 6 +----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h index 1c6133e..a7041f4 100644 --- a/include/bodydynamics/processor/processor_force_torque_preint.h +++ b/include/bodydynamics/processor/processor_force_torque_preint.h @@ -132,7 +132,7 @@ inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor) inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const { - return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // p, q, v + return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation } } // namespace wolf diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp index e24f86d..cb39f2a 100644 --- a/src/processor/processor_force_torque_preint.cpp +++ b/src/processor/processor_force_torque_preint.cpp @@ -46,11 +46,7 @@ ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorqu dimc_(_params_motion_force_torque_preint->dimc) { - // Set constant parts of Jacobians - jacobian_delta_preint_.setIdentity(12,12); // delta composition / CURRENT delta - jacobian_delta_.setIdentity(12,12); // delta composition / PREVIOUS delta - jacobian_calib_.setZero(12,6); // delta preint / biases - unmeasured_perturbation_cov_ = pow(params_motion_force_torque_preint_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 12, 12>::Identity(); + // } ProcessorForceTorquePreint::~ProcessorForceTorquePreint() -- GitLab