diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h index 4de0c9357ff363e87790e3429aa9e85b171a8777..c834f71b8e1fc42b2d9d8e7dfd1ee43157627d43 100644 --- a/include/bodydynamics/processor/processor_force_torque_preint.h +++ b/include/bodydynamics/processor/processor_force_torque_preint.h @@ -126,8 +126,8 @@ namespace wolf{ inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor) { - sensor_ikin_ = _sensor->getProblem()->getSensor(params_motion_force_torque_preint_->sensor_ikin_name); - sensor_angvel_ = _sensor->getProblem()->getSensor(params_motion_force_torque_preint_->sensor_angvel_name); + sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_ikin_name); + sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_angvel_name); }; inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp index d9dc8630d7dbd048bd8be48e3cb919b52a4c0014..46eb8f944900d0eff9596ad239351d19ec74f1a3 100644 --- a/src/processor/processor_inertial_kinematics.cpp +++ b/src/processor/processor_inertial_kinematics.cpp @@ -72,7 +72,7 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture) auto buffer_frame_it = buffer_frame_.getContainer().begin(); auto buffer_capture_it = buffer_capture_.getContainer().begin(); - auto sensor_angvel = getProblem()->getSensor(params_ikin_->sensor_angvel_name); + auto sensor_angvel = getProblem()->findSensor(params_ikin_->sensor_angvel_name); while ((buffer_frame_it != buffer_frame_.getContainer().end()) && (buffer_capture_it != buffer_capture_.getContainer().end())) { @@ -126,7 +126,7 @@ inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureI Eigen::Matrix3d B_I_q = _cap_ikin->getBIq(); Eigen::Vector3d B_Lc_m = _cap_ikin->getBLcm(); auto sensor_ikin = std::static_pointer_cast<SensorInertialKinematics>(getSensor()); - auto sensor_angvel_base = getProblem()->getSensor(params_ikin_->sensor_angvel_name); + auto sensor_angvel_base = getProblem()->findSensor(params_ikin_->sensor_angvel_name); Eigen::Matrix3d Qp = pow(sensor_ikin->getPbcNoiseStd(), 2) * Eigen::Matrix3d::Identity(); Eigen::Matrix3d Qv = pow(sensor_ikin->getVbcNoiseStd(), 2) * Eigen::Matrix3d::Identity(); Eigen::Matrix3d Qw = sensor_angvel_base->getNoiseCov().bottomRightCorner<3,3>();