Skip to content

Commit

Permalink
Merge pull request #112 from robotology/fix_yarp_deprecations
Browse files Browse the repository at this point in the history
Fix yarp deprecation of methods like asDouble
  • Loading branch information
S-Dafarra authored May 19, 2022
2 parents 8179e20 + 279c41a commit 9202b74
Show file tree
Hide file tree
Showing 17 changed files with 94 additions and 94 deletions.
2 changes: 1 addition & 1 deletion src/JoypadModule/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf)
}

// get the period
m_dT = rf.check("period", yarp::os::Value(0.1)).asDouble();
m_dT = rf.check("period", yarp::os::Value(0.1)).asFloat64();

// set the module name
std::string name;
Expand Down
2 changes: 1 addition & 1 deletion src/KinDynWrapper/src/Wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -226,7 +226,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config,
yError() << "[WalkingFK::initialize] Unable to get the double from searchable.";
return false;
}
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asDouble();
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asFloat64();
m_omega = sqrt(gravityAcceleration / comHeight);

// init filters
Expand Down
4 changes: 2 additions & 2 deletions src/LoggerClient/src/LoggerClient.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ bool LoggerClient::startRecord(const std::initializer_list<std::string>& strings
YarpUtilities::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;
Expand All @@ -84,7 +84,7 @@ void LoggerClient::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
Expand Down
12 changes: 6 additions & 6 deletions src/LoggerModule/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,11 @@ bool WalkingLoggerModule::respond(const yarp::os::Bottle& command, yarp::os::Bot
if(!m_stream.is_open())
{
yError() << "[RPC Server] The stream is not open.";
reply.addInt(0);
reply.addInt32(0);
return true;
}
m_stream.close();
reply.addInt(1);
reply.addInt32(1);

yInfo() << "[RPC Server] The stream is closed.";
return true;
Expand All @@ -58,7 +58,7 @@ bool WalkingLoggerModule::respond(const yarp::os::Bottle& command, yarp::os::Bot
if(m_stream.is_open())
{
yError() << "[RPC Server] The stream is already open.";
reply.addInt(0);
reply.addInt32(0);
return false;
}

Expand Down Expand Up @@ -87,13 +87,13 @@ bool WalkingLoggerModule::respond(const yarp::os::Bottle& command, yarp::os::Bot
// write the head of the table
m_stream << head << std::endl;

reply.addInt(1);
reply.addInt32(1);
return true;
}
else
{
yError() << "[RPC Server] Unknown command.";
reply.addInt(0);
reply.addInt32(0);
return false;
}
return true;
Expand Down Expand Up @@ -136,7 +136,7 @@ bool WalkingLoggerModule::configure(yarp::os::ResourceFinder &rf)
attach(m_rpcPort);

// set the RFModule period
m_dT = rf.check("sampling_time", yarp::os::Value(0.005)).asDouble();
m_dT = rf.check("sampling_time", yarp::os::Value(0.005)).asFloat64();

return true;
}
Expand Down
4 changes: 2 additions & 2 deletions src/RobotInterface/src/Helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,7 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
// robot name: used to connect to the robot
std::string robot = config.check("robot", yarp::os::Value("icubSim")).asString();

double sampligTime = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
double sampligTime = config.check("sampling_time", yarp::os::Value(0.016)).asFloat64();

std::string name;
if(!YarpUtilities::getStringFromSearchable(config, "name", name))
Expand Down Expand Up @@ -527,7 +527,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
return false;
}

double samplingTime = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
double samplingTime = config.check("sampling_time", yarp::os::Value(0.016)).asFloat64();

// collect the information for the ports of the left foot wrenches
// from now on we assume that the wrenches are expressed in the same frame (e.g l_sole)
Expand Down
22 changes: 11 additions & 11 deletions src/RobotInterface/src/PIDHandler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,14 +134,14 @@ bool WalkingPIDHandler::parsePIDElement(const yarp::os::Value &groupElement, std

for (int g = 1; g < 4; ++g) {
yarp::os::Value gain = element->get(g);
if(!(gain.isDouble())&& !(gain.isInt())){
if(!(gain.isFloat64())&& !(gain.isInt32())){
yError() << "The gains are supposed to be numeric. " << jointName << ": " << element->get(g).toString();
return false;
}
}
pid.setKp(element->get(1).asDouble());
pid.setKi(element->get(2).asDouble());
pid.setKd(element->get(3).asDouble());
pid.setKp(element->get(1).asFloat64());
pid.setKi(element->get(2).asFloat64());
pid.setKd(element->get(3).asFloat64());
} else {
return false;
}
Expand All @@ -151,8 +151,8 @@ bool WalkingPIDHandler::parsePIDElement(const yarp::os::Value &groupElement, std
bool WalkingPIDHandler::parsePIDConfigurationFile(const yarp::os::Bottle &PIDSettings)
{
m_useGainScheduling = PIDSettings.check("useGainScheduling", yarp::os::Value(false)).asBool();
m_firmwareDelay = PIDSettings.check("firmwareDelay", yarp::os::Value(0.0)).asDouble();
double smoothingTime = PIDSettings.check("smoothingTime", yarp::os::Value(1.0)).asDouble();
m_firmwareDelay = PIDSettings.check("firmwareDelay", yarp::os::Value(0.0)).asFloat64();
double smoothingTime = PIDSettings.check("smoothingTime", yarp::os::Value(1.0)).asFloat64();

if (smoothingTime <= 0.0) {
yError() << "The smoothing time is supposed to be positive.";
Expand Down Expand Up @@ -186,8 +186,8 @@ bool WalkingPIDHandler::parsePIDConfigurationFile(const yarp::os::Bottle &PIDSet
if (!fromStringToPIDPhase(phaseInput.asString(), phase))
return false;

double activationOffset = group->check("activationOffset", yarp::os::Value(0.0)).asDouble();
// double smoothingTime = group->check("smoothingTime", yarp::os::Value(1.0)).asDouble(); //For the time being we use a common smoothingTime
double activationOffset = group->check("activationOffset", yarp::os::Value(0.0)).asFloat64();
// double smoothingTime = group->check("smoothingTime", yarp::os::Value(1.0)).asFloat64(); //For the time being we use a common smoothingTime

PIDmap groupMap;
if (!parsePIDGroup(group, groupMap))
Expand Down Expand Up @@ -345,7 +345,7 @@ void WalkingPIDHandler::setPIDThread()
if (m_originalSmoothingTimesInMs.get(i).isList()) {
for (int j = 0; j < m_originalSmoothingTimesInMs.get(i).asList()->size(); ++j){
newList.addInt(smoothingTimeinMs);
newList.addInt32(smoothingTimeinMs);
}
} else {
yError() << "The structure of the original smoothing times is not the expected one.";
Expand Down Expand Up @@ -399,9 +399,9 @@ bool WalkingPIDHandler::setGeneralSmoothingTime(double smoothingTime)
yarp::os::Bottle &innerOutput = output.addList();
yarp::os::Bottle *innerInput = input.get(i).asList();
for (int j = 0; j < innerInput->size(); ++j)
innerOutput.addInt(smoothingTimeinMs);
innerOutput.addInt32(smoothingTimeinMs);
} else {
output.addInt(smoothingTimeinMs);
output.addInt32(smoothingTimeinMs);
}
}

Expand Down
20 changes: 10 additions & 10 deletions src/SimplifiedModelControllers/src/DCMModelPredictiveController.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -182,11 +182,11 @@ bool WalkingController::initializeMatrices(const yarp::os::Searchable& config)
}

// get sampling time
double dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
double dT = config.check("sampling_time", yarp::os::Value(0.016)).asFloat64();

// evaluate the controller horizon
double controllerHorizonSeconds = config.check("controllerHorizon",
yarp::os::Value(2.0)).asDouble();
yarp::os::Value(2.0)).asFloat64();
m_controllerHorizon = round(controllerHorizonSeconds / dT);

// get the state weight matrix
Expand Down Expand Up @@ -230,7 +230,7 @@ bool WalkingController::initializeMatrices(const yarp::os::Searchable& config)
yError() << "[initialize] Unable to get the double from searchable.";
return false;
}
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asDouble();
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asFloat64();
double omega = sqrt(gravityAcceleration / comHeight);

// evaluate dynamics matrix
Expand Down Expand Up @@ -275,8 +275,8 @@ bool WalkingController::initializeConstraints(const yarp::os::Searchable& config
return false;
}

double xlimit1 = xLimitsPtr->get(0).asDouble();
double xlimit2 = xLimitsPtr->get(1).asDouble();
double xlimit1 = xLimitsPtr->get(0).asFloat64();
double xlimit2 = xLimitsPtr->get(1).asFloat64();

yarp::os::Value& yLimits = feetDimensionsPointer->get(1);
if(yLimits.isNull() || !yLimits.isList())
Expand All @@ -292,8 +292,8 @@ bool WalkingController::initializeConstraints(const yarp::os::Searchable& config
return false;
}

double ylimit1 = yLimitsPtr->get(0).asDouble();
double ylimit2 = yLimitsPtr->get(1).asDouble();
double ylimit1 = yLimitsPtr->get(0).asFloat64();
double ylimit2 = yLimitsPtr->get(1).asFloat64();

// evaluate the foot polygon
iDynTree::Polygon foot;
Expand All @@ -306,7 +306,7 @@ bool WalkingController::initializeConstraints(const yarp::os::Searchable& config
m_feetPolygons[1] = foot;

// set the tolerance of the convex hull
m_convexHullTolerance = config.check("convex_hull_tolerance", yarp::os::Value(0.01)).asDouble();
m_convexHullTolerance = config.check("convex_hull_tolerance", yarp::os::Value(0.01)).asFloat64();

return true;
}
Expand Down Expand Up @@ -338,12 +338,12 @@ bool WalkingController::initialize(const yarp::os::Searchable& config)

for (int i = 0; i < inputPtr->size(); ++i)
{
if(!inputPtr->get(i).isDouble())
if(!inputPtr->get(i).isFloat64())
{
yError() << "[initialize] The zmp position is expected to be a double";
return false;
}
m_output(i) = inputPtr->get(i).asDouble();
m_output(i) = inputPtr->get(i).asFloat64();
}

if(!initializeMatrices(config))
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ bool WalkingDCMReactiveController::initialize(const yarp::os::Searchable& config
yError() << "[initialize] Unable to get the double from searchable.";
return false;
}
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asDouble();
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asFloat64();
m_omega = sqrt(gravityAcceleration / comHeight);

m_isInitialized = true;
Expand Down
4 changes: 2 additions & 2 deletions src/TrajectoryPlanner/src/FreeSpaceEllipseManager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,14 @@ void WalkingControllers::FreeSpaceEllipseManager::referenceThread()

for (const auto& i : inputs)
{
if (!bottleInput->get(std::get<0>(i)).isDouble())
if (!bottleInput->get(std::get<0>(i)).isFloat64())
{
std::lock_guard<std::mutex> lock(m_mutex);
yError() << "[FreeSpaceEllipseManager::referenceThread] The input number " << std::get<0>(i) << "(0 based) is not a double.";
continue;
}

std::get<1>(i) = bottleInput->get(std::get<0>(i)).asDouble();
std::get<1>(i) = bottleInput->get(std::get<0>(i)).asFloat64();
}

if (!inputEllipse.setEllipse(a, b, theta, centerX, centerY))
Expand Down
2 changes: 1 addition & 1 deletion src/TrajectoryPlanner/src/StableDCMModel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ bool StableDCMModel::initialize(const yarp::os::Searchable& config)
yError() << "[initialize] Unable to get a double from a searchable.";
return false;
}
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asDouble();
double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asFloat64();

m_omega = sqrt(gravityAcceleration / comHeight);

Expand Down
58 changes: 29 additions & 29 deletions src/TrajectoryPlanner/src/TrajectoryGenerator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,10 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
}


m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble();
double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asDouble();
double stancePhaseDelaySeconds = config.check("stance_phase_delay",yarp::os::Value(0.0)).asDouble();
m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asFloat64();
m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asFloat64();
double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asFloat64();
double stancePhaseDelaySeconds = config.check("stance_phase_delay",yarp::os::Value(0.0)).asFloat64();

m_stancePhaseDelay = (std::size_t) std::round(stancePhaseDelaySeconds / m_dT);

Expand All @@ -80,40 +80,40 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
return false;
}

double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asDouble();
double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asDouble();
double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble();
double slowWhenBackwardFactor = config.check("slowWhenBackwardFactor", yarp::os::Value(1.0)).asDouble();
double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asDouble();
double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asDouble();
double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble();
double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asFloat64();
double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asFloat64();
double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asFloat64();
double slowWhenBackwardFactor = config.check("slowWhenBackwardFactor", yarp::os::Value(1.0)).asFloat64();
double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asFloat64();
double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asFloat64();
double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asFloat64();
double maxAngleVariation = iDynTree::deg2rad(config.check("maxAngleVariation",
yarp::os::Value(40.0)).asDouble());
yarp::os::Value(40.0)).asFloat64());
double minAngleVariation = iDynTree::deg2rad(config.check("minAngleVariation",
yarp::os::Value(5.0)).asDouble());
double maxStepDuration = config.check("maxStepDuration", yarp::os::Value(8.0)).asDouble();
double minStepDuration = config.check("minStepDuration", yarp::os::Value(2.9)).asDouble();
double stepHeight = config.check("stepHeight", yarp::os::Value(0.005)).asDouble();
double landingVelocity = config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble();
double apexTime = config.check("footApexTime", yarp::os::Value(0.5)).asDouble();
double comHeight = config.check("com_height", yarp::os::Value(0.49)).asDouble();
double comHeightDelta = config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble();
double nominalDuration = config.check("nominalDuration", yarp::os::Value(4.0)).asDouble();
double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble();
yarp::os::Value(5.0)).asFloat64());
double maxStepDuration = config.check("maxStepDuration", yarp::os::Value(8.0)).asFloat64();
double minStepDuration = config.check("minStepDuration", yarp::os::Value(2.9)).asFloat64();
double stepHeight = config.check("stepHeight", yarp::os::Value(0.005)).asFloat64();
double landingVelocity = config.check("stepLandingVelocity", yarp::os::Value(0.0)).asFloat64();
double apexTime = config.check("footApexTime", yarp::os::Value(0.5)).asFloat64();
double comHeight = config.check("com_height", yarp::os::Value(0.49)).asFloat64();
double comHeightDelta = config.check("comHeightDelta", yarp::os::Value(0.01)).asFloat64();
double nominalDuration = config.check("nominalDuration", yarp::os::Value(4.0)).asFloat64();
double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asFloat64();
double switchOverSwingRatio = config.check("switchOverSwingRatio",
yarp::os::Value(0.4)).asDouble();
double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble();
double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asDouble();
m_leftYawDeltaInRad = iDynTree::deg2rad(config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asDouble());
m_rightYawDeltaInRad = iDynTree::deg2rad(config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asDouble());
yarp::os::Value(0.4)).asFloat64();
double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asFloat64();
double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asFloat64();
m_leftYawDeltaInRad = iDynTree::deg2rad(config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asFloat64());
m_rightYawDeltaInRad = iDynTree::deg2rad(config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asFloat64());

m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble();
m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asFloat64();

m_swingLeft = config.check("swingLeft", yarp::os::Value(true)).asBool();
bool startWithSameFoot = config.check("startAlwaysSameFoot", yarp::os::Value(false)).asBool();
m_useMinimumJerk = config.check("useMinimumJerkFootTrajectory",
yarp::os::Value(false)).asBool();
double pitchDelta = config.check("pitchDelta", yarp::os::Value(0.0)).asDouble();
double pitchDelta = config.check("pitchDelta", yarp::os::Value(0.0)).asFloat64();

yarp::os::Bottle ellipseMethodGroup = config.findGroup("ELLIPSE_METHOD_SETTINGS");
double freeSpaceConservativeFactor = ellipseMethodGroup.check("conservative_factor", yarp::os::Value(2.0)).asFloat64();
Expand Down
Loading

0 comments on commit 9202b74

Please sign in to comment.