diff --git a/core/include/jiminy/core/constants.h b/core/include/jiminy/core/constants.h index aa0b8e3865..a48133bc50 100644 --- a/core/include/jiminy/core/constants.h +++ b/core/include/jiminy/core/constants.h @@ -1,6 +1,7 @@ #ifndef JIMINY_CONSTANTS_H #define JIMINY_CONSTANTS_H +#include #include #include "jiminy/core/fwd.h" @@ -9,12 +10,8 @@ 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; + inline constexpr std::string_view TELEMETRY_FIELDNAME_DELIMITER{"."}; + inline constexpr std::string_view TELEMETRY_CONSTANT_DELIMITER{"="}; extern const int64_t TELEMETRY_MIN_BUFFER_SIZE; extern const float64_t TELEMETRY_DEFAULT_TIME_UNIT; diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h index fc7a4f7597..73c0757ca9 100644 --- a/core/include/jiminy/core/control/abstract_controller.h +++ b/core/include/jiminy/core/control/abstract_controller.h @@ -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; @@ -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 - 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. /// @@ -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 - 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. /// diff --git a/core/include/jiminy/core/control/abstract_controller.hxx b/core/include/jiminy/core/control/abstract_controller.hxx index e1a8644fd4..0ea21256f8 100644 --- a/core/include/jiminy/core/control/abstract_controller.hxx +++ b/core/include/jiminy/core/control/abstract_controller.hxx @@ -16,7 +16,8 @@ namespace jiminy /// /// \return Return code to determine whether the execution of the method was successful. template - 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') @@ -42,7 +43,7 @@ namespace jiminy } template - 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_) { @@ -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; } diff --git a/core/include/jiminy/core/engine/engine_multi_robot.h b/core/include/jiminy/core/engine/engine_multi_robot.h index bb8b8522bc..dbc8bfc8bc 100644 --- a/core/include/jiminy/core/engine/engine_multi_robot.h +++ b/core/include/jiminy/core/engine/engine_multi_robot.h @@ -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 }; @@ -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. @@ -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; @@ -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; @@ -191,9 +191,9 @@ namespace jiminy const uint32_t successiveSolveFailedMax; constraintOptions_t(const GenericConfig & options) : - solver(boost::get(options.at("solver"))), - regularization(boost::get(options.at("regularization"))), - successiveSolveFailedMax(boost::get(options.at("successiveSolveFailedMax"))) + solver{boost::get(options.at("solver"))}, + regularization{boost::get(options.at("regularization"))}, + successiveSolveFailedMax{boost::get(options.at("successiveSolveFailedMax"))} { } }; @@ -210,14 +210,14 @@ namespace jiminy const float64_t stabilizationFreq; contactOptions_t(const GenericConfig & options) : - model(boost::get(options.at("model"))), - stiffness(boost::get(options.at("stiffness"))), - damping(boost::get(options.at("damping"))), - friction(boost::get(options.at("friction"))), - torsion(boost::get(options.at("torsion"))), - transitionEps(boost::get(options.at("transitionEps"))), - transitionVelocity(boost::get(options.at("transitionVelocity"))), - stabilizationFreq(boost::get(options.at("stabilizationFreq"))) + model{boost::get(options.at("model"))}, + stiffness{boost::get(options.at("stiffness"))}, + damping{boost::get(options.at("damping"))}, + friction{boost::get(options.at("friction"))}, + torsion{boost::get(options.at("torsion"))}, + transitionEps{boost::get(options.at("transitionEps"))}, + transitionVelocity{boost::get(options.at("transitionVelocity"))}, + stabilizationFreq{boost::get(options.at("stabilizationFreq"))} { } }; @@ -228,8 +228,8 @@ namespace jiminy const float64_t boundDamping; jointOptions_t(const GenericConfig & options) : - boundStiffness(boost::get(options.at("boundStiffness"))), - boundDamping(boost::get(options.at("boundDamping"))) + boundStiffness{boost::get(options.at("boundStiffness"))}, + boundDamping{boost::get(options.at("boundDamping"))} { } }; @@ -240,8 +240,8 @@ namespace jiminy const HeightmapFunctor groundProfile; worldOptions_t(const GenericConfig & options) : - gravity(boost::get(options.at("gravity"))), - groundProfile(boost::get(options.at("groundProfile"))) + gravity{boost::get(options.at("gravity"))}, + groundProfile{boost::get(options.at("groundProfile"))} { } }; @@ -263,19 +263,19 @@ namespace jiminy const bool_t logInternalStepperSteps; stepperOptions_t(const GenericConfig & options) : - verbose(boost::get(options.at("verbose"))), - randomSeed(boost::get(options.at("randomSeed"))), - odeSolver(boost::get(options.at("odeSolver"))), - tolAbs(boost::get(options.at("tolAbs"))), - tolRel(boost::get(options.at("tolRel"))), - dtMax(boost::get(options.at("dtMax"))), - dtRestoreThresholdRel(boost::get(options.at("dtRestoreThresholdRel"))), - successiveIterFailedMax(boost::get(options.at("successiveIterFailedMax"))), - iterMax(boost::get(options.at("iterMax"))), - timeout(boost::get(options.at("timeout"))), - sensorsUpdatePeriod(boost::get(options.at("sensorsUpdatePeriod"))), - controllerUpdatePeriod(boost::get(options.at("controllerUpdatePeriod"))), - logInternalStepperSteps(boost::get(options.at("logInternalStepperSteps"))) + verbose{boost::get(options.at("verbose"))}, + randomSeed{boost::get(options.at("randomSeed"))}, + odeSolver{boost::get(options.at("odeSolver"))}, + tolAbs{boost::get(options.at("tolAbs"))}, + tolRel{boost::get(options.at("tolRel"))}, + dtMax{boost::get(options.at("dtMax"))}, + dtRestoreThresholdRel{boost::get(options.at("dtRestoreThresholdRel"))}, + successiveIterFailedMax{boost::get(options.at("successiveIterFailedMax"))}, + iterMax{boost::get(options.at("iterMax"))}, + timeout{boost::get(options.at("timeout"))}, + sensorsUpdatePeriod{boost::get(options.at("sensorsUpdatePeriod"))}, + controllerUpdatePeriod{boost::get(options.at("controllerUpdatePeriod"))}, + logInternalStepperSteps{boost::get(options.at("logInternalStepperSteps"))} { } }; @@ -292,14 +292,14 @@ namespace jiminy const bool_t enableEnergy; telemetryOptions_t(const GenericConfig & options) : - isPersistent(boost::get(options.at("isPersistent"))), - enableConfiguration(boost::get(options.at("enableConfiguration"))), - enableVelocity(boost::get(options.at("enableVelocity"))), - enableAcceleration(boost::get(options.at("enableAcceleration"))), - enableForceExternal(boost::get(options.at("enableForceExternal"))), - enableCommand(boost::get(options.at("enableCommand"))), - enableMotorEffort(boost::get(options.at("enableMotorEffort"))), - enableEnergy(boost::get(options.at("enableEnergy"))) + isPersistent{boost::get(options.at("isPersistent"))}, + enableConfiguration{boost::get(options.at("enableConfiguration"))}, + enableVelocity{boost::get(options.at("enableVelocity"))}, + enableAcceleration{boost::get(options.at("enableAcceleration"))}, + enableForceExternal{boost::get(options.at("enableForceExternal"))}, + enableCommand{boost::get(options.at("enableCommand"))}, + enableMotorEffort{boost::get(options.at("enableMotorEffort"))}, + enableEnergy{boost::get(options.at("enableEnergy"))} { } }; @@ -314,12 +314,12 @@ namespace jiminy const contactOptions_t contacts; engineOptions_t(const GenericConfig & options) : - telemetry(boost::get(options.at("telemetry"))), - stepper(boost::get(options.at("stepper"))), - world(boost::get(options.at("world"))), - joints(boost::get(options.at("joints"))), - constraints(boost::get(options.at("constraints"))), - contacts(boost::get(options.at("contacts"))) + telemetry{boost::get(options.at("telemetry"))}, + stepper{boost::get(options.at("stepper"))}, + world{boost::get(options.at("world"))}, + joints{boost::get(options.at("joints"))}, + constraints{boost::get(options.at("constraints"))}, + contacts{boost::get(options.at("contacts"))} { } }; diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index a833fc6d72..fd731d4afd 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -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 && diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h index 48e62df952..ea52fa6f6c 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.h +++ b/core/include/jiminy/core/hardware/abstract_sensor.h @@ -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; @@ -402,7 +402,7 @@ namespace jiminy TelemetrySender telemetrySender_; }; - template + template class AbstractSensorTpl : public AbstractSensorBase { public: diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index c229d7c9ee..f8b5bee706 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -259,12 +259,13 @@ namespace jiminy { return fieldnames_.size(); } + template std::string AbstractSensorTpl::getTelemetryName() const { if (areFieldnamesGrouped_) { - return getType() + TELEMETRY_FIELDNAME_DELIMITER + name_; + return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER); } else { diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h index 6df33586a6..48af760a35 100644 --- a/core/include/jiminy/core/hardware/basic_sensors.h +++ b/core/include/jiminy/core/hardware/basic_sensors.h @@ -45,9 +45,9 @@ namespace jiminy template<> const std::string AbstractSensorTpl::type_; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_; - template<> const std::vector AbstractSensorTpl::fieldnames_; + template<> + const bool_t AbstractSensorTpl::areFieldnamesGrouped_; class JIMINY_DLLAPI ContactSensor : public AbstractSensorTpl { @@ -80,9 +80,9 @@ namespace jiminy template<> const std::string AbstractSensorTpl::type_; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_; - template<> const std::vector AbstractSensorTpl::fieldnames_; + template<> + const bool_t AbstractSensorTpl::areFieldnamesGrouped_; class JIMINY_DLLAPI ForceSensor : public AbstractSensorTpl { @@ -118,9 +118,9 @@ namespace jiminy template<> const std::string AbstractSensorTpl::type_; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_; - template<> const std::vector AbstractSensorTpl::fieldnames_; + template<> + const bool_t AbstractSensorTpl::areFieldnamesGrouped_; class JIMINY_DLLAPI EncoderSensor : public AbstractSensorTpl { @@ -155,9 +155,9 @@ namespace jiminy template<> const std::string AbstractSensorTpl::type_; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_; - template<> const std::vector AbstractSensorTpl::fieldnames_; + template<> + const bool_t AbstractSensorTpl::areFieldnamesGrouped_; class JIMINY_DLLAPI EffortSensor : public AbstractSensorTpl { @@ -190,9 +190,9 @@ namespace jiminy template<> const std::string AbstractSensorTpl::type_; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_; - template<> const std::vector AbstractSensorTpl::fieldnames_; + template<> + const bool_t AbstractSensorTpl::areFieldnamesGrouped_; } #endif // end of JIMINY_BASIC_SENSORS_H \ No newline at end of file diff --git a/core/include/jiminy/core/io/abstract_io_device.hxx b/core/include/jiminy/core/io/abstract_io_device.hxx index 646595fb24..a8c330c91a 100644 --- a/core/include/jiminy/core/io/abstract_io_device.hxx +++ b/core/include/jiminy/core/io/abstract_io_device.hxx @@ -34,7 +34,7 @@ namespace jiminy // write template<> - hresult_t AbstractIODevice::write(const std::string & str); + hresult_t AbstractIODevice::write(const std::string_view & str); template<> hresult_t AbstractIODevice::write>(const std::vector & v); template<> diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index 886f815d27..3cd4dc24e0 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -13,6 +13,10 @@ namespace jiminy { + inline constexpr std::string_view JOINT_PREFIX_BASE{"current"}; + inline constexpr std::string_view FREE_FLYER_NAME{"Freeflyer"}; + inline constexpr std::string_view FLEXIBLE_JOINT_SUFFIX{"Flexibility"}; + class AbstractConstraintBase; class FrameConstraint; class JointConstraint; @@ -124,11 +128,11 @@ namespace jiminy GenericConfig config; config["enablePositionLimit"] = true; config["positionLimitFromUrdf"] = true; - config["positionLimitMin"] = Eigen::VectorXd(); - config["positionLimitMax"] = Eigen::VectorXd(); + config["positionLimitMin"] = Eigen::VectorXd{}; + config["positionLimitMax"] = Eigen::VectorXd{}; config["enableVelocityLimit"] = true; config["velocityLimitFromUrdf"] = true; - config["velocityLimit"] = Eigen::VectorXd(); + config["velocityLimit"] = Eigen::VectorXd{}; return config; }; @@ -142,7 +146,7 @@ namespace jiminy config["centerOfMassPositionBodiesBiasStd"] = 0.0; config["relativePositionBodiesBiasStd"] = 0.0; config["enableFlexibleModel"] = true; - config["flexibilityConfig"] = FlexibilityConfig(); + config["flexibilityConfig"] = FlexibilityConfig{}; return config; }; @@ -180,13 +184,13 @@ namespace jiminy const Eigen::VectorXd velocityLimit; jointOptions_t(const GenericConfig & options) : - enablePositionLimit(boost::get(options.at("enablePositionLimit"))), - positionLimitFromUrdf(boost::get(options.at("positionLimitFromUrdf"))), - positionLimitMin(boost::get(options.at("positionLimitMin"))), - positionLimitMax(boost::get(options.at("positionLimitMax"))), - enableVelocityLimit(boost::get(options.at("enableVelocityLimit"))), - velocityLimitFromUrdf(boost::get(options.at("velocityLimitFromUrdf"))), - velocityLimit(boost::get(options.at("velocityLimit"))) + enablePositionLimit{boost::get(options.at("enablePositionLimit"))}, + positionLimitFromUrdf{boost::get(options.at("positionLimitFromUrdf"))}, + positionLimitMin{boost::get(options.at("positionLimitMin"))}, + positionLimitMax{boost::get(options.at("positionLimitMax"))}, + enableVelocityLimit{boost::get(options.at("enableVelocityLimit"))}, + velocityLimitFromUrdf{boost::get(options.at("velocityLimitFromUrdf"))}, + velocityLimit{boost::get(options.at("velocityLimit"))} { } }; @@ -201,14 +205,14 @@ namespace jiminy const FlexibilityConfig flexibilityConfig; dynamicsOptions_t(const GenericConfig & options) : - inertiaBodiesBiasStd(boost::get(options.at("inertiaBodiesBiasStd"))), - massBodiesBiasStd(boost::get(options.at("massBodiesBiasStd"))), - centerOfMassPositionBodiesBiasStd( - boost::get(options.at("centerOfMassPositionBodiesBiasStd"))), - relativePositionBodiesBiasStd( - boost::get(options.at("relativePositionBodiesBiasStd"))), - enableFlexibleModel(boost::get(options.at("enableFlexibleModel"))), - flexibilityConfig(boost::get(options.at("flexibilityConfig"))) + inertiaBodiesBiasStd{boost::get(options.at("inertiaBodiesBiasStd"))}, + massBodiesBiasStd{boost::get(options.at("massBodiesBiasStd"))}, + centerOfMassPositionBodiesBiasStd{ + boost::get(options.at("centerOfMassPositionBodiesBiasStd"))}, + relativePositionBodiesBiasStd{ + boost::get(options.at("relativePositionBodiesBiasStd"))}, + enableFlexibleModel{boost::get(options.at("enableFlexibleModel"))}, + flexibilityConfig{boost::get(options.at("flexibilityConfig"))} { } }; @@ -218,7 +222,7 @@ namespace jiminy const uint32_t maxContactPointsPerBody; collisionOptions_t(const GenericConfig & options) : - maxContactPointsPerBody(boost::get(options.at("maxContactPointsPerBody"))) + maxContactPointsPerBody{boost::get(options.at("maxContactPointsPerBody"))} { } }; @@ -230,9 +234,9 @@ namespace jiminy const collisionOptions_t collisions; modelOptions_t(const GenericConfig & options) : - dynamics(boost::get(options.at("dynamics"))), - joints(boost::get(options.at("joints"))), - collisions(boost::get(options.at("collisions"))) + dynamics{boost::get(options.at("dynamics"))}, + joints{boost::get(options.at("joints"))}, + collisions{boost::get(options.at("collisions"))} { } }; diff --git a/core/include/jiminy/core/telemetry/telemetry_data.h b/core/include/jiminy/core/telemetry/telemetry_data.h index aa0035c853..31c7edfa8b 100644 --- a/core/include/jiminy/core/telemetry/telemetry_data.h +++ b/core/include/jiminy/core/telemetry/telemetry_data.h @@ -9,23 +9,23 @@ namespace jiminy { /// \brief Version of the telemetry format. - const int32_t TELEMETRY_VERSION = 1; + inline constexpr int32_t TELEMETRY_VERSION{1}; /// \brief Number of integers in the data section. - const std::string NUM_INTS("NumIntEntries="); + inline constexpr std::string_view NUM_INTS{"NumIntEntries="}; /// \brief Number of floats in the data section. - const std::string NUM_FLOATS("NumFloatEntries="); + inline constexpr std::string_view NUM_FLOATS{"NumFloatEntries="}; /// \brief Special column - const std::string GLOBAL_TIME("Global.Time"); + inline constexpr std::string_view GLOBAL_TIME{"Global.Time"}; /// \brief Special constant - const std::string TIME_UNIT("Global.TIME_UNIT"); + inline constexpr std::string_view TIME_UNIT{"Global.TIME_UNIT"}; /// \brief Marker of the beginning the constants section. - const std::string START_CONSTANTS("StartConstants"); + inline constexpr std::string_view START_CONSTANTS{"StartConstants"}; /// \brief Marker of the beginning the columns section. - const std::string START_COLUMNS("StartColumns"); + inline constexpr std::string_view START_COLUMNS{"StartColumns"}; /// \brief Marker of the beginning of a line of data. - const std::string START_LINE_TOKEN("StartLine"); + inline constexpr std::string_view START_LINE_TOKEN{"StartLine"}; /// \brief Marker of the beginning of the data section. - const std::string START_DATA("StartData"); + inline constexpr std::string_view START_DATA{"StartData"}; /// \brief This class manages the data structures of the telemetry. class JIMINY_DLLAPI TelemetryData @@ -44,21 +44,20 @@ namespace jiminy /// /// \warning The only supported types are int64_t and float64_t. /// - /// \param[in] variableNameIn Name of the variable to register. - /// \param[out] positionInBufferOut Pointer on the allocated buffer holding the variable. + /// \param[in] name Name of the variable to register. + /// \param[out] positionInBuffer Pointer on the allocated buffer holding the variable. /// /// \return S_OK if successful, the corresponding telemetry error otherwise. template - hresult_t registerVariable(const std::string & variableNameIn, T *& positionInBufferOut); + hresult_t registerVariable(const std::string & name, T *& positionInBuffer); /// \brief Register a constant for the telemetry. /// - /// \param[in] invariantNameIn Name of the invariant. - /// \param[in] valueIn Value of the invariant. + /// \param[in] name Name of the invariant. + /// \param[in] value Value of the invariant. /// /// \return S_OK if successful, the corresponding telemetry error otherwise. - hresult_t registerConstant(const std::string & invariantNameIn, - const std::string & valueIn); + hresult_t registerConstant(const std::string & name, const std::string & value); /// \brief Format the telemetry header with the current recorded informations. /// diff --git a/core/include/jiminy/core/telemetry/telemetry_data.hxx b/core/include/jiminy/core/telemetry/telemetry_data.hxx index b957a90d16..b4186d1c4d 100644 --- a/core/include/jiminy/core/telemetry/telemetry_data.hxx +++ b/core/include/jiminy/core/telemetry/telemetry_data.hxx @@ -9,21 +9,19 @@ namespace jiminy { template - hresult_t TelemetryData::registerVariable(const std::string & variableName, - T *& positionInBufferOut) + hresult_t TelemetryData::registerVariable(const std::string & name, T *& positionInBuffer) { // Get the right registry std::deque> * registry = getRegistry(); // Check if already in memory - auto variableIt = - std::find_if(registry->begin(), - registry->end(), - [&variableName](const std::pair & element) -> bool_t - { return element.first == variableName; }); + auto variableIt = std::find_if(registry->begin(), + registry->end(), + [&name](const std::pair & element) -> bool_t + { return element.first == name; }); if (variableIt != registry->end()) { - positionInBufferOut = &(variableIt->second); + positionInBuffer = &(variableIt->second); return hresult_t::SUCCESS; } @@ -35,8 +33,8 @@ namespace jiminy } // Create new variable in registry - registry->push_back({variableName, {}}); - positionInBufferOut = &(registry->back().second); + registry->emplace_back(name, T{}); + positionInBuffer = &(registry->back().second); return hresult_t::SUCCESS; } diff --git a/core/include/jiminy/core/telemetry/telemetry_sender.h b/core/include/jiminy/core/telemetry/telemetry_sender.h index 7ce1be2414..031775f4d2 100644 --- a/core/include/jiminy/core/telemetry/telemetry_sender.h +++ b/core/include/jiminy/core/telemetry/telemetry_sender.h @@ -13,7 +13,7 @@ namespace jiminy { class TelemetryData; - const std::string DEFAULT_TELEMETRY_NAMESPACE("Uninitialized Object"); + inline constexpr std::string_view DEFAULT_TELEMETRY_NAMESPACE{"Uninitialized Object"}; /// \brief Class to inherit for sending telemetry data. class JIMINY_DLLAPI TelemetrySender @@ -40,7 +40,7 @@ namespace jiminy /// \param[in] telemetryDataInstance Shared pointer to the telemetry instance. /// \param[in] objectName Name of the object. void configureObject(std::shared_ptr telemetryDataInstance, - const std::string & objectName); + const std::string_view & objectName); /// \brief Register a new variable to the telemetry. /// @@ -48,20 +48,21 @@ namespace jiminy /// user is responsible for managing its lifetime and updating it in-place. The /// telemetry sender will fetch its value when calling 'updateValues'. /// - /// \param[in] fieldname Name of the field to record in the telemetry. + /// \param[in] name Name of the field to record in the telemetry. /// \param[in] value Pointer to the newly recorded field. - template - hresult_t registerVariable(const std::string & fieldname, const T * value); + template + std::enable_if_t, hresult_t> + registerVariable(const std::string & name, const Scalar * value); - template - hresult_t registerVariable(const std::vector & fieldnames, + template + hresult_t registerVariable(const std::vector & fieldnames, const Eigen::MatrixBase & values); /// \brief Add an invariant header entry in the log file. /// - /// \param[in] invariantName Name of the invariant. + /// \param[in] name Name of the invariant. /// \param[in] value Value of the invariant. - hresult_t registerConstant(const std::string & invariantName, const std::string & value); + hresult_t registerConstant(const std::string & name, const std::string & value); /// \brief Update all registered variables in the telemetry buffer. void updateValues(); diff --git a/core/include/jiminy/core/telemetry/telemetry_sender.hxx b/core/include/jiminy/core/telemetry/telemetry_sender.hxx index ea3ea198d5..a93ab67061 100644 --- a/core/include/jiminy/core/telemetry/telemetry_sender.hxx +++ b/core/include/jiminy/core/telemetry/telemetry_sender.hxx @@ -1,30 +1,34 @@ #ifndef JIMINY_TELEMETRY_SENDER_HXX #define JIMINY_TELEMETRY_SENDER_HXX -#include "jiminy/core/telemetry/telemetry_data.h" #include "jiminy/core/constants.h" +#include "jiminy/core/exceptions.h" +#include "jiminy/core/utilities/helpers.h" +#include "jiminy/core/telemetry/telemetry_data.h" namespace jiminy { - template - hresult_t TelemetrySender::registerVariable(const std::string & fieldname, const T * value) + template + std::enable_if_t, hresult_t> + TelemetrySender::registerVariable(const std::string & name, const Scalar * value) { - T * positionInBuffer = nullptr; - const std::string fullFieldName = objectName_ + TELEMETRY_FIELDNAME_DELIMITER + fieldname; + Scalar * positionInBuffer = nullptr; + const std::string fullName = + addCircumfix(name, objectName_, {}, TELEMETRY_FIELDNAME_DELIMITER); - hresult_t returnCode = telemetryData_->registerVariable(fullFieldName, positionInBuffer); + hresult_t returnCode = telemetryData_->registerVariable(fullName, positionInBuffer); if (returnCode == hresult_t::SUCCESS) { - bufferPosition_.emplace_back(telemetry_data_pair_t{value, positionInBuffer}); + bufferPosition_.emplace_back(telemetry_data_pair_t{value, positionInBuffer}); *positionInBuffer = *value; } return returnCode; } - template - hresult_t TelemetrySender::registerVariable(const std::vector & fieldnames, + template + hresult_t TelemetrySender::registerVariable(const std::vector & fieldnames, const Eigen::MatrixBase & values) { hresult_t returnCode = hresult_t::SUCCESS; diff --git a/core/include/jiminy/core/utilities/helpers.h b/core/include/jiminy/core/utilities/helpers.h index 5c78cfef48..252db4f98a 100644 --- a/core/include/jiminy/core/utilities/helpers.h +++ b/core/include/jiminy/core/utilities/helpers.h @@ -99,13 +99,13 @@ namespace jiminy bool_t endsWith(const std::string & text, const std::string & ending); std::string addCircumfix(std::string fieldname, // Make a copy - const std::string & prefix = {}, - const std::string & suffix = {}, - const std::string & delimiter = {}); + const std::string_view & prefix = {}, + const std::string_view & suffix = {}, + const std::string_view & delimiter = {}); std::vector addCircumfix(const std::vector & fieldnamesIn, - const std::string & prefix = {}, - const std::string & suffix = {}, - const std::string & delimiter = {}); + const std::string_view & prefix = {}, + const std::string_view & suffix = {}, + const std::string_view & delimiter = {}); std::string removeSuffix(std::string fieldname, // Make a copy const std::string & suffix); @@ -149,23 +149,23 @@ namespace jiminy template bool_t checkDuplicates(const std::vector & vect); - template - bool_t checkIntersection(const std::vector & vect1, const std::vector & vect2); + template + bool_t checkIntersection(const std::vector & vect1, const std::vector & vect2); - template - bool_t checkInclusion(const std::vector & vect1, const std::vector & vect2); + template + bool_t checkInclusion(const std::vector & vect1, const std::vector & vect2); - template - void eraseVector(std::vector & vect1, const std::vector & vect2); + template + void eraseVector(std::vector & vect1, const std::vector & vect2); // *********************** Miscellaneous ************************** template - void swapMatrixBlocks(const Eigen::MatrixBase & matrixIn, - Eigen::Index firstBlockStart, - Eigen::Index firstBlockLength, - Eigen::Index secondBlockStart, - Eigen::Index secondBlockLength); + void swapMatrixRows(const Eigen::MatrixBase & matrixIn, + Eigen::Index firstBlockStart, + Eigen::Index firstBlockLength, + Eigen::Index secondBlockStart, + Eigen::Index secondBlockLength); } #include "jiminy/core/utilities/helpers.hxx" diff --git a/core/include/jiminy/core/utilities/helpers.hxx b/core/include/jiminy/core/utilities/helpers.hxx index 78cafcbacc..822a61a302 100644 --- a/core/include/jiminy/core/utilities/helpers.hxx +++ b/core/include/jiminy/core/utilities/helpers.hxx @@ -192,8 +192,8 @@ namespace jiminy return false; } - template - bool_t checkIntersection(const std::vector & vect1, const std::vector & vect2) + template + bool_t checkIntersection(const std::vector & vect1, const std::vector & vect2) { auto vect2It = std::find_if(vect2.begin(), vect2.end(), @@ -206,8 +206,8 @@ namespace jiminy return (vect2It != vect2.end()); } - template - bool_t checkInclusion(const std::vector & vect1, const std::vector & vect2) + template + bool_t checkInclusion(const std::vector & vect1, const std::vector & vect2) { for (const auto & elem2 : vect2) { @@ -220,8 +220,8 @@ namespace jiminy return true; } - template - void eraseVector(std::vector & vect1, const std::vector & vect2) + template + void eraseVector(std::vector & vect1, const std::vector & vect2) { vect1.erase(std::remove_if(vect1.begin(), vect1.end(), @@ -237,8 +237,8 @@ namespace jiminy /// \brief Swap two non-overlapping row-blocks of data in a matrix. /// - /// \details Given two blocks of arbitrary sizes b1, b2 in a matrix B = (... b1 ... b2 ...), - /// this function re-assigns B to (... b2 ... b1 ...). + /// \details Let b1, b2 be two row-blocks of arbitrary sizes of a matrix B s.t. + /// B = (... b1 ... b2 ...).T. This function re-assigns B to (... b2 ... b1 ...).T. /// /// \pre firstBlockStart + firstBlockLength <= secondBlockStart /// @@ -248,11 +248,11 @@ namespace jiminy /// \param[in] secondBlockStart Start index of the second block. /// \param[in] secondBlockLength Length of the second block. template - void swapMatrixBlocks(const Eigen::MatrixBase & matrixIn, - Eigen::Index firstBlockStart, - Eigen::Index firstBlockLength, - Eigen::Index secondBlockStart, - Eigen::Index secondBlockLength) + void swapMatrixRows(const Eigen::MatrixBase & matrixIn, + Eigen::Index firstBlockStart, + Eigen::Index firstBlockLength, + Eigen::Index secondBlockStart, + Eigen::Index secondBlockLength) { // Get plain matrix type and cast away constness using Matrix = typename Eigen::MatrixBase::PlainObject; diff --git a/core/include/jiminy/core/utilities/pinocchio.h b/core/include/jiminy/core/utilities/pinocchio.h index e6a69de8a7..bd97eeb74d 100644 --- a/core/include/jiminy/core/utilities/pinocchio.h +++ b/core/include/jiminy/core/utilities/pinocchio.h @@ -10,20 +10,22 @@ namespace jiminy { + JointModelType getJointType(const pinocchio::JointModel & jointModel); + + hresult_t getJointTypeFromIdx( + const pinocchio::Model & model, pinocchio::JointIndex idIn, JointModelType & jointTypeOut); + hresult_t getJointNameFromPositionIdx( const pinocchio::Model & model, pinocchio::JointIndex idIn, std::string & jointNameOut); hresult_t getJointNameFromVelocityIdx( const pinocchio::Model & model, pinocchio::JointIndex idIn, std::string & jointNameOut); - hresult_t getJointTypeFromIdx( - const pinocchio::Model & model, pinocchio::JointIndex idIn, JointModelType & jointTypeOut); - hresult_t getJointTypePositionSuffixes(JointModelType jointTypeIn, - std::vector & jointTypeSuffixesOut); + std::vector & jointTypeSuffixesOut); hresult_t getJointTypeVelocitySuffixes(JointModelType jointTypeIn, - std::vector & jointTypeSuffixesOut); + std::vector & jointTypeSuffixesOut); hresult_t getFrameIdx(const pinocchio::Model & model, const std::string & frameName, diff --git a/core/src/constants.cc b/core/src/constants.cc index 028309e4ae..ef9afc05a4 100644 --- a/core/src/constants.cc +++ b/core/src/constants.cc @@ -1,14 +1,10 @@ +#include + #include "jiminy/core/constants.h" namespace jiminy { - const std::string JOINT_PREFIX_BASE = "current"; - const std::string FREE_FLYER_PREFIX_BASE_NAME = JOINT_PREFIX_BASE + "Freeflyer"; - const std::string FLEXIBLE_JOINT_SUFFIX = "Flexibility"; - - const std::string TELEMETRY_FIELDNAME_DELIMITER = "."; - const std::string TELEMETRY_CONSTANT_DELIMITER = "="; const int64_t TELEMETRY_MIN_BUFFER_SIZE = 256U * 1024U; // 256Ko const uint8_t DELAY_MIN_BUFFER_RESERVE = 20U; diff --git a/core/src/constraints/abstract_constraint.cc b/core/src/constraints/abstract_constraint.cc index 435fa3703f..babc7184d5 100644 --- a/core/src/constraints/abstract_constraint.cc +++ b/core/src/constraints/abstract_constraint.cc @@ -1,5 +1,5 @@ #include "jiminy/core/exceptions.h" -#include "jiminy/core/robot/robot.h" +#include "jiminy/core/robot/model.h" #include "jiminy/core/constraints/abstract_constraint.h" diff --git a/core/src/control/abstract_controller.cc b/core/src/control/abstract_controller.cc index 0c80bd1c2d..7ab24f06b0 100644 --- a/core/src/control/abstract_controller.cc +++ b/core/src/control/abstract_controller.cc @@ -57,8 +57,8 @@ namespace jiminy try { float64_t t = 0.0; - Eigen::VectorXd const q = pinocchio::neutral(robot->pncModel_); - Eigen::VectorXd const v = Eigen::VectorXd::Zero(robot->nv()); + const Eigen::VectorXd q = pinocchio::neutral(robot->pncModel_); + const Eigen::VectorXd v = Eigen::VectorXd::Zero(robot->nv()); Eigen::VectorXd command = Eigen::VectorXd(robot->getMotorsNames().size()); Eigen::VectorXd uCustom = Eigen::VectorXd(robot->nv()); hresult_t returnCode = computeCommand(t, q, v, command); @@ -140,17 +140,18 @@ namespace jiminy { if (telemetryData) { - std::string objectName = CONTROLLER_TELEMETRY_NAMESPACE; + std::string objectName{CONTROLLER_TELEMETRY_NAMESPACE}; if (!objectPrefixName.empty()) { - objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName; + objectName = addCircumfix( + objectName, objectPrefixName, {}, TELEMETRY_FIELDNAME_DELIMITER); } telemetrySender_.configureObject(telemetryData, objectName); for (const auto & [name, valuePtr] : registeredVariables_) { if (returnCode == hresult_t::SUCCESS) { - // TODO Remove explicit `name` capture when moving to C++20 + // FIXME: Remove explicit `name` capture when moving to C++20 std::visit([&, &name = name](auto && arg) { telemetrySender_.registerVariable(name, arg); }, valuePtr); @@ -178,13 +179,13 @@ namespace jiminy return returnCode; } - template + template hresult_t registerVariableImpl( static_map_t> & registeredVariables, bool_t isTelemetryConfigured, const std::vector & fieldnames, - const Eigen::Ref, 0, Eigen::InnerStride<>> & values) + const Eigen::Ref, 0, Eigen::InnerStride<>> & values) { if (isTelemetryConfigured) { diff --git a/core/src/engine/engine_multi_robot.cc b/core/src/engine/engine_multi_robot.cc index 94d03869b6..e6e683f40a 100644 --- a/core/src/engine/engine_multi_robot.cc +++ b/core/src/engine/engine_multi_robot.cc @@ -62,7 +62,7 @@ namespace jiminy isSimulationRunning_(false), engineOptionsHolder_(), timer_(std::make_unique()), - contactModel_(contactModel_t::NONE), + contactModel_(contactModel_t::UNSUPPORTED), telemetrySender_(), telemetryData_(nullptr), telemetryRecorder_(nullptr), @@ -195,15 +195,15 @@ namespace jiminy } // Create and initialize a controller doing nothing - auto bypassFunctor = [](float64_t /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const SensorsDataMap & /* sensorsData */, - Eigen::VectorXd & /* out */) { + auto noopFunctor = +[](float64_t /* t */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const SensorsDataMap & /* sensorsData */, + Eigen::VectorXd & /* out */) { }; auto controller = - std::make_shared>( - bypassFunctor, bypassFunctor); + std::make_shared>( + noopFunctor, noopFunctor); controller->initialize(robot); return addSystem(systemName, robot, controller, std::move(callbackFct)); @@ -418,8 +418,8 @@ namespace jiminy { // Allocate memory float64_t angle{0.0}; - Eigen::Matrix3d rot12, rotJLog12, rotJExp12, rotRef12, omega; - Eigen::Vector3d rotLog12, pos12, posLocal12, fLin, fAng; + Eigen::Matrix3d rot12{}, rotJLog12{}, rotJExp12{}, rotRef12{}, omega{}; + Eigen::Vector3d rotLog12{}, pos12{}, posLocal12{}, fLin{}, fAng{}; auto forceFct = [=](float64_t /*t*/, const Eigen::VectorXd & /*q_1*/, @@ -440,16 +440,16 @@ namespace jiminy } // Get the frames positions and velocities in world - const pinocchio::SE3 & oMf1 = system1->robot->pncData_.oMf[frameIdx1]; - const pinocchio::SE3 & oMf2 = system2->robot->pncData_.oMf[frameIdx2]; - const pinocchio::Motion oVf1 = getFrameVelocity(system1->robot->pncModel_, - system1->robot->pncData_, - frameIdx1, - pinocchio::LOCAL_WORLD_ALIGNED); - const pinocchio::Motion oVf2 = getFrameVelocity(system2->robot->pncModel_, - system2->robot->pncData_, - frameIdx2, - pinocchio::LOCAL_WORLD_ALIGNED); + const pinocchio::SE3 & oMf1{system1->robot->pncData_.oMf[frameIdx1]}; + const pinocchio::SE3 & oMf2{system2->robot->pncData_.oMf[frameIdx2]}; + const pinocchio::Motion oVf1{getFrameVelocity(system1->robot->pncModel_, + system1->robot->pncData_, + frameIdx1, + pinocchio::LOCAL_WORLD_ALIGNED)}; + const pinocchio::Motion oVf2{getFrameVelocity(system2->robot->pncModel_, + system2->robot->pncData_, + frameIdx2, + pinocchio::LOCAL_WORLD_ALIGNED)}; // Compute intermediary quantities rot12.noalias() = oMf1.rotation().transpose() * oMf2.rotation(); @@ -469,14 +469,14 @@ namespace jiminy /* Compute the relative velocity. The application point is the "linear" interpolation between the frames placement with alpha ratio. */ - const pinocchio::Motion oVf12 = oVf2 - oVf1; - pinocchio::Motion velLocal12( + const pinocchio::Motion oVf12{oVf2 - oVf1}; + pinocchio::Motion velLocal12{ rotRef12.transpose() * (oVf12.linear() + pos12.cross(oVf2.angular() - alpha * oVf12.angular())), - rotRef12.transpose() * oVf12.angular()); + rotRef12.transpose() * oVf12.angular()}; // Compute the coupling force acting on frame 2 - pinocchio::Force f; + pinocchio::Force f{}; f.linear() = damping.head<3>().array() * velLocal12.linear().array(); f.angular() = (1.0 - alpha) * f.linear().cross(posLocal12); f.angular().array() += damping.tail<3>().array() * velLocal12.angular().array(); @@ -573,20 +573,20 @@ namespace jiminy } // Get the frames positions and velocities in world - const pinocchio::SE3 & oMf1 = system1->robot->pncData_.oMf[frameIdx1]; - const pinocchio::SE3 & oMf2 = system2->robot->pncData_.oMf[frameIdx2]; - const pinocchio::Motion oVf1 = getFrameVelocity(system1->robot->pncModel_, - system1->robot->pncData_, - frameIdx1, - pinocchio::LOCAL_WORLD_ALIGNED); - const pinocchio::Motion oVf2 = getFrameVelocity(system2->robot->pncModel_, - system2->robot->pncData_, - frameIdx2, - pinocchio::LOCAL_WORLD_ALIGNED); + const pinocchio::SE3 & oMf1{system1->robot->pncData_.oMf[frameIdx1]}; + const pinocchio::SE3 & oMf2{system2->robot->pncData_.oMf[frameIdx2]}; + const pinocchio::Motion oVf1{getFrameVelocity(system1->robot->pncModel_, + system1->robot->pncData_, + frameIdx1, + pinocchio::LOCAL_WORLD_ALIGNED)}; + const pinocchio::Motion oVf2{getFrameVelocity(system2->robot->pncModel_, + system2->robot->pncData_, + frameIdx2, + pinocchio::LOCAL_WORLD_ALIGNED)}; // Compute the linear force coupling them - Eigen::Vector3d dir12 = oMf2.translation() - oMf1.translation(); - const float64_t length = dir12.norm(); + Eigen::Vector3d dir12{oMf2.translation() - oMf1.translation()}; + const float64_t length{dir12.norm()}; auto vel12 = oVf2.linear() - oVf1.linear(); if (length > EPS) { @@ -595,13 +595,9 @@ namespace jiminy return {(stiffness * (length - restLength) + damping * vel12Proj) * dir12, Eigen::Vector3d::Zero()}; } - else - { - /* The direction between frames is ill-defined, so applying - force in the direction of the velocity instead. */ - return {damping * vel12, Eigen::Vector3d::Zero()}; - } - return pinocchio::Force::Zero(); + /* The direction between frames is ill-defined, so applying force in the direction + of the linear velocity instead. */ + return {damping * vel12, Eigen::Vector3d::Zero()}; }; returnCode = @@ -755,35 +751,35 @@ namespace jiminy systemDataIt->logFieldnamesPosition = addCircumfix(systemIt->robot->getLogFieldnamesPosition(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnamesVelocity = addCircumfix(systemIt->robot->getLogFieldnamesVelocity(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnamesAcceleration = addCircumfix(systemIt->robot->getLogFieldnamesAcceleration(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnamesForceExternal = addCircumfix(systemIt->robot->getLogFieldnamesForceExternal(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnamesCommand = addCircumfix(systemIt->robot->getCommandFieldnames(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnamesMotorEffort = addCircumfix(systemIt->robot->getMotorEffortFieldnames(), systemIt->name, - "", + {}, TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->logFieldnameEnergy = - addCircumfix("energy", systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER); + addCircumfix("energy", systemIt->name, {}, TELEMETRY_FIELDNAME_DELIMITER); // Register variables to the telemetry senders if (returnCode == hresult_t::SUCCESS) @@ -1213,9 +1209,9 @@ namespace jiminy for (auto & system : systems_) { - for (const auto & sensorGroup : system.robot->getSensors()) + for (const auto & sensorsGroupItem : system.robot->getSensors()) { - for (const auto & sensor : sensorGroup.second) + for (const auto & sensor : sensorsGroupItem.second) { if (!sensor->getIsInitialized()) { @@ -1507,7 +1503,7 @@ namespace jiminy engineOptions_->stepper.tolRel, PGS_MAX_ITERATIONS); break; - case constraintSolver_t::NONE: + case constraintSolver_t::UNSUPPORTED: default: break; } @@ -1627,19 +1623,19 @@ namespace jiminy { // Backup URDF file const std::string telemetryUrdfFile = - addCircumfix("urdf_file", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + addCircumfix("urdf_file", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); const std::string & urdfFileString = system.robot->getUrdfAsString(); telemetrySender_.registerConstant(telemetryUrdfFile, urdfFileString); // Backup 'has_freeflyer' option const std::string telemetrHasFreeflyer = - addCircumfix("has_freeflyer", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + addCircumfix("has_freeflyer", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); telemetrySender_.registerConstant(telemetrHasFreeflyer, toString(system.robot->getHasFreeflyer())); // Backup mesh package lookup directories const std::string telemetryMeshPackageDirs = addCircumfix( - "mesh_package_dirs", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + "mesh_package_dirs", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); std::string meshPackageDirsString; std::stringstream meshPackageDirsStream; const std::vector & meshPackageDirs = @@ -1657,7 +1653,7 @@ namespace jiminy // Backup the true and theoretical Pinocchio::Model std::string key = addCircumfix( - "pinocchio_model", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + "pinocchio_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); std::string value = saveToBinary(system.robot->pncModel_); telemetrySender_.registerConstant(key, value); @@ -1670,18 +1666,18 @@ namespace jiminy try { key = addCircumfix( - "collision_model", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + "collision_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); value = saveToBinary(system.robot->collisionModel_); telemetrySender_.registerConstant(key, value); key = addCircumfix( - "visual_model", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + "visual_model", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); value = saveToBinary(system.robot->visualModel_); telemetrySender_.registerConstant(key, value); } catch (const std::exception & e) { - std::string msg = "Failed to log the collision and/or visual model."; + std::string msg{"Failed to log the collision and/or visual model."}; if (urdfFileString.empty()) { msg += " It will be impossible to replay log files because no URDF " @@ -1698,7 +1694,7 @@ namespace jiminy for (const auto & system : systems_) { const std::string telemetryRobotOptions = - addCircumfix("system", system.name, "", TELEMETRY_FIELDNAME_DELIMITER); + addCircumfix("system", system.name, {}, TELEMETRY_FIELDNAME_DELIMITER); GenericConfig systemOptions; systemOptions["robot"] = system.robot->getOptions(); systemOptions["controller"] = system.controller->getOptions(); @@ -4264,7 +4260,7 @@ namespace jiminy static_cast(&logData)); // Extract the times - const H5::DataSet globalTimeDataSet = file->openDataSet(GLOBAL_TIME); + const H5::DataSet globalTimeDataSet = file->openDataSet(std::string{GLOBAL_TIME}); const H5::DataSpace timeSpace = globalTimeDataSet.getSpace(); const hssize_t numData = timeSpace.getSimpleExtentNpoints(); logData.times.resize(numData); @@ -4309,7 +4305,7 @@ namespace jiminy Eigen::Matrix intVector(numData); Eigen::Matrix floatVector(numData); logData.variableNames.reserve(1 + numInt + numFloat); - logData.variableNames.push_back(GLOBAL_TIME); + logData.variableNames.emplace_back(GLOBAL_TIME); // Read all variables while preserving ordering using opDataT = std::tupletimes.size())}; const H5::DataSpace globalTimeSpace = H5::DataSpace(1, timeDims); - const H5::DataSet globalTimeDataSet = - file->createDataSet(GLOBAL_TIME, H5::PredType::NATIVE_INT64, globalTimeSpace); + const H5::DataSet globalTimeDataSet = file->createDataSet( + std::string{GLOBAL_TIME}, H5::PredType::NATIVE_INT64, globalTimeSpace); globalTimeDataSet.write(logData->times.data(), H5::PredType::NATIVE_INT64); // Add "unit" attribute to GLOBAL_TIME vector @@ -4462,7 +4458,7 @@ namespace jiminy plist.setDeflate(4); // Create time dataset using symbolic link - fieldGroup.link(H5L_TYPE_HARD, "/" + GLOBAL_TIME, "time"); + fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); // Create variable dataset H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); @@ -4490,7 +4486,7 @@ namespace jiminy plist.setDeflate(4); // Create time dataset using symbolic link - fieldGroup.link(H5L_TYPE_HARD, "/" + GLOBAL_TIME, "time"); + fieldGroup.link(H5L_TYPE_HARD, toString("/", GLOBAL_TIME), "time"); // Create variable dataset H5::DataSpace valueSpace = H5::DataSpace(1, timeDims); diff --git a/core/src/hardware/abstract_sensor.cc b/core/src/hardware/abstract_sensor.cc index a5c513c998..989d0d5176 100644 --- a/core/src/hardware/abstract_sensor.cc +++ b/core/src/hardware/abstract_sensor.cc @@ -41,7 +41,8 @@ namespace jiminy std::string objectName = getTelemetryName(); if (!objectPrefixName.empty()) { - objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName; + objectName = addCircumfix( + objectName, objectPrefixName, {}, TELEMETRY_FIELDNAME_DELIMITER); } telemetrySender_.configureObject(telemetryData, objectName); returnCode = telemetrySender_.registerVariable(getFieldnames(), get()); diff --git a/core/src/hardware/basic_sensors.cc b/core/src/hardware/basic_sensors.cc index cd373a325d..fb08fd15cc 100644 --- a/core/src/hardware/basic_sensors.cc +++ b/core/src/hardware/basic_sensors.cc @@ -66,12 +66,12 @@ namespace jiminy // ===================== ImuSensor ========================= template<> - const std::string AbstractSensorTpl::type_("ImuSensor"); + const std::string AbstractSensorTpl::type_{"ImuSensor"}; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_(false); + const std::vector AbstractSensorTpl::fieldnames_{ + "Gyrox", "Gyroy", "Gyroz", "Accelx", "Accely", "Accelz"}; template<> - const std::vector AbstractSensorTpl::fieldnames_( - {"Gyrox", "Gyroy", "Gyroz", "Accelx", "Accely", "Accelz"}); + const bool_t AbstractSensorTpl::areFieldnamesGrouped_{false}; ImuSensor::ImuSensor(const std::string & name) : AbstractSensorTpl(name), @@ -219,12 +219,11 @@ namespace jiminy // ===================== ContactSensor ========================= template<> - const std::string AbstractSensorTpl::type_("ContactSensor"); + const std::string AbstractSensorTpl::type_{"ContactSensor"}; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_(false); + const std::vector AbstractSensorTpl::fieldnames_{"FX", "FY", "FZ"}; template<> - const std::vector - AbstractSensorTpl::fieldnames_({"FX", "FY", "FZ"}); + const bool_t AbstractSensorTpl::areFieldnamesGrouped_{false}; ContactSensor::ContactSensor(const std::string & name) : AbstractSensorTpl(name), @@ -306,12 +305,12 @@ namespace jiminy // ===================== ForceSensor ========================= template<> - const std::string AbstractSensorTpl::type_("ForceSensor"); + const std::string AbstractSensorTpl::type_{"ForceSensor"}; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_(false); + const std::vector AbstractSensorTpl::fieldnames_{ + "FX", "FY", "FZ", "MX", "MY", "MZ"}; template<> - const std::vector - AbstractSensorTpl::fieldnames_({"FX", "FY", "FZ", "MX", "MY", "MZ"}); + const bool_t AbstractSensorTpl::areFieldnamesGrouped_{false}; ForceSensor::ForceSensor(const std::string & name) : AbstractSensorTpl(name), @@ -400,11 +399,11 @@ namespace jiminy // ===================== EncoderSensor ========================= template<> - const std::string AbstractSensorTpl::type_("EncoderSensor"); + const std::string AbstractSensorTpl::type_{"EncoderSensor"}; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_(true); + const std::vector AbstractSensorTpl::fieldnames_{"Q", "V"}; template<> - const std::vector AbstractSensorTpl::fieldnames_({"Q", "V"}); + const bool_t AbstractSensorTpl::areFieldnamesGrouped_{true}; EncoderSensor::EncoderSensor(const std::string & name) : AbstractSensorTpl(name), @@ -509,11 +508,11 @@ namespace jiminy // ===================== EffortSensor ========================= template<> - const std::string AbstractSensorTpl::type_("EffortSensor"); + const std::string AbstractSensorTpl::type_{"EffortSensor"}; template<> - const bool_t AbstractSensorTpl::areFieldnamesGrouped_(true); + const std::vector AbstractSensorTpl::fieldnames_{"U"}; template<> - const std::vector AbstractSensorTpl::fieldnames_({"U"}); + const bool_t AbstractSensorTpl::areFieldnamesGrouped_{true}; EffortSensor::EffortSensor(const std::string & name) : AbstractSensorTpl(name), diff --git a/core/src/io/abstract_io_device.cc b/core/src/io/abstract_io_device.cc index b68f94ba47..7d32b4c46f 100644 --- a/core/src/io/abstract_io_device.cc +++ b/core/src/io/abstract_io_device.cc @@ -240,10 +240,10 @@ namespace jiminy // Specific implementation - std::string template<> - hresult_t AbstractIODevice::write(const std::string & str) + hresult_t AbstractIODevice::write(const std::string_view & str) { int64_t toWrite = static_cast(str.size()); - const uint8_t * bufferPos = reinterpret_cast(str.c_str()); + const uint8_t * bufferPos = reinterpret_cast(str.data()); return write(bufferPos, toWrite); } diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index 83fa46360c..260be180bc 100644 --- a/core/src/robot/model.cc +++ b/core/src/robot/model.cc @@ -362,7 +362,7 @@ namespace jiminy { urdfPath_ = urdfPath; std::ifstream urdfFileStream(urdfPath_); - urdfData_ = std::string((std::istreambuf_iterator(urdfFileStream)), + urdfData_ = std::string(std::istreambuf_iterator(urdfFileStream), std::istreambuf_iterator()); meshPackageDirs_ = meshPackageDirs; } @@ -1302,97 +1302,65 @@ namespace jiminy /* Generate the fieldnames associated with the configuration vector, velocity, acceleration and external force vectors. */ logFieldnamesPosition_.clear(); - logFieldnamesPosition_.resize(static_cast(nq_)); + logFieldnamesPosition_.reserve(static_cast(nq_)); logFieldnamesVelocity_.clear(); - logFieldnamesVelocity_.resize(static_cast(nv_)); + logFieldnamesVelocity_.reserve(static_cast(nv_)); logFieldnamesAcceleration_.clear(); - logFieldnamesAcceleration_.resize(static_cast(nv_)); + logFieldnamesAcceleration_.reserve(static_cast(nv_)); logFieldnamesForceExternal_.clear(); - logFieldnamesForceExternal_.resize(6U * (pncModel_.njoints - 1)); + logFieldnamesForceExternal_.reserve(6U * (pncModel_.njoints - 1)); for (std::size_t i = 1; i < pncModel_.joints.size(); ++i) { // Get joint name without "Joint" suffix, if any - std::string jointShortName = removeSuffix(pncModel_.names[i], "Joint"); - - // Get joint postion and velocity starting indices - const int32_t idx_q = pncModel_.joints[i].idx_q(); - const int32_t idx_v = pncModel_.joints[i].idx_v(); + std::string jointShortName{removeSuffix(pncModel_.names[i], "Joint")}; // Get joint prefix depending on its type - JointModelType jointType; - std::string jointPrefix; - if (returnCode == hresult_t::SUCCESS) - { - returnCode = getJointTypeFromIdx(pncModel_, i, jointType); - } - if (returnCode == hresult_t::SUCCESS) + const JointModelType jointType{getJointType(pncModel_.joints[i])}; + std::string jointPrefix{JOINT_PREFIX_BASE}; + if (jointType == JointModelType::FREE) { - if (jointType == JointModelType::FREE) - { - // Discard the joint name for FREE joint type since it is unique if any - jointPrefix = FREE_FLYER_PREFIX_BASE_NAME; - jointShortName = ""; - } - else - { - jointPrefix = JOINT_PREFIX_BASE; - } + jointPrefix += FREE_FLYER_NAME; + jointShortName = ""; } - // Get joint position and velocity suffices depending on its type - std::vector jointTypePositionSuffixes; - std::vector jointTypeVelocitySuffixes; + // Get joint position suffices depending on its type + std::vector jointTypePositionSuffixes{}; + std::vector jointTypeVelocitySuffixes{}; if (returnCode == hresult_t::SUCCESS) { returnCode = getJointTypePositionSuffixes(jointType, jointTypePositionSuffixes); } - if (returnCode == hresult_t::SUCCESS) - { - returnCode = - getJointTypeVelocitySuffixes(jointType, jointTypeVelocitySuffixes); - } if (returnCode == hresult_t::SUCCESS) { - // Define complete position fieldnames and backup them - std::vector jointPositionFieldnames; - for (const std::string & suffix : jointTypePositionSuffixes) + // Get joint velocity suffices depending on its type + getJointTypeVelocitySuffixes( + jointType, jointTypeVelocitySuffixes); // Cannot fail at this point + + // Define complete position fieldnames + for (const std::string_view & suffix : jointTypePositionSuffixes) { - jointPositionFieldnames.emplace_back( // - jointPrefix + "Position" + jointShortName + suffix); + logFieldnamesPosition_.emplace_back( + toString(jointPrefix, "Position", jointShortName, suffix)); } - std::copy(jointPositionFieldnames.begin(), - jointPositionFieldnames.end(), - logFieldnamesPosition_.begin() + idx_q); - - // Define complete velocity and acceleration fieldnames and backup them - std::vector jointVelocityFieldnames; - std::vector jointAccelerationFieldnames; - for (const std::string & suffix : jointTypeVelocitySuffixes) + + // Define complete velocity and acceleration fieldnames + for (const std::string_view & suffix : jointTypeVelocitySuffixes) { - jointVelocityFieldnames.emplace_back( // - jointPrefix + "Velocity" + jointShortName + suffix); - jointAccelerationFieldnames.emplace_back( // - jointPrefix + "Acceleration" + jointShortName + suffix); + logFieldnamesVelocity_.emplace_back( + toString(jointPrefix, "Velocity", jointShortName, suffix)); + logFieldnamesAcceleration_.emplace_back( + toString(jointPrefix, "Acceleration", jointShortName, suffix)); } - std::copy(jointVelocityFieldnames.begin(), - jointVelocityFieldnames.end(), - logFieldnamesVelocity_.begin() + idx_v); - std::copy(jointAccelerationFieldnames.begin(), - jointAccelerationFieldnames.end(), - logFieldnamesAcceleration_.begin() + idx_v); // Define complete external force fieldnames and backup them std::vector jointForceExternalFieldnames; for (const std::string & suffix : ForceSensor::fieldnames_) { - jointForceExternalFieldnames.emplace_back( // - jointPrefix + "ForceExternal" + jointShortName + suffix); + logFieldnamesForceExternal_.emplace_back( + toString(jointPrefix, "ForceExternal", jointShortName, suffix)); } - std::copy(jointForceExternalFieldnames.begin(), - jointForceExternalFieldnames.end(), - logFieldnamesForceExternal_.begin() + 6U * (i - 1)); } } } @@ -1427,29 +1395,29 @@ namespace jiminy /* Overwrite the position bounds for some specific joint type, mainly due to quaternion normalization and cos/sin representation. */ - for (int32_t i = 0; i < pncModel_.njoints; ++i) + for (const auto & joint : pncModel_.joints) { - JointModelType jointType(JointModelType::UNSUPPORTED); - getJointTypeFromIdx(pncModel_, i, jointType); - - if (jointType == JointModelType::SPHERICAL) + uint32_t positionIdx, positionNq; + switch (getJointType(joint)) { - uint32_t positionIdx = pncModel_.joints[i].idx_q(); - positionLimitMin_.segment<4>(positionIdx).setConstant(-1.0 - EPS); - positionLimitMax_.segment<4>(positionIdx).setConstant(+1.0 + EPS); - } - if (jointType == JointModelType::FREE) - { - uint32_t positionIdx = pncModel_.joints[i].idx_q(); - positionLimitMin_.segment<4>(positionIdx + 3).setConstant(-1.0 - EPS); - positionLimitMax_.segment<4>(positionIdx + 3).setConstant(+1.0 + EPS); - } - if (jointType == JointModelType::ROTARY_UNBOUNDED) - { - uint32_t positionIdx = pncModel_.joints[i].idx_q(); - positionLimitMin_.segment<2>(positionIdx).setConstant(-1.0 - EPS); - positionLimitMax_.segment<2>(positionIdx).setConstant(+1.0 + EPS); + case JointModelType::ROTARY_UNBOUNDED: + case JointModelType::SPHERICAL: + positionIdx = joint.idx_q(); + positionNq = joint.nq(); + break; + case JointModelType::FREE: + positionIdx = joint.idx_q() + 3; + positionNq = 4; + case JointModelType::UNSUPPORTED: + case JointModelType::LINEAR: + case JointModelType::ROTARY: + case JointModelType::PLANAR: + case JointModelType::TRANSLATION: + default: + continue; } + positionLimitMin_.segment(positionIdx, positionNq).setConstant(-1.0 - EPS); + positionLimitMax_.segment(positionIdx, positionNq).setConstant(+1.0 + EPS); } // Get the joint velocity limits from the URDF or the user options diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 70909b46bb..52465b6ccf 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -3,7 +3,6 @@ #include "jiminy/core/exceptions.h" #include "jiminy/core/io/file_device.h" -#include "jiminy/core/telemetry/telemetry_data.h" #include "jiminy/core/hardware/abstract_motor.h" #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/utilities/helpers.h" @@ -36,8 +35,8 @@ namespace jiminy Robot::~Robot() { // Detach all the motors and sensors - detachSensors({}); - detachMotors({}); + detachSensors(); + detachMotors(); } hresult_t Robot::initialize(const std::string & urdfPath, @@ -46,8 +45,8 @@ namespace jiminy bool_t loadVisualMeshes) { // Detach all the motors and sensors - detachSensors({}); - detachMotors({}); + detachSensors(); + detachMotors(); /* Delete the current model and generate a new one. Note that is also refresh all proxies automatically. */ @@ -59,8 +58,8 @@ namespace jiminy const pinocchio::GeometryModel & visualModel) { // Detach all the motors and sensors - detachSensors({}); - detachMotors({}); + detachSensors(); + detachMotors(); /* Delete the current model and generate a new one. Note that is also refresh all proxies automatically. */ @@ -79,11 +78,11 @@ namespace jiminy } // Reset the sensors - for (auto & sensorGroup : sensorsGroupHolder_) + for (auto & sensorsGroupItem : sensorsGroupHolder_) { - if (!sensorGroup.second.empty()) + if (!sensorsGroupItem.second.empty()) { - (*sensorGroup.second.begin())->resetAll(); + (*sensorsGroupItem.second.begin())->resetAll(); } } @@ -112,13 +111,13 @@ namespace jiminy { if (!isTelemetryConfigured_) { - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & [sensorType, sensorsGroup] : sensorsGroupHolder_) { - for (const auto & sensor : sensorGroup.second) + for (const auto & sensor : sensorsGroup) { if (returnCode == hresult_t::SUCCESS) { - if (sensorTelemetryOptions_[sensorGroup.first]) + if (sensorTelemetryOptions_[sensorType]) { returnCode = sensor->configureTelemetry(telemetryData_, objectPrefixName); @@ -260,7 +259,8 @@ namespace jiminy return hresult_t::SUCCESS; } - hresult_t Robot::detachMotors(const std::vector & motorsNames) + template + hresult_t detachMotorsImpl(Robot & robot, const std::vector & motorsNames) { hresult_t returnCode = hresult_t::SUCCESS; @@ -273,31 +273,22 @@ namespace jiminy returnCode = hresult_t::ERROR_BAD_INPUT; } + // Make sure that every motor name exist if (returnCode == hresult_t::SUCCESS) { - // Make sure that every motor name exist - if (!checkInclusion(motorsNames_, motorsNames)) + if (!checkInclusion(robot.getMotorsNames(), motorsNames)) { PRINT_ERROR("At least one of the motor names does not exist."); returnCode = hresult_t::ERROR_BAD_INPUT; } } - for (const std::string & name : motorsNames) + // Remove motors one-by-one + for (const T & name : motorsNames) { if (returnCode == hresult_t::SUCCESS) { - returnCode = detachMotor(name); - } - } - } - else - { - if (returnCode == hresult_t::SUCCESS) - { - if (!motorsNames_.empty()) - { - returnCode = detachMotors(std::vector(motorsNames_)); + returnCode = robot.detachMotor(name); } } } @@ -305,6 +296,16 @@ namespace jiminy return returnCode; } + hresult_t Robot::detachMotors(const std::vector & motorsNames) + { + // Remove all sensors if none is specified + if (motorsNames.empty()) + { + return detachMotorsImpl(*this, motorsNames_); + } + return detachMotorsImpl(*this, motorsNames); + } + hresult_t Robot::attachSensor(std::shared_ptr sensor) { // The sensors' names must be unique, even if their type is different. @@ -352,10 +353,10 @@ namespace jiminy // Create a new sensor data holder if necessary if (sensorGroupIt == sensorsGroupHolder_.end()) { - sensorsSharedHolder_.emplace( - std::make_pair(sensorType, std::make_shared())); - sensorTelemetryOptions_.emplace( - std::make_pair(sensorType, true)); // Enable the telemetry by default + sensorsSharedHolder_.emplace(sensorType, + std::make_shared()); + sensorTelemetryOptions_.emplace(sensorType, + true); // Enable the telemetry by default } // Attach the sensor @@ -390,7 +391,8 @@ namespace jiminy return hresult_t::ERROR_INIT_FAILED; } - auto sensorGroupIt = sensorsGroupHolder_.find(sensorType); + // FIXME: remove explicit conversion to `std::string` when moving to C++20 + auto sensorGroupIt = sensorsGroupHolder_.find(std::string{sensorType}); if (sensorGroupIt == sensorsGroupHolder_.end()) { PRINT_ERROR("This type of sensor does not exist."); @@ -525,22 +527,22 @@ namespace jiminy // Generate the fieldnames associated with command logFieldnamesCommand_.clear(); logFieldnamesCommand_.reserve(nmotors_); - std::transform(motorsHolder_.begin(), - motorsHolder_.end(), - std::back_inserter(logFieldnamesCommand_), - [](const auto & elem) -> std::string { - return addCircumfix(elem->getName(), JOINT_PREFIX_BASE + "Command"); - }); + std::transform( + motorsHolder_.begin(), + motorsHolder_.end(), + std::back_inserter(logFieldnamesCommand_), + [](const auto & elem) -> std::string + { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Command")); }); // Generate the fieldnames associated with motor efforts logFieldnamesMotorEffort_.clear(); logFieldnamesMotorEffort_.reserve(nmotors_); - std::transform(motorsHolder_.begin(), - motorsHolder_.end(), - std::back_inserter(logFieldnamesMotorEffort_), - [](const auto & elem) -> std::string { - return addCircumfix(elem->getName(), JOINT_PREFIX_BASE + "Effort"); - }); + std::transform( + motorsHolder_.begin(), + motorsHolder_.end(), + std::back_inserter(logFieldnamesMotorEffort_), + [](const auto & elem) -> std::string + { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Effort")); }); } return returnCode; @@ -561,15 +563,15 @@ namespace jiminy // Extract the motor names sensorsNames_.clear(); sensorsNames_.reserve(sensorsGroupHolder_.size()); - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & [sensorType, sensorsGroup] : sensorsGroupHolder_) { std::vector sensorGroupNames; - sensorGroupNames.reserve(sensorGroup.second.size()); - std::transform(sensorGroup.second.begin(), - sensorGroup.second.end(), + sensorGroupNames.reserve(sensorsGroup.size()); + std::transform(sensorsGroup.begin(), + sensorsGroup.end(), std::back_inserter(sensorGroupNames), [](const auto & elem) -> std::string { return elem->getName(); }); - sensorsNames_.insert({sensorGroup.first, std::move(sensorGroupNames)}); + sensorsNames_.emplace(sensorType, std::move(sensorGroupNames)); } } @@ -979,19 +981,18 @@ namespace jiminy returnCode = hresult_t::ERROR_GENERIC; } - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & [sensorType, sensorsGroup] : sensorsGroupHolder_) { if (returnCode == hresult_t::SUCCESS) { - const std::string & sensorType = sensorGroup.first; - - auto sensorGroupOptionsIt = sensorsOptions.find(sensorType); + // FIXME: remove explicit conversion to `std::string` when moving to C++20 + auto sensorGroupOptionsIt = sensorsOptions.find(std::string{sensorType}); if (sensorGroupOptionsIt != sensorsOptions.end()) { const GenericConfig & sensorGroupOptions = boost::get(sensorGroupOptionsIt->second); - for (const auto & sensor : sensorGroup.second) + for (const auto & sensor : sensorsGroup) { if (returnCode == hresult_t::SUCCESS) { @@ -1071,14 +1072,14 @@ namespace jiminy GenericConfig Robot::getSensorsOptions() const { GenericConfig sensorsOptions; - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & [sensorType, sensorsGroup] : sensorsGroupHolder_) { GenericConfig sensorsGroupOptions; - for (const auto & sensor : sensorGroup.second) + for (const auto & sensor : sensorsGroup) { sensorsGroupOptions[sensor->getName()] = sensor->getOptions(); } - sensorsOptions[sensorGroup.first] = sensorsGroupOptions; + sensorsOptions[sensorType] = sensorsGroupOptions; } return sensorsOptions; } @@ -1102,17 +1103,16 @@ namespace jiminy return hresult_t::ERROR_GENERIC; } - for (auto & sensorGroupTelemetryOption : sensorTelemetryOptions_) + for (auto & [sensorType, sensorGroupTelemetryOption] : sensorTelemetryOptions_) { - std::string optionTelemetryName = "enable" + sensorGroupTelemetryOption.first + "s"; + const std::string optionTelemetryName = toString("enable", sensorType, "s"); auto sensorTelemetryOptionIt = telemetryOptions.find(optionTelemetryName); if (sensorTelemetryOptionIt == telemetryOptions.end()) { PRINT_ERROR("Missing field."); return hresult_t::ERROR_GENERIC; } - sensorGroupTelemetryOption.second = - boost::get(sensorTelemetryOptionIt->second); + sensorGroupTelemetryOption = boost::get(sensorTelemetryOptionIt->second); } return hresult_t::SUCCESS; @@ -1121,10 +1121,10 @@ namespace jiminy GenericConfig Robot::getTelemetryOptions() const { GenericConfig telemetryOptions; - for (const auto & sensorGroupTelemetryOption : sensorTelemetryOptions_) + for (const auto & [sensorType, sensorGroupTelemetryOption] : sensorTelemetryOptions_) { - std::string optionTelemetryName = "enable" + sensorGroupTelemetryOption.first + "s"; - telemetryOptions[optionTelemetryName] = sensorGroupTelemetryOption.second; + const std::string optionTelemetryName = toString("enable", sensorType, "s"); + telemetryOptions[optionTelemetryName] = sensorGroupTelemetryOption; } return telemetryOptions; } @@ -1208,11 +1208,11 @@ namespace jiminy one is supposed to call `pinocchio::forwardKinematics` and `pinocchio::updateFramePlacements` before calling this method. */ - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & sensorsGroupItem : sensorsGroupHolder_) { - if (!sensorGroup.second.empty()) + if (!sensorsGroupItem.second.empty()) { - (*sensorGroup.second.begin())->setAll(t, q, v, a, uMotor, fExternal); + (*sensorsGroupItem.second.begin())->setAll(t, q, v, a, uMotor, fExternal); } } } @@ -1224,13 +1224,14 @@ namespace jiminy sensorsSharedHolder_t::const_iterator sensorsSharedIt = sensorsSharedHolder_.begin(); for (; sensorsGroupIt != sensorsGroupHolder_.end(); ++sensorsGroupIt, ++sensorsSharedIt) { + auto & [sensorType, sensorsGroup] = *sensorsGroupIt; SensorDataTypeMap dataType(&sensorsSharedIt->second->dataMeasured_); - for (auto & sensor : sensorsGroupIt->second) + for (auto & sensor : sensorsGroup) { auto & sensorConst = const_cast(*sensor); dataType.insert({sensorConst.getName(), sensorConst.getIdx(), sensorConst.get()}); } - data.emplace(sensorsGroupIt->first, std::move(dataType)); + data.emplace(sensorType, std::move(dataType)); } return data; } @@ -1259,11 +1260,11 @@ namespace jiminy void Robot::updateTelemetry() { - for (const auto & sensorGroup : sensorsGroupHolder_) + for (const auto & sensorsGroupItem : sensorsGroupHolder_) { - if (!sensorGroup.second.empty()) + if (!sensorsGroupItem.second.empty()) { - (*sensorGroup.second.begin())->updateTelemetryAll(); + (*sensorsGroupItem.second.begin())->updateTelemetryAll(); } } } diff --git a/core/src/telemetry/telemetry_data.cc b/core/src/telemetry/telemetry_data.cc index cf6c59a4f9..8b200881fd 100644 --- a/core/src/telemetry/telemetry_data.cc +++ b/core/src/telemetry/telemetry_data.cc @@ -23,8 +23,8 @@ namespace jiminy isRegisteringAvailable_ = true; } - hresult_t TelemetryData::registerConstant(const std::string & variableNameIn, - const std::string & constantValueIn) + hresult_t TelemetryData::registerConstant(const std::string & name, + const std::string & value) { // Check if registration is possible if (!isRegisteringAvailable_) @@ -34,11 +34,11 @@ namespace jiminy } // Check if already in memory - auto variableIt = std::find_if( - constantsRegistry_.begin(), - constantsRegistry_.end(), - [&variableNameIn](const std::pair & element) -> bool_t - { return element.first == variableNameIn; }); + auto variableIt = + std::find_if(constantsRegistry_.begin(), + constantsRegistry_.end(), + [&name](const std::pair & element) -> bool_t + { return element.first == name; }); if (variableIt != constantsRegistry_.end()) { PRINT_ERROR("Entry already exists."); @@ -46,7 +46,7 @@ namespace jiminy } // Register new constant - constantsRegistry_.emplace_back(variableNameIn, constantValueIn); + constantsRegistry_.emplace_back(name, value); return hresult_t::SUCCESS; } diff --git a/core/src/telemetry/telemetry_recorder.cc b/core/src/telemetry/telemetry_recorder.cc index 8b295651ee..e6aba2b962 100644 --- a/core/src/telemetry/telemetry_recorder.cc +++ b/core/src/telemetry/telemetry_recorder.cc @@ -35,7 +35,8 @@ namespace jiminy std::ostringstream timeUnitStr; int precision = -static_cast(std::ceil(std::log10(STEPPER_MIN_TIMESTEP))); timeUnitStr << std::scientific << std::setprecision(precision) << timeUnit; - telemetryData->registerConstant(TIME_UNIT, timeUnitStr.str()); + // FIXME: remove explicit conversion to `std::string` when moving to C++20 + telemetryData->registerConstant(std::string{TIME_UNIT}, timeUnitStr.str()); std::vector header; if (returnCode == hresult_t::SUCCESS) diff --git a/core/src/telemetry/telemetry_sender.cc b/core/src/telemetry/telemetry_sender.cc index 3dba5fa852..9ace19b48f 100644 --- a/core/src/telemetry/telemetry_sender.cc +++ b/core/src/telemetry/telemetry_sender.cc @@ -11,18 +11,18 @@ namespace jiminy } void TelemetrySender::configureObject(std::shared_ptr telemetryDataInstance, - const std::string & objectName) + const std::string_view & objectName) { objectName_ = objectName; telemetryData_ = telemetryDataInstance; bufferPosition_.clear(); } - hresult_t TelemetrySender::registerConstant(const std::string & variableName, + hresult_t TelemetrySender::registerConstant(const std::string & name, const std::string & value) { const std::string fullFieldName = - objectName_ + TELEMETRY_FIELDNAME_DELIMITER + variableName; + addCircumfix(name, objectName_, {}, TELEMETRY_FIELDNAME_DELIMITER); return telemetryData_->registerConstant(fullFieldName, value); } diff --git a/core/src/utilities/helpers.cc b/core/src/utilities/helpers.cc index 10be249131..c696437a9d 100644 --- a/core/src/utilities/helpers.cc +++ b/core/src/utilities/helpers.cc @@ -9,7 +9,7 @@ #include "jiminy/core/constants.h" #include "jiminy/core/exceptions.h" -#include "jiminy/core/telemetry/telemetry_data.h" // `Global.Time` +#include "jiminy/core/telemetry/telemetry_data.h" // `GLOBAL_TIME` #include "jiminy/core/telemetry/telemetry_recorder.h" // `LogData` #include "jiminy/core/utilities/helpers.h" @@ -94,25 +94,25 @@ namespace jiminy } std::string addCircumfix(std::string fieldname, - const std::string & prefix, - const std::string & suffix, - const std::string & delimiter) + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter) { if (!prefix.empty()) { - fieldname = prefix + delimiter + fieldname; + fieldname = toString(prefix, delimiter, fieldname); } if (!suffix.empty()) { - fieldname = fieldname + delimiter + suffix; + fieldname = toString(fieldname, delimiter, suffix); } return fieldname; } std::vector addCircumfix(const std::vector & fieldnamesIn, - const std::string & prefix, - const std::string & suffix, - const std::string & delimiter) + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter) { std::vector fieldnames; fieldnames.reserve(fieldnamesIn.size()); diff --git a/core/src/utilities/pinocchio.cc b/core/src/utilities/pinocchio.cc index 9c5a96886f..eceb22f549 100644 --- a/core/src/utilities/pinocchio.cc +++ b/core/src/utilities/pinocchio.cc @@ -148,62 +148,58 @@ namespace jiminy } }; + JointModelType getJointType(const pinocchio::JointModel & jointModel) + { + JointModelType jointTypeOut{JointModelType::UNSUPPORTED}; + getJointTypeAlgo::run(jointModel, typename getJointTypeAlgo::ArgsType(jointTypeOut)); + return jointTypeOut; + } + hresult_t getJointTypeFromIdx( const pinocchio::Model & model, pinocchio::JointIndex idIn, JointModelType & jointTypeOut) { if (model.njoints < static_cast(idIn) - 1) { + jointTypeOut = JointModelType::UNSUPPORTED; PRINT_ERROR("Joint index '", idIn, "' is out of range."); return hresult_t::ERROR_GENERIC; } - getJointTypeAlgo::run(model.joints[idIn], - typename getJointTypeAlgo::ArgsType(jointTypeOut)); + jointTypeOut = getJointType(model.joints[idIn]); return hresult_t::SUCCESS; } hresult_t getJointTypePositionSuffixes(JointModelType jointTypeIn, - std::vector & jointTypeSuffixesOut) + std::vector & jointTypeSuffixesOut) { // If no extra discrimination is needed - jointTypeSuffixesOut = std::vector({std::string("")}); switch (jointTypeIn) { case JointModelType::LINEAR: - break; case JointModelType::ROTARY: + jointTypeSuffixesOut = {""}; break; case JointModelType::ROTARY_UNBOUNDED: - jointTypeSuffixesOut = - std::vector({std::string("Cos"), std::string("Sin")}); + jointTypeSuffixesOut = {"Cos", "Sin"}; break; case JointModelType::PLANAR: - jointTypeSuffixesOut = - std::vector({std::string("TransX"), std::string("TransY")}); + jointTypeSuffixesOut = {"TransX", "TransY"}; break; case JointModelType::TRANSLATION: - jointTypeSuffixesOut = std::vector( - {std::string("TransX"), std::string("TransY"), std::string("TransZ")}); + jointTypeSuffixesOut = {"TransX", "TransY", "TransZ"}; break; case JointModelType::SPHERICAL: - jointTypeSuffixesOut = std::vector({std::string("QuatX"), - std::string("QuatY"), - std::string("QuatZ"), - std::string("QuatW")}); + jointTypeSuffixesOut = {"QuatX", "QuatY", "QuatZ", "QuatW"}; break; case JointModelType::FREE: - jointTypeSuffixesOut = std::vector({std::string("TransX"), - std::string("TransY"), - std::string("TransZ"), - std::string("QuatX"), - std::string("QuatY"), - std::string("QuatZ"), - std::string("QuatW")}); + jointTypeSuffixesOut = { + "TransX", "TransY", "TransZ", "QuatX", "QuatY", "QuatZ", "QuatW"}; break; case JointModelType::UNSUPPORTED: default: - PRINT_ERROR("Joints of type 'NONE' do not have fieldnames."); + jointTypeSuffixesOut = {""}; + PRINT_ERROR("Joints of type 'UNSUPPORTED' do not have fieldnames."); return hresult_t::ERROR_GENERIC; } @@ -211,41 +207,33 @@ namespace jiminy } hresult_t getJointTypeVelocitySuffixes(JointModelType jointTypeIn, - std::vector & jointTypeSuffixesOut) + std::vector & jointTypeSuffixesOut) { // If no extra discrimination is needed - jointTypeSuffixesOut = std::vector({std::string("")}); + jointTypeSuffixesOut = {""}; switch (jointTypeIn) { case JointModelType::LINEAR: - break; case JointModelType::ROTARY: - break; case JointModelType::ROTARY_UNBOUNDED: + jointTypeSuffixesOut = {""}; break; case JointModelType::PLANAR: - jointTypeSuffixesOut = - std::vector({std::string("LinX"), std::string("LinY")}); + jointTypeSuffixesOut = {"LinX", "LinY"}; break; case JointModelType::TRANSLATION: - jointTypeSuffixesOut = std::vector( - {std::string("LinX"), std::string("LinY"), std::string("LinZ")}); + jointTypeSuffixesOut = {"LinX", "LinY", "LinZ"}; break; case JointModelType::SPHERICAL: - jointTypeSuffixesOut = std::vector( - {std::string("AngX"), std::string("AngY"), std::string("AngZ")}); + jointTypeSuffixesOut = {"AngX", "AngY", "AngZ"}; break; case JointModelType::FREE: - jointTypeSuffixesOut = std::vector({std::string("LinX"), - std::string("LinY"), - std::string("LinZ"), - std::string("AngX"), - std::string("AngY"), - std::string("AngZ")}); + jointTypeSuffixesOut = {"LinX", "LinY", "LinZ", "AngX", "AngY", "AngZ"}; break; case JointModelType::UNSUPPORTED: default: - PRINT_ERROR("Joints of type 'NONE' do not have fieldnames."); + jointTypeSuffixesOut = {""}; + PRINT_ERROR("Joints of type 'UNSUPPORTED' do not have fieldnames."); return hresult_t::ERROR_GENERIC; } @@ -582,41 +570,41 @@ namespace jiminy /* Update vectors based on joint index: effortLimit, velocityLimit, lowerPositionLimit and upperPositionLimit. */ - swapMatrixBlocks(modelInOut.effortLimit, - modelInOut.joints[firstJointIdx].idx_v(), - modelInOut.joints[firstJointIdx].nv(), - modelInOut.joints[secondJointIdx].idx_v(), - modelInOut.joints[secondJointIdx].nv()); - swapMatrixBlocks(modelInOut.velocityLimit, - modelInOut.joints[firstJointIdx].idx_v(), - modelInOut.joints[firstJointIdx].nv(), - modelInOut.joints[secondJointIdx].idx_v(), - modelInOut.joints[secondJointIdx].nv()); - swapMatrixBlocks(modelInOut.lowerPositionLimit, - modelInOut.joints[firstJointIdx].idx_q(), - modelInOut.joints[firstJointIdx].nq(), - modelInOut.joints[secondJointIdx].idx_q(), - modelInOut.joints[secondJointIdx].nq()); - swapMatrixBlocks(modelInOut.upperPositionLimit, - modelInOut.joints[firstJointIdx].idx_q(), - modelInOut.joints[firstJointIdx].nq(), - modelInOut.joints[secondJointIdx].idx_q(), - modelInOut.joints[secondJointIdx].nq()); - swapMatrixBlocks(modelInOut.rotorInertia, - modelInOut.joints[firstJointIdx].idx_v(), - modelInOut.joints[firstJointIdx].nv(), - modelInOut.joints[secondJointIdx].idx_v(), - modelInOut.joints[secondJointIdx].nv()); - swapMatrixBlocks(modelInOut.friction, - modelInOut.joints[firstJointIdx].idx_v(), - modelInOut.joints[firstJointIdx].nv(), - modelInOut.joints[secondJointIdx].idx_v(), - modelInOut.joints[secondJointIdx].nv()); - swapMatrixBlocks(modelInOut.damping, - modelInOut.joints[firstJointIdx].idx_v(), - modelInOut.joints[firstJointIdx].nv(), - modelInOut.joints[secondJointIdx].idx_v(), - modelInOut.joints[secondJointIdx].nv()); + swapMatrixRows(modelInOut.effortLimit, + modelInOut.joints[firstJointIdx].idx_v(), + modelInOut.joints[firstJointIdx].nv(), + modelInOut.joints[secondJointIdx].idx_v(), + modelInOut.joints[secondJointIdx].nv()); + swapMatrixRows(modelInOut.velocityLimit, + modelInOut.joints[firstJointIdx].idx_v(), + modelInOut.joints[firstJointIdx].nv(), + modelInOut.joints[secondJointIdx].idx_v(), + modelInOut.joints[secondJointIdx].nv()); + swapMatrixRows(modelInOut.lowerPositionLimit, + modelInOut.joints[firstJointIdx].idx_q(), + modelInOut.joints[firstJointIdx].nq(), + modelInOut.joints[secondJointIdx].idx_q(), + modelInOut.joints[secondJointIdx].nq()); + swapMatrixRows(modelInOut.upperPositionLimit, + modelInOut.joints[firstJointIdx].idx_q(), + modelInOut.joints[firstJointIdx].nq(), + modelInOut.joints[secondJointIdx].idx_q(), + modelInOut.joints[secondJointIdx].nq()); + swapMatrixRows(modelInOut.rotorInertia, + modelInOut.joints[firstJointIdx].idx_v(), + modelInOut.joints[firstJointIdx].nv(), + modelInOut.joints[secondJointIdx].idx_v(), + modelInOut.joints[secondJointIdx].nv()); + swapMatrixRows(modelInOut.friction, + modelInOut.joints[firstJointIdx].idx_v(), + modelInOut.joints[firstJointIdx].nv(), + modelInOut.joints[secondJointIdx].idx_v(), + modelInOut.joints[secondJointIdx].nv()); + swapMatrixRows(modelInOut.damping, + modelInOut.joints[firstJointIdx].idx_v(), + modelInOut.joints[firstJointIdx].nv(), + modelInOut.joints[secondJointIdx].idx_v(), + modelInOut.joints[secondJointIdx].nv()); /* Switch elements in joint-indexed vectors: parents, names, subtrees, joints, jointPlacements, inertias. */