diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h index 378546875f201e511ee73535c912c69d607b3022..cd38fb90ffa4ad84d66519a73bfd0f771787cd08 100644 --- a/include/bodydynamics/processor/processor_force_torque_preint.h +++ b/include/bodydynamics/processor/processor_force_torque_preint.h @@ -71,7 +71,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{ Eigen::VectorXd deltaZero() const override; Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, const Eigen::VectorXd& delta_step) const override; - VectorXd getCalibration (const CaptureBasePtr _capture) const override; + VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override; void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; bool voteForKeyFrame() const override; diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp index c73189ab1188ce2ff5bc87a23bbcfac9539676a8..17cb798053d93f941e1e33da26516a00ff486c45 100644 --- a/src/processor/processor_force_torque_preint.cpp +++ b/src/processor/processor_force_torque_preint.cpp @@ -121,15 +121,27 @@ Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const { + VectorXd bias_vec(6); - CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture)); - // get calib part from Ikin capture - bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState(); + if (_capture) // access from capture is quicker + { + CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture)); - // get calib part from IMU capture - bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] + // get calib part from Ikin capture + bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState(); + // get calib part from IMU capture + bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] + } + else // access from sensor is slower + { + // get calib part from Ikin capture + bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState(); + + // get calib part from IMU capture + bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] + } return bias_vec; }