diff --git a/CHANGELOG.md b/CHANGELOG.md index 137ccf9..bf39f47 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Fixed - Fixed the possibility of deadlock in detachAll method in `wholebodydynamics` and `baseEstimatorV1`. This deadlock could lead to freeze during closing of the yarprobotinterface or the Gazebo simulator if the `gazebo_yarp_robotinterface` plugin was used (https://github.com/robotology/whole-body-estimators/pull/146). +- Fixed YARP deprecations. (See [!151](https://github.com/robotology/whole-body-estimators/pull/151)) ## [0.6.1] - 2021-01-03 diff --git a/devices/baseEstimatorV1/include/Utils.tpp b/devices/baseEstimatorV1/include/Utils.tpp index 53058b2..6ab8d27 100644 --- a/devices/baseEstimatorV1/include/Utils.tpp +++ b/devices/baseEstimatorV1/include/Utils.tpp @@ -36,12 +36,12 @@ bool YarpHelper::yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, i for (int i = 0; i < inputPtr->size(); i++) { - if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + if (!inputPtr->get(i).isFloat64() && !inputPtr->get(i).isInt32()) { yError() << "[yarpListToiDynTreeVectorFixSize] The input is expected to be a double"; return false; } - output(i) = inputPtr->get(i).asDouble(); + output(i) = inputPtr->get(i).asFloat64(); } return true; } diff --git a/devices/baseEstimatorV1/src/Utils.cpp b/devices/baseEstimatorV1/src/Utils.cpp index 3b75ba0..6f07cbf 100644 --- a/devices/baseEstimatorV1/src/Utils.cpp +++ b/devices/baseEstimatorV1/src/Utils.cpp @@ -90,15 +90,15 @@ bool iDynTreeHelper::Triplets::getTripletsFromValues(const yarp::os::Value& inpu return false; } - int row = tripletPtr->get(0).asInt(); - int col = tripletPtr->get(1).asInt(); + int row = tripletPtr->get(0).asInt32(); + int col = tripletPtr->get(1).asInt32(); if(col >= matrixDimension || row >= matrixDimension) { yError() << "[getSparseMatrixFromTriplets] element position exceeds the matrix dimension."; return false; } - output.pushTriplet(iDynTree::Triplet(col, row, tripletPtr->get(2).asDouble())); + output.pushTriplet(iDynTree::Triplet(col, row, tripletPtr->get(2).asFloat64())); } return true; } @@ -141,12 +141,12 @@ bool YarpHelper::yarpListToiDynTreeVectorDynSize(const yarp::os::Value& input, i for (int i = 0; i < inputPtr->size(); i++) { - if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + if (!inputPtr->get(i).isFloat64() && !inputPtr->get(i).isInt32()) { yError() << "[yarpListToiDynTreeVectorDynSize] The input is expected to be a double or a int"; return false; } - output(i) = inputPtr->get(i).asDouble(); + output(i) = inputPtr->get(i).asFloat64(); } return true; } @@ -225,13 +225,13 @@ bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, con return false; } - if(!value->isDouble()) + if(!value->isFloat64()) { yError() << "[getNumberFromSearchable] the value is not a double."; return false; } - number = value->asDouble(); + number = value->asFloat64(); return true; } @@ -245,13 +245,13 @@ bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, con return false; } - if(!value->isInt()) + if(!value->isInt32()) { yError() << "[getNumberFromSearchable] the value is not an integer."; return false; } - number = value->asInt(); + number = value->asInt32(); return true; } diff --git a/devices/baseEstimatorV1/src/WalkingLogger.cpp b/devices/baseEstimatorV1/src/WalkingLogger.cpp index 3848fa4..3368d4f 100644 --- a/devices/baseEstimatorV1/src/WalkingLogger.cpp +++ b/devices/baseEstimatorV1/src/WalkingLogger.cpp @@ -68,7 +68,7 @@ bool WalkingLogger::startRecord(const std::initializer_list& string YarpHelper::populateBottleWithStrings(cmd, strings); m_rpcPort.write(cmd, outcome); - if(outcome.get(0).asInt() != 1) + if(outcome.get(0).asInt32() != 1) { yError() << "[startWalking] Unable to store data"; return false; @@ -82,7 +82,7 @@ void WalkingLogger::quit() yarp::os::Bottle cmd, outcome; cmd.addString("quit"); m_rpcPort.write(cmd, outcome); - if(outcome.get(0).asInt() != 1) + if(outcome.get(0).asInt32() != 1) yInfo() << "[close] Unable to close the stream."; // close ports diff --git a/devices/baseEstimatorV1/src/baseEstimatorV1.cpp b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp index ebce272..861616b 100644 --- a/devices/baseEstimatorV1/src/baseEstimatorV1.cpp +++ b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp @@ -546,12 +546,12 @@ void yarp::dev::baseEstimatorV1::publishFloatingBaseState() state_bottle.clear(); for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) { - state_bottle.addDouble(m_world_pose_base_in_R6[i]); + state_bottle.addFloat64(m_world_pose_base_in_R6[i]); } for (unsigned int i = 0; i < m_joint_positions.size(); i++) { - state_bottle.addDouble(m_joint_positions(i)); + state_bottle.addFloat64(m_joint_positions(i)); } m_floating_base_state_port.write(); @@ -563,12 +563,12 @@ void yarp::dev::baseEstimatorV1::publishFloatingBasePoseVelocity() state_bottle.clear(); for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) { - state_bottle.addDouble(m_world_pose_base_in_R6[i]); + state_bottle.addFloat64(m_world_pose_base_in_R6[i]); } for (unsigned int i = 0; i < m_world_velocity_base.size(); i++) { - state_bottle.addDouble(m_world_velocity_base[i]); + state_bottle.addFloat64(m_world_velocity_base[i]); } m_floating_base_pose_port.write(); @@ -580,12 +580,12 @@ void yarp::dev::baseEstimatorV1::publishCOMEstimate() com_bottle.clear(); for (unsigned int i = 0; i < m_com_position.size(); i++) { - com_bottle.addDouble(m_com_position(i)); + com_bottle.addFloat64(m_com_position(i)); } for (unsigned int i = 0; i < m_com_velocity.size(); i++) { - com_bottle.addDouble(m_com_velocity(i)); + com_bottle.addFloat64(m_com_velocity(i)); } m_com_port.write(); @@ -595,10 +595,10 @@ void yarp::dev::baseEstimatorV1::publishContactState() { yarp::os::Bottle &state_bottle = m_contact_state_port.prepare(); state_bottle.clear(); - state_bottle.addDouble(m_left_foot_contact_normal_force); - state_bottle.addDouble(m_right_foot_contact_normal_force); - state_bottle.addInt(m_biped_foot_contact_classifier->getLeftFootContactState()); - state_bottle.addInt(m_biped_foot_contact_classifier->getRightFootContactState()); + state_bottle.addFloat64(m_left_foot_contact_normal_force); + state_bottle.addFloat64(m_right_foot_contact_normal_force); + state_bottle.addInt32(m_biped_foot_contact_classifier->getLeftFootContactState()); + state_bottle.addInt32(m_biped_foot_contact_classifier->getRightFootContactState()); state_bottle.addString(m_current_fixed_frame); m_contact_state_port.write(); } @@ -629,15 +629,15 @@ void yarp::dev::baseEstimatorV1::publishIMUAttitudeEstimatorStates() yarp::os::Bottle &state_bottle = m_imu_attitude_observer_estimated_state_port.prepare(); state_bottle.clear(); - state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(0))); // orientation roll - state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(1))); // orientation pitch - state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(2))); // orientation yaw - state_bottle.addDouble(rad2deg(attitude_observer_state(4))); // ang vel about x - state_bottle.addDouble(rad2deg(attitude_observer_state(5))); // ang vel about y - state_bottle.addDouble(rad2deg(attitude_observer_state(6))); // ang vel about z - state_bottle.addDouble(rad2deg(attitude_observer_state(7))); // gyro bias about x - state_bottle.addDouble(rad2deg(attitude_observer_state(8))); // gyro bias about y - state_bottle.addDouble(rad2deg(attitude_observer_state(9))); // gyro bias about z + state_bottle.addFloat64(rad2deg(m_imu_attitude_estimate_as_rpy(0))); // orientation roll + state_bottle.addFloat64(rad2deg(m_imu_attitude_estimate_as_rpy(1))); // orientation pitch + state_bottle.addFloat64(rad2deg(m_imu_attitude_estimate_as_rpy(2))); // orientation yaw + state_bottle.addFloat64(rad2deg(attitude_observer_state(4))); // ang vel about x + state_bottle.addFloat64(rad2deg(attitude_observer_state(5))); // ang vel about y + state_bottle.addFloat64(rad2deg(attitude_observer_state(6))); // ang vel about z + state_bottle.addFloat64(rad2deg(attitude_observer_state(7))); // gyro bias about x + state_bottle.addFloat64(rad2deg(attitude_observer_state(8))); // gyro bias about y + state_bottle.addFloat64(rad2deg(attitude_observer_state(9))); // gyro bias about z m_imu_attitude_observer_estimated_state_port.write(); } @@ -649,9 +649,9 @@ void yarp::dev::baseEstimatorV1::publishIMUAttitudeQEKFEstimates() yarp::os::Bottle &state_bottle = m_imu_attitude_qekf_estimated_state_port.prepare(); state_bottle.clear(); - state_bottle.addDouble(rad2deg(rpy(0))); - state_bottle.addDouble(rad2deg(rpy(1))); - state_bottle.addDouble(rad2deg(rpy(2))); + state_bottle.addFloat64(rad2deg(rpy(0))); + state_bottle.addFloat64(rad2deg(rpy(1))); + state_bottle.addFloat64(rad2deg(rpy(2))); m_imu_attitude_qekf_estimated_state_port.write(); } diff --git a/devices/baseEstimatorV1/src/configureEstimator.cpp b/devices/baseEstimatorV1/src/configureEstimator.cpp index b79458f..b873eee 100644 --- a/devices/baseEstimatorV1/src/configureEstimator.cpp +++ b/devices/baseEstimatorV1/src/configureEstimator.cpp @@ -30,9 +30,9 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o " Loading default robot name " << m_robot; } - if (config.check("device_period_in_seconds") && config.find("device_period_in_seconds").isDouble()) + if (config.check("device_period_in_seconds") && config.find("device_period_in_seconds").isFloat64()) { - m_device_period_in_s = config.find("device_period_in_seconds").asDouble(); + m_device_period_in_s = config.find("device_period_in_seconds").asFloat64(); } else { @@ -98,7 +98,7 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o for (size_t i =0; i < 4; i++) { - m_initial_attitude_estimate_as_quaternion(i) = attitude->get(i).asDouble(); + m_initial_attitude_estimate_as_quaternion(i) = attitude->get(i).asFloat64(); } } else @@ -110,9 +110,9 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o "uing default value for initial attitude estimate: " << m_initial_attitude_estimate_as_quaternion.toString(); } - if (config.check("imu_confidence_roll") && config.find("imu_confidence_roll").isDouble()) + if (config.check("imu_confidence_roll") && config.find("imu_confidence_roll").isFloat64()) { - m_imu_confidence_roll = config.find("imu_confidence_roll").asDouble(); + m_imu_confidence_roll = config.find("imu_confidence_roll").asFloat64(); } else { @@ -120,9 +120,9 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o " using default value " << m_imu_confidence_roll ; } - if (config.check("imu_confidence_pitch") && config.find("imu_confidence_pitch").isDouble()) + if (config.check("imu_confidence_pitch") && config.find("imu_confidence_pitch").isFloat64()) { - m_imu_confidence_pitch = config.find("imu_confidence_pitch").asDouble(); + m_imu_confidence_pitch = config.find("imu_confidence_pitch").asFloat64(); } else { @@ -186,7 +186,7 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o m_head_to_base_joints_list_zero_pos.resize(upper_body_joint_zero_pos->size()); for (auto idx = 0; idx < upper_body_joint_zero_pos->size(); idx++) { - m_head_to_base_joints_list_zero_pos(idx) = upper_body_joint_zero_pos->get(idx).asDouble(); + m_head_to_base_joints_list_zero_pos(idx) = upper_body_joint_zero_pos->get(idx).asFloat64(); } } else @@ -210,7 +210,7 @@ bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::o if (m_use_lpf) { - m_joint_vel_filter_cutoff_freq = config.check("joint_vel_lpf_cutoff_freq", yarp::os::Value(0.0)).asDouble(); + m_joint_vel_filter_cutoff_freq = config.check("joint_vel_lpf_cutoff_freq", yarp::os::Value(0.0)).asFloat64(); m_joint_vel_filter_sample_time_in_s = m_device_period_in_s; } @@ -252,13 +252,13 @@ bool yarp::dev::baseEstimatorV1::loadLeggedOdometryParametersFromConfig(const ya } iDynTree::Position initial_reference_frame_p_world; - initial_reference_frame_p_world(0) = pose->get(0).asDouble(); - initial_reference_frame_p_world(1) = pose->get(1).asDouble(); - initial_reference_frame_p_world(2) = pose->get(2).asDouble(); + initial_reference_frame_p_world(0) = pose->get(0).asFloat64(); + initial_reference_frame_p_world(1) = pose->get(1).asFloat64(); + initial_reference_frame_p_world(2) = pose->get(2).asFloat64(); - iDynTree::Rotation initial_reference_frame_R_world = iDynTree::Rotation::RPY(pose->get(3).asDouble(), - pose->get(4).asDouble(), - pose->get(5).asDouble()); + iDynTree::Rotation initial_reference_frame_R_world = iDynTree::Rotation::RPY(pose->get(3).asFloat64(), + pose->get(4).asFloat64(), + pose->get(5).asFloat64()); m_initial_reference_frame_H_world = iDynTree::Transform(initial_reference_frame_R_world, initial_reference_frame_p_world); } @@ -285,9 +285,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon " Loading default initial primary foot " << m_initial_primary_foot; } - if (config.check("schmitt_stable_contact_make_time") && config.find("schmitt_stable_contact_make_time").isDouble()) + if (config.check("schmitt_stable_contact_make_time") && config.find("schmitt_stable_contact_make_time").isFloat64()) { - m_left_foot_contact_schmitt_params.stableTimeContactMake = config.find("schmitt_stable_contact_make_time").asDouble(); + m_left_foot_contact_schmitt_params.stableTimeContactMake = config.find("schmitt_stable_contact_make_time").asFloat64(); m_right_foot_contact_schmitt_params.stableTimeContactMake = m_left_foot_contact_schmitt_params.stableTimeContactMake; } else @@ -297,9 +297,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon return false; } - if (config.check("schmitt_stable_contact_break_time") && config.find("schmitt_stable_contact_break_time").isDouble()) + if (config.check("schmitt_stable_contact_break_time") && config.find("schmitt_stable_contact_break_time").isFloat64()) { - m_left_foot_contact_schmitt_params.stableTimeContactBreak = config.find("schmitt_stable_contact_break_time").asDouble(); + m_left_foot_contact_schmitt_params.stableTimeContactBreak = config.find("schmitt_stable_contact_break_time").asFloat64(); m_right_foot_contact_schmitt_params.stableTimeContactBreak = m_left_foot_contact_schmitt_params.stableTimeContactBreak; } else @@ -309,9 +309,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon return false; } - if (config.check("left_schmitt_contact_make_force_threshold") && config.find("left_schmitt_contact_make_force_threshold").isDouble()) + if (config.check("left_schmitt_contact_make_force_threshold") && config.find("left_schmitt_contact_make_force_threshold").isFloat64()) { - m_left_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("left_schmitt_contact_make_force_threshold").asDouble(); + m_left_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("left_schmitt_contact_make_force_threshold").asFloat64(); } else { @@ -320,9 +320,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon return false; } - if (config.check("left_schmitt_contact_break_force_threshold") && config.find("left_schmitt_contact_break_force_threshold").isDouble()) + if (config.check("left_schmitt_contact_break_force_threshold") && config.find("left_schmitt_contact_break_force_threshold").isFloat64()) { - m_left_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("left_schmitt_contact_break_force_threshold").asDouble(); + m_left_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("left_schmitt_contact_break_force_threshold").asFloat64(); } else { @@ -331,9 +331,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon return false; } - if (config.check("right_schmitt_contact_make_force_threshold") && config.find("right_schmitt_contact_make_force_threshold").isDouble()) + if (config.check("right_schmitt_contact_make_force_threshold") && config.find("right_schmitt_contact_make_force_threshold").isFloat64()) { - m_right_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("right_schmitt_contact_make_force_threshold").asDouble(); + m_right_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("right_schmitt_contact_make_force_threshold").asFloat64(); } else { @@ -342,9 +342,9 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon return false; } - if (config.check("right_schmitt_contact_break_force_threshold") && config.find("right_schmitt_contact_break_force_threshold").isDouble()) + if (config.check("right_schmitt_contact_break_force_threshold") && config.find("right_schmitt_contact_break_force_threshold").isFloat64()) { - m_right_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("right_schmitt_contact_break_force_threshold").asDouble(); + m_right_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("right_schmitt_contact_break_force_threshold").asFloat64(); } else { @@ -358,18 +358,18 @@ bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromCon bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config) { - if (config.check("mahony_kp") && config.find("mahony_kp").isDouble()) + if (config.check("mahony_kp") && config.find("mahony_kp").isFloat64()) { - m_imu_attitude_observer_params.kp = config.find("mahony_kp").asDouble(); + m_imu_attitude_observer_params.kp = config.find("mahony_kp").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gain kp: " << m_imu_attitude_observer_params.kp ; } - if (config.check("mahony_ki") && config.find("mahony_ki").isDouble()) + if (config.check("mahony_ki") && config.find("mahony_ki").isFloat64()) { - m_imu_attitude_observer_params.ki = config.find("mahony_ki").asDouble(); + m_imu_attitude_observer_params.ki = config.find("mahony_ki").asFloat64(); } else { @@ -385,9 +385,9 @@ bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromCon yWarning() << "floatingBaseEstimatorV1: " << "use magnetometer flag set to " << m_imu_attitude_observer_params.use_magnetometer_measurements; } - if (config.check("mahony_discretization_time_step_in_seconds") && config.find("mahony_discretization_time_step_in_seconds").isDouble()) + if (config.check("mahony_discretization_time_step_in_seconds") && config.find("mahony_discretization_time_step_in_seconds").isFloat64()) { - m_imu_attitude_observer_params.time_step_in_seconds = config.find("mahony_discretization_time_step_in_seconds").asDouble(); + m_imu_attitude_observer_params.time_step_in_seconds = config.find("mahony_discretization_time_step_in_seconds").asFloat64(); } else { @@ -399,81 +399,81 @@ bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromCon bool yarp::dev::baseEstimatorV1::loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config) { - if (config.check("qekf_discretization_time_step_in_seconds") && config.find("qekf_discretization_time_step_in_seconds").isDouble()) + if (config.check("qekf_discretization_time_step_in_seconds") && config.find("qekf_discretization_time_step_in_seconds").isFloat64()) { - m_imu_attitude_qekf_params.time_step_in_seconds = config.find("qekf_discretization_time_step_in_seconds").asDouble(); + m_imu_attitude_qekf_params.time_step_in_seconds = config.find("qekf_discretization_time_step_in_seconds").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default discretization time step: " << m_imu_attitude_qekf_params.time_step_in_seconds ; } - if (config.check("qekf_accelerometer_noise_variance") && config.find("qekf_accelerometer_noise_variance").isDouble()) + if (config.check("qekf_accelerometer_noise_variance") && config.find("qekf_accelerometer_noise_variance").isFloat64()) { - m_imu_attitude_qekf_params.accelerometer_noise_variance = config.find("qekf_accelerometer_noise_variance").asDouble(); + m_imu_attitude_qekf_params.accelerometer_noise_variance = config.find("qekf_accelerometer_noise_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default accelerometer noise variance: " << m_imu_attitude_qekf_params.accelerometer_noise_variance ; } - if (config.check("qekf_magnetometer_noise_variance") && config.find("qekf_magnetometer_noise_variance").isDouble()) + if (config.check("qekf_magnetometer_noise_variance") && config.find("qekf_magnetometer_noise_variance").isFloat64()) { - m_imu_attitude_qekf_params.magnetometer_noise_variance = config.find("qekf_magnetometer_noise_variance").asDouble(); + m_imu_attitude_qekf_params.magnetometer_noise_variance = config.find("qekf_magnetometer_noise_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default magnetometer noise variance: " << m_imu_attitude_qekf_params.magnetometer_noise_variance ; } - if (config.check("qekf_gyroscope_noise_variance") && config.find("qekf_gyroscope_noise_variance").isDouble()) + if (config.check("qekf_gyroscope_noise_variance") && config.find("qekf_gyroscope_noise_variance").isFloat64()) { - m_imu_attitude_qekf_params.gyroscope_noise_variance = config.find("qekf_gyroscope_noise_variance").asDouble(); + m_imu_attitude_qekf_params.gyroscope_noise_variance = config.find("qekf_gyroscope_noise_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyroscope noise variance: " << m_imu_attitude_qekf_params.gyroscope_noise_variance ; } - if (config.check("qekf_gyro_bias_noise_variance") && config.find("qekf_gyro_bias_noise_variance").isDouble()) + if (config.check("qekf_gyro_bias_noise_variance") && config.find("qekf_gyro_bias_noise_variance").isFloat64()) { - m_imu_attitude_qekf_params.gyro_bias_noise_variance = config.find("qekf_gyro_bias_noise_variance").asDouble(); + m_imu_attitude_qekf_params.gyro_bias_noise_variance = config.find("qekf_gyro_bias_noise_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyro bias noise variance: " << m_imu_attitude_qekf_params.gyro_bias_noise_variance ; } - if (config.check("qekf_initial_orientation_error_variance") && config.find("qekf_initial_orientation_error_variance").isDouble()) + if (config.check("qekf_initial_orientation_error_variance") && config.find("qekf_initial_orientation_error_variance").isFloat64()) { - m_imu_attitude_qekf_params.initial_orientation_error_variance = config.find("qekf_initial_orientation_error_variance").asDouble(); + m_imu_attitude_qekf_params.initial_orientation_error_variance = config.find("qekf_initial_orientation_error_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state orientation variance: " << m_imu_attitude_qekf_params.initial_orientation_error_variance ; } - if (config.check("qekf_initial_ang_vel_error_variance") && config.find("qekf_initial_ang_vel_error_variance").isDouble()) + if (config.check("qekf_initial_ang_vel_error_variance") && config.find("qekf_initial_ang_vel_error_variance").isFloat64()) { - m_imu_attitude_qekf_params.initial_ang_vel_error_variance = config.find("qekf_initial_ang_vel_error_variance").asDouble(); + m_imu_attitude_qekf_params.initial_ang_vel_error_variance = config.find("qekf_initial_ang_vel_error_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state angular velocity variance: " << m_imu_attitude_qekf_params.initial_ang_vel_error_variance ; } - if (config.check("qekf_initial_gyro_bias_error_variance") && config.find("qekf_initial_gyro_bias_error_variance").isDouble()) + if (config.check("qekf_initial_gyro_bias_error_variance") && config.find("qekf_initial_gyro_bias_error_variance").isFloat64()) { - m_imu_attitude_qekf_params.initial_gyro_bias_error_variance = config.find("qekf_initial_gyro_bias_error_variance").asDouble(); + m_imu_attitude_qekf_params.initial_gyro_bias_error_variance = config.find("qekf_initial_gyro_bias_error_variance").asFloat64(); } else { yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state gyro bias variance: " << m_imu_attitude_qekf_params.initial_gyro_bias_error_variance ; } - if (config.check("qekf_bias_correlation_time_factor") && config.find("qekf_bias_correlation_time_factor").isDouble()) + if (config.check("qekf_bias_correlation_time_factor") && config.find("qekf_bias_correlation_time_factor").isFloat64()) { - m_imu_attitude_qekf_params.bias_correlation_time_factor = config.find("qekf_bias_correlation_time_factor").asDouble(); + m_imu_attitude_qekf_params.bias_correlation_time_factor = config.find("qekf_bias_correlation_time_factor").asFloat64(); } else { diff --git a/devices/virtualAnalogClient/VirtualAnalogClient.cpp b/devices/virtualAnalogClient/VirtualAnalogClient.cpp index 5f3d3f1..2504414 100644 --- a/devices/virtualAnalogClient/VirtualAnalogClient.cpp +++ b/devices/virtualAnalogClient/VirtualAnalogClient.cpp @@ -99,13 +99,13 @@ bool VirtualAnalogClient::open(Searchable& config) } } - if( !( prop.check("virtualAnalogSensorInteger") && prop.find("virtualAnalogSensorInteger").isInt() ) ) + if( !( prop.check("virtualAnalogSensorInteger") && prop.find("virtualAnalogSensorInteger").isInt32() ) ) { yError("VirtualAnalogClient: Missing required virtualAnalogSensorInteger int parameter"); return false; } - m_virtualAnalogSensorInteger = prop.find("virtualAnalogSensorInteger").isInt(); + m_virtualAnalogSensorInteger = prop.find("virtualAnalogSensorInteger").isInt32(); // Resize buffer measureBuffer.resize(m_axisName.size(),0.0); @@ -187,10 +187,10 @@ void VirtualAnalogClient::sendData() { Bottle & a = m_outputPort.prepare(); a.clear(); - a.addInt(m_virtualAnalogSensorInteger); + a.addInt32(m_virtualAnalogSensorInteger); for(size_t i=0;iget(index).asDouble(); + contactWrenchDirection[ax][ay] = _propContactWrenchDirection->get(index).asFloat64(); } } } @@ -542,7 +542,7 @@ bool WholeBodyDynamicsDevice::parseOverrideContactFramesData(yarp::os::Bottle *_ for(int ay=0; ay < 3; ay++) { int index = 3*ax+ay; - contactWrenchPosition[ax][ay] = _propContactWrenchPosition->get(index).asDouble(); + contactWrenchPosition[ax][ay] = _propContactWrenchPosition->get(index).asFloat64(); } } } @@ -621,8 +621,8 @@ bool WholeBodyDynamicsDevice::openSkinContactListPorts(os::Searchable& config) std::string iDynTree_link_name = map_bot->get(0).asString(); std::string iDynTree_skinFrame_name = map_bot->get(1).asList()->get(0).asString(); - int skinDynLib_body_part = map_bot->get(1).asList()->get(1).asInt(); - int skinDynLib_link_index = map_bot->get(1).asList()->get(2).asInt(); + int skinDynLib_body_part = map_bot->get(1).asList()->get(1).asInt32(); + int skinDynLib_link_index = map_bot->get(1).asList()->get(2).asInt32(); bool ret_sdl = conversionHelper.addSkinDynLibAlias(estimator.model(), iDynTree_link_name,iDynTree_skinFrame_name, @@ -905,7 +905,7 @@ void WholeBodyDynamicsDevice::resizeBuffers() for (size_t i = 0; i < 6; ++i) { - wrenchValues.addDouble(0.0); + wrenchValues.addFloat64(0.0); } } } @@ -921,12 +921,12 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config) if (prop.check("devicePeriodInSeconds")) { - if(!prop.find("devicePeriodInSeconds").isDouble()) + if(!prop.find("devicePeriodInSeconds").isFloat64()) { yError() << "wholeBodyDynamics : The devicePeriodInSeconds must be a double"; return false; } - this->setPeriod(prop.find("devicePeriodInSeconds").asDouble()); + this->setPeriod(prop.find("devicePeriodInSeconds").asFloat64()); } else { @@ -987,9 +987,9 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config) prop.find("fixedFrameGravity").isList() && prop.find("fixedFrameGravity").asList()->size() == 3 ) { - settings.fixedFrameGravity.x = prop.find("fixedFrameGravity").asList()->get(0).asDouble(); - settings.fixedFrameGravity.y = prop.find("fixedFrameGravity").asList()->get(1).asDouble(); - settings.fixedFrameGravity.z = prop.find("fixedFrameGravity").asList()->get(2).asDouble(); + settings.fixedFrameGravity.x = prop.find("fixedFrameGravity").asList()->get(0).asFloat64(); + settings.fixedFrameGravity.y = prop.find("fixedFrameGravity").asList()->get(1).asFloat64(); + settings.fixedFrameGravity.z = prop.find("fixedFrameGravity").asList()->get(2).asFloat64(); } else { @@ -1090,9 +1090,9 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config) // Set time to check for a new temperature measurement. The default value is 0.55; // is set in the device constructor if( prop.check("checkTemperatureEvery_seconds") && - prop.find("checkTemperatureEvery_seconds").isDouble()) + prop.find("checkTemperatureEvery_seconds").isFloat64()) { - checkTemperatureEvery_seconds = prop.find("checkTemperatureEvery_seconds").asDouble(); + checkTemperatureEvery_seconds = prop.find("checkTemperatureEvery_seconds").asFloat64(); } if ( !(prop.check("startWithZeroFTSensorOffsets") && prop.find("startWithZeroFTSensorOffsets").isBool()) ) @@ -1148,9 +1148,9 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config) bool WholeBodyDynamicsDevice::applyLPFSettingsFromConfig(const yarp::os::Property& prop, const std::string& setting_name) { if( prop.check(setting_name) && - prop.find(setting_name).isDouble() ) + prop.find(setting_name).isFloat64() ) { - double cut_off_freq = prop.find(setting_name).asDouble(); + double cut_off_freq = prop.find(setting_name).asFloat64(); if (setting_name == "imuFilterCutoffInHz") { settings.imuFilterCutoffInHz = cut_off_freq; } if (setting_name == "forceTorqueFilterCutoffInHz") { settings.forceTorqueFilterCutoffInHz = cut_off_freq; } if (setting_name == "jointVelFilterCutoffInHz") { settings.jointVelFilterCutoffInHz = cut_off_freq; } @@ -1195,7 +1195,7 @@ bool WholeBodyDynamicsDevice::loadSecondaryCalibrationSettingsFromConfig(os::Sea for(int c=0; c < 6; c++) { int rowMajorIndex = 6*r+c; - secondaryCalibMat(r,c) = map_bot->get(1).asList()->get(rowMajorIndex).asDouble(); + secondaryCalibMat(r,c) = map_bot->get(1).asList()->get(rowMajorIndex).asFloat64(); } } @@ -1256,9 +1256,9 @@ bool WholeBodyDynamicsDevice::loadTemperatureCoefficientsSettingsFromConfig(os:: for(auto r=0u; r < 6; r++) { - temperatureCoeffs(r) = map_bot->get(1).asList()->get(r).asDouble(); + temperatureCoeffs(r) = map_bot->get(1).asList()->get(r).asFloat64(); } - tempOffset= map_bot->get(1).asList()->get(6).asDouble(); + tempOffset= map_bot->get(1).asList()->get(6).asFloat64(); // Linearly search for the specified sensor bool sensorFound = false; @@ -1319,7 +1319,7 @@ bool WholeBodyDynamicsDevice::loadFTSensorOffsetFromConfig(os::Searchable& confi for(int r=0; r < 6; r++) { - ftOffset(r) = map_bot->get(1).asList()->get(r).asDouble(); + ftOffset(r) = map_bot->get(1).asList()->get(r).asFloat64(); }