Skip to content

Commit

Permalink
[core] Minor cleanup for cpp codebase. Start using string_view.
Browse files Browse the repository at this point in the history
  • Loading branch information
duburcqa committed Dec 21, 2023
1 parent ed24cbf commit 12d1e49
Show file tree
Hide file tree
Showing 34 changed files with 509 additions and 573 deletions.
1 change: 0 additions & 1 deletion core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${WARN_FULL}")

# Add sources
set(SRC
"${CMAKE_CURRENT_SOURCE_DIR}/src/constants.cc"
"${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/helpers.cc"
"${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/pinocchio.cc"
"${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/json.cc"
Expand Down
33 changes: 11 additions & 22 deletions core/include/jiminy/core/constants.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#ifndef JIMINY_CONSTANTS_H
#define JIMINY_CONSTANTS_H

#include <string_view>
#include <math.h>

#include "jiminy/core/fwd.h"
Expand All @@ -9,28 +10,16 @@

namespace jiminy
{
extern const std::string JOINT_PREFIX_BASE;
extern const std::string FREE_FLYER_PREFIX_BASE_NAME;
extern const std::string FLEXIBLE_JOINT_SUFFIX;

extern const std::string TELEMETRY_FIELDNAME_DELIMITER;
extern const std::string TELEMETRY_CONSTANT_DELIMITER;
extern const int64_t TELEMETRY_MIN_BUFFER_SIZE;
extern const float64_t TELEMETRY_DEFAULT_TIME_UNIT;

/// \brief Minimum memory allocation if buffer is full and oldest stored data must be kept.
extern const uint8_t DELAY_MIN_BUFFER_RESERVE;

/// \brief Maximum amount of outdated data allowed to kept for longer than strictly necessary.
extern const uint8_t DELAY_MAX_BUFFER_EXCEED;

extern const float64_t STEPPER_MIN_TIMESTEP;
extern const float64_t SIMULATION_MIN_TIMESTEP;
extern const float64_t SIMULATION_MAX_TIMESTEP;

extern const uint32_t INIT_ITERATIONS;
extern const uint32_t PGS_MAX_ITERATIONS;
extern const float64_t PGS_MIN_REGULARIZER;
inline constexpr std::string_view TELEMETRY_FIELDNAME_DELIMITER{"."};
inline constexpr std::string_view TELEMETRY_CONSTANT_DELIMITER{"="};
/// \brief Special constant storing time unit
inline constexpr std::string_view TIME_UNIT{"Global.TIME_UNIT"};
/// \brief Special variable storing timesteps
inline constexpr std::string_view GLOBAL_TIME{"Global.Time"};

inline constexpr float64_t STEPPER_MIN_TIMESTEP{1e-10};
inline constexpr float64_t SIMULATION_MIN_TIMESTEP{1e-6};
inline constexpr float64_t SIMULATION_MAX_TIMESTEP{0.02};
}

#endif // JIMINY_CONSTANTS_H
10 changes: 5 additions & 5 deletions core/include/jiminy/core/control/abstract_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
namespace jiminy
{
/// \brief Namespace of the telemetry object.
const std::string CONTROLLER_TELEMETRY_NAMESPACE("HighLevelController");
inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"HighLevelController"};

class TelemetryData;
class Robot;
Expand Down Expand Up @@ -67,12 +67,12 @@ namespace jiminy
/// check. The user has to take care of the life span of the variable, and to
/// update it manually whenever it is necessary to do so.
///
/// \param[in] fieldnames Name of the variable. It will appear in the header of the log.
/// \param[in] name Name of the variable. It will appear in the header of the log.
/// \param[in] values Variable to add to the telemetry.
///
/// \return Return code to determine whether the execution of the method was successful.
template<typename T>
hresult_t registerVariable(const std::string & fieldname, const T & value);
hresult_t registerVariable(const std::string_view & name, const T & value);

/// \brief Dynamically registered a Eigen Vector to the telemetry.
///
Expand All @@ -91,12 +91,12 @@ namespace jiminy

/// \brief Register a constant float64 to the telemetry.
///
/// \param[in] fieldnames Name of the variable.
/// \param[in] name Name of the variable.
/// \param[in] values Variable to add to the telemetry
///
/// \return Return code to determine whether the execution of the method was successful.
template<typename T>
hresult_t registerConstant(const std::string & fieldname, const T & value);
hresult_t registerConstant(const std::string_view & name, const T & value);

/// \brief Remove all variables dynamically registered to the telemetry.
///
Expand Down
11 changes: 6 additions & 5 deletions core/include/jiminy/core/control/abstract_controller.hxx
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,8 @@ namespace jiminy
///
/// \return Return code to determine whether the execution of the method was successful.
template<typename T>
hresult_t AbstractController::registerConstant(const std::string & fieldname, const T & value)
hresult_t AbstractController::registerConstant(const std::string_view & fieldname,
const T & value)
{
// Delayed variable registration (Taken into account by 'configureTelemetry')

Expand All @@ -42,7 +43,7 @@ namespace jiminy
}

template<typename T>
hresult_t AbstractController::registerVariable(const std::string & fieldname, const T & value)
hresult_t AbstractController::registerVariable(const std::string_view & name, const T & value)
{
if (isTelemetryConfigured_)
{
Expand All @@ -53,14 +54,14 @@ namespace jiminy
// Check in local cache before.
auto variableIt = std::find_if(registeredVariables_.begin(),
registeredVariables_.end(),
[&fieldname](const auto & element)
{ return element.first == fieldname; });
[&name](const auto & element)
{ return element.first == name; });
if (variableIt != registeredVariables_.end())
{
PRINT_ERROR("Variable already registered.");
return hresult_t::ERROR_BAD_INPUT;
}
registeredVariables_.emplace_back(fieldname, &value);
registeredVariables_.emplace_back(name, &value);

return hresult_t::SUCCESS;
}
Expand Down
96 changes: 48 additions & 48 deletions core/include/jiminy/core/engine/engine_multi_robot.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,18 +12,18 @@

namespace jiminy
{
const std::string ENGINE_TELEMETRY_NAMESPACE("HighLevelController");
inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"};

enum class JIMINY_DLLAPI contactModel_t : uint8_t
{
NONE = 0,
UNSUPPORTED = 0,
SPRING_DAMPER = 1,
CONSTRAINT = 2
};

enum class JIMINY_DLLAPI constraintSolver_t : uint8_t
{
NONE = 0,
UNSUPPORTED = 0,
PGS = 1 // Projected Gauss-Seidel
};

Expand Down Expand Up @@ -90,7 +90,7 @@ namespace jiminy
GenericConfig getDefaultConstraintOptions()
{
GenericConfig config;
config["solver"] = std::string("PGS"); // ["PGS",]
config["solver"] = std::string{"PGS"}; // ["PGS",]
/// \brief Relative inverse damping wrt diagonal of J.Minv.J.t.
///
/// \details 0.0 enforces the minimum absolute regularizer.
Expand All @@ -103,7 +103,7 @@ namespace jiminy
GenericConfig getDefaultContactOptions()
{
GenericConfig config;
config["model"] = std::string("constraint"); // ["spring_damper", "constraint"]
config["model"] = std::string{"constraint"}; // ["spring_damper", "constraint"]
config["stiffness"] = 1.0e6;
config["damping"] = 2.0e3;
config["friction"] = 1.0;
Expand Down Expand Up @@ -142,7 +142,7 @@ namespace jiminy
config["verbose"] = false;
config["randomSeed"] = 0U;
/// \details Must be either "runge_kutta_dopri5", "runge_kutta_4" or "euler_explicit".
config["odeSolver"] = std::string("runge_kutta_dopri5");
config["odeSolver"] = std::string{"runge_kutta_dopri5"};
config["tolAbs"] = 1.0e-5;
config["tolRel"] = 1.0e-4;
config["dtMax"] = SIMULATION_MAX_TIMESTEP;
Expand Down Expand Up @@ -191,9 +191,9 @@ namespace jiminy
const uint32_t successiveSolveFailedMax;

constraintOptions_t(const GenericConfig & options) :
solver(boost::get<std::string>(options.at("solver"))),
regularization(boost::get<float64_t>(options.at("regularization"))),
successiveSolveFailedMax(boost::get<uint32_t>(options.at("successiveSolveFailedMax")))
solver{boost::get<std::string>(options.at("solver"))},
regularization{boost::get<float64_t>(options.at("regularization"))},
successiveSolveFailedMax{boost::get<uint32_t>(options.at("successiveSolveFailedMax"))}
{
}
};
Expand All @@ -210,14 +210,14 @@ namespace jiminy
const float64_t stabilizationFreq;

contactOptions_t(const GenericConfig & options) :
model(boost::get<std::string>(options.at("model"))),
stiffness(boost::get<float64_t>(options.at("stiffness"))),
damping(boost::get<float64_t>(options.at("damping"))),
friction(boost::get<float64_t>(options.at("friction"))),
torsion(boost::get<float64_t>(options.at("torsion"))),
transitionEps(boost::get<float64_t>(options.at("transitionEps"))),
transitionVelocity(boost::get<float64_t>(options.at("transitionVelocity"))),
stabilizationFreq(boost::get<float64_t>(options.at("stabilizationFreq")))
model{boost::get<std::string>(options.at("model"))},
stiffness{boost::get<float64_t>(options.at("stiffness"))},
damping{boost::get<float64_t>(options.at("damping"))},
friction{boost::get<float64_t>(options.at("friction"))},
torsion{boost::get<float64_t>(options.at("torsion"))},
transitionEps{boost::get<float64_t>(options.at("transitionEps"))},
transitionVelocity{boost::get<float64_t>(options.at("transitionVelocity"))},
stabilizationFreq{boost::get<float64_t>(options.at("stabilizationFreq"))}
{
}
};
Expand All @@ -228,8 +228,8 @@ namespace jiminy
const float64_t boundDamping;

jointOptions_t(const GenericConfig & options) :
boundStiffness(boost::get<float64_t>(options.at("boundStiffness"))),
boundDamping(boost::get<float64_t>(options.at("boundDamping")))
boundStiffness{boost::get<float64_t>(options.at("boundStiffness"))},
boundDamping{boost::get<float64_t>(options.at("boundDamping"))}
{
}
};
Expand All @@ -240,8 +240,8 @@ namespace jiminy
const HeightmapFunctor groundProfile;

worldOptions_t(const GenericConfig & options) :
gravity(boost::get<Eigen::VectorXd>(options.at("gravity"))),
groundProfile(boost::get<HeightmapFunctor>(options.at("groundProfile")))
gravity{boost::get<Eigen::VectorXd>(options.at("gravity"))},
groundProfile{boost::get<HeightmapFunctor>(options.at("groundProfile"))}
{
}
};
Expand All @@ -263,19 +263,19 @@ namespace jiminy
const bool_t logInternalStepperSteps;

stepperOptions_t(const GenericConfig & options) :
verbose(boost::get<bool_t>(options.at("verbose"))),
randomSeed(boost::get<uint32_t>(options.at("randomSeed"))),
odeSolver(boost::get<std::string>(options.at("odeSolver"))),
tolAbs(boost::get<float64_t>(options.at("tolAbs"))),
tolRel(boost::get<float64_t>(options.at("tolRel"))),
dtMax(boost::get<float64_t>(options.at("dtMax"))),
dtRestoreThresholdRel(boost::get<float64_t>(options.at("dtRestoreThresholdRel"))),
successiveIterFailedMax(boost::get<uint32_t>(options.at("successiveIterFailedMax"))),
iterMax(boost::get<uint32_t>(options.at("iterMax"))),
timeout(boost::get<float64_t>(options.at("timeout"))),
sensorsUpdatePeriod(boost::get<float64_t>(options.at("sensorsUpdatePeriod"))),
controllerUpdatePeriod(boost::get<float64_t>(options.at("controllerUpdatePeriod"))),
logInternalStepperSteps(boost::get<bool_t>(options.at("logInternalStepperSteps")))
verbose{boost::get<bool_t>(options.at("verbose"))},
randomSeed{boost::get<uint32_t>(options.at("randomSeed"))},
odeSolver{boost::get<std::string>(options.at("odeSolver"))},
tolAbs{boost::get<float64_t>(options.at("tolAbs"))},
tolRel{boost::get<float64_t>(options.at("tolRel"))},
dtMax{boost::get<float64_t>(options.at("dtMax"))},
dtRestoreThresholdRel{boost::get<float64_t>(options.at("dtRestoreThresholdRel"))},
successiveIterFailedMax{boost::get<uint32_t>(options.at("successiveIterFailedMax"))},
iterMax{boost::get<uint32_t>(options.at("iterMax"))},
timeout{boost::get<float64_t>(options.at("timeout"))},
sensorsUpdatePeriod{boost::get<float64_t>(options.at("sensorsUpdatePeriod"))},
controllerUpdatePeriod{boost::get<float64_t>(options.at("controllerUpdatePeriod"))},
logInternalStepperSteps{boost::get<bool_t>(options.at("logInternalStepperSteps"))}
{
}
};
Expand All @@ -292,14 +292,14 @@ namespace jiminy
const bool_t enableEnergy;

telemetryOptions_t(const GenericConfig & options) :
isPersistent(boost::get<bool_t>(options.at("isPersistent"))),
enableConfiguration(boost::get<bool_t>(options.at("enableConfiguration"))),
enableVelocity(boost::get<bool_t>(options.at("enableVelocity"))),
enableAcceleration(boost::get<bool_t>(options.at("enableAcceleration"))),
enableForceExternal(boost::get<bool_t>(options.at("enableForceExternal"))),
enableCommand(boost::get<bool_t>(options.at("enableCommand"))),
enableMotorEffort(boost::get<bool_t>(options.at("enableMotorEffort"))),
enableEnergy(boost::get<bool_t>(options.at("enableEnergy")))
isPersistent{boost::get<bool_t>(options.at("isPersistent"))},
enableConfiguration{boost::get<bool_t>(options.at("enableConfiguration"))},
enableVelocity{boost::get<bool_t>(options.at("enableVelocity"))},
enableAcceleration{boost::get<bool_t>(options.at("enableAcceleration"))},
enableForceExternal{boost::get<bool_t>(options.at("enableForceExternal"))},
enableCommand{boost::get<bool_t>(options.at("enableCommand"))},
enableMotorEffort{boost::get<bool_t>(options.at("enableMotorEffort"))},
enableEnergy{boost::get<bool_t>(options.at("enableEnergy"))}
{
}
};
Expand All @@ -314,12 +314,12 @@ namespace jiminy
const contactOptions_t contacts;

engineOptions_t(const GenericConfig & options) :
telemetry(boost::get<GenericConfig>(options.at("telemetry"))),
stepper(boost::get<GenericConfig>(options.at("stepper"))),
world(boost::get<GenericConfig>(options.at("world"))),
joints(boost::get<GenericConfig>(options.at("joints"))),
constraints(boost::get<GenericConfig>(options.at("constraints"))),
contacts(boost::get<GenericConfig>(options.at("contacts")))
telemetry{boost::get<GenericConfig>(options.at("telemetry"))},
stepper{boost::get<GenericConfig>(options.at("stepper"))},
world{boost::get<GenericConfig>(options.at("world"))},
joints{boost::get<GenericConfig>(options.at("joints"))},
constraints{boost::get<GenericConfig>(options.at("constraints"))},
contacts{boost::get<GenericConfig>(options.at("contacts"))}
{
}
};
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/fwd.h
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ namespace jiminy
// Flexible joints
struct FlexibleJointData
{
// FIXME: Replace by default spaceship operator `<=>` starting from C++20.
// FIXME: Replace by default spaceship operator `<=>` when moving to C++20.
inline bool_t operator==(const FlexibleJointData & other) const noexcept
{
return (this->frameName == other.frameName && this->stiffness == other.stiffness &&
Expand Down
6 changes: 3 additions & 3 deletions core/include/jiminy/core/hardware/abstract_sensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -153,8 +153,8 @@ namespace jiminy
virtual GenericConfig getDefaultSensorOptions()
{
GenericConfig config;
config["noiseStd"] = Eigen::VectorXd();
config["bias"] = Eigen::VectorXd();
config["noiseStd"] = Eigen::VectorXd{};
config["bias"] = Eigen::VectorXd{};
config["delay"] = 0.0;
config["jitter"] = 0.0;
config["delayInterpolationOrder"] = 1U;
Expand Down Expand Up @@ -402,7 +402,7 @@ namespace jiminy
TelemetrySender telemetrySender_;
};

template<class T>
template<typename T>
class AbstractSensorTpl : public AbstractSensorBase
{
public:
Expand Down
6 changes: 5 additions & 1 deletion core/include/jiminy/core/hardware/abstract_sensor.hxx
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@

namespace jiminy
{
inline constexpr uint8_t DELAY_MIN_BUFFER_RESERVE{20U};
inline constexpr uint8_t DELAY_MAX_BUFFER_EXCEED{100U};

// ========================== AbstractSensorBase ==============================

template<typename DerivedType>
Expand Down Expand Up @@ -259,12 +262,13 @@ namespace jiminy
{
return fieldnames_.size();
}

template<typename T>
std::string AbstractSensorTpl<T>::getTelemetryName() const
{
if (areFieldnamesGrouped_)
{
return getType() + TELEMETRY_FIELDNAME_DELIMITER + name_;
return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER);
}
else
{
Expand Down
Loading

0 comments on commit 12d1e49

Please sign in to comment.