From 78515b28c0bcf5c68c24f6f82b732c9672e4153d Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Thu, 14 Dec 2023 14:30:56 +0100 Subject: [PATCH] [core] Pass and return fundamental types by value instead of const ref. (#678) --- core/include/jiminy/core/engine/system.h | 1 - .../jiminy/core/hardware/abstract_motor.h | 3 +- .../jiminy/core/hardware/abstract_sensor.h | 5 +- .../jiminy/core/hardware/abstract_sensor.hxx | 2 +- .../jiminy/core/hardware/basic_sensors.h | 2 +- core/src/constraints/frame_constraint.cc | 2 +- core/src/engine/engine_multi_robot.cc | 126 +++++++++--------- core/src/hardware/abstract_motor.cc | 11 +- core/src/hardware/basic_motors.cc | 15 +-- core/src/hardware/basic_sensors.cc | 12 +- core/src/robot/model.cc | 13 +- core/src/solver/constraint_solvers.cc | 10 +- core/src/utilities/json.cc | 2 +- core/src/utilities/pinocchio.cc | 22 +-- .../include/jiminy/python/utilities.h | 4 +- python/jiminy_pywrap/src/sensors.cc | 4 +- 16 files changed, 118 insertions(+), 116 deletions(-) diff --git a/core/include/jiminy/core/engine/system.h b/core/include/jiminy/core/engine/system.h index 08a4f26ea..1a054324a 100644 --- a/core/include/jiminy/core/engine/system.h +++ b/core/include/jiminy/core/engine/system.h @@ -18,7 +18,6 @@ namespace jiminy class AbstractController; class LockGuardLocal; - // External force functors using ForceProfileFunctor = std::function; diff --git a/core/include/jiminy/core/hardware/abstract_motor.h b/core/include/jiminy/core/hardware/abstract_motor.h index 802cb50c2..d967c9965 100644 --- a/core/include/jiminy/core/hardware/abstract_motor.h +++ b/core/include/jiminy/core/hardware/abstract_motor.h @@ -16,7 +16,6 @@ namespace jiminy { class Robot; - class AbstractMotorBase; /// \brief Structure holding the data for every motor. @@ -123,7 +122,7 @@ namespace jiminy const std::string & getName() const; /// \brief Index of the motor. - const std::size_t & getIdx() const; + std::size_t getIdx() const; /// \brief Name of the joint associated with the motor. const std::string & getJointName() const; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h index 82ddb2fec..48e62df95 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.h +++ b/core/include/jiminy/core/hardware/abstract_sensor.h @@ -19,7 +19,6 @@ namespace jiminy { class TelemetryData; class Robot; - class AbstractSensorBase; // Sensor data holder @@ -286,7 +285,7 @@ namespace jiminy const std::string & getName() const; /// \brief Index of the sensor of the global shared buffer. - virtual const std::size_t & getIdx() const = 0; + virtual std::size_t getIdx() const = 0; /// \brief Type of the sensor. virtual const std::string & getType() const = 0; @@ -420,7 +419,7 @@ namespace jiminy void updateTelemetryAll() override final; virtual hresult_t setOptionsAll(const GenericConfig & sensorOptions) override final; - virtual const std::size_t & getIdx() const override final; + virtual std::size_t getIdx() const override final; virtual const std::string & getType() const override final; virtual const std::vector & getFieldnames() const final; virtual uint64_t getSize() const override final; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index d32aa1427..c229d7c9e 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -237,7 +237,7 @@ namespace jiminy } template - const std::size_t & AbstractSensorTpl::getIdx() const + std::size_t AbstractSensorTpl::getIdx() const { return sensorIdx_; } diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h index 51584a168..6df33586a 100644 --- a/core/include/jiminy/core/hardware/basic_sensors.h +++ b/core/include/jiminy/core/hardware/basic_sensors.h @@ -172,7 +172,7 @@ namespace jiminy virtual hresult_t refreshProxies() final override; const std::string & getMotorName() const; - const std::size_t & getMotorIdx() const; + std::size_t getMotorIdx() const; private: virtual hresult_t set(float64_t t, diff --git a/core/src/constraints/frame_constraint.cc b/core/src/constraints/frame_constraint.cc index 993122a79..68c995e3c 100644 --- a/core/src/constraints/frame_constraint.cc +++ b/core/src/constraints/frame_constraint.cc @@ -180,7 +180,7 @@ namespace jiminy // Extract masked jacobian and drift, only containing fixed dofs for (uint32_t i = 0; i < dofsFixed_.size(); ++i) { - uint32_t dofIndex = dofsFixed_[i]; + const uint32_t dofIndex = dofsFixed_[i]; jacobian_.row(i) = frameJacobian_.row(dofIndex); drift_[i] = frameDrift_.toVector()[dofIndex]; } diff --git a/core/src/engine/engine_multi_robot.cc b/core/src/engine/engine_multi_robot.cc index bdf1c336a..94d03869b 100644 --- a/core/src/engine/engine_multi_robot.cc +++ b/core/src/engine/engine_multi_robot.cc @@ -973,8 +973,8 @@ namespace jiminy } for (int32_t i = model.njoints - 1; i > 0; --i) { - pinocchio::JointIndex jointIdx = model.joints[i].id(); - pinocchio::JointIndex parentIdx = model.parents[jointIdx]; + const pinocchio::JointIndex jointIdx = model.joints[i].id(); + const pinocchio::JointIndex parentIdx = model.parents[jointIdx]; if (parentIdx > 0) { data.Ycrb[parentIdx] += data.liMi[jointIdx].act(data.Ycrb[jointIdx]); @@ -998,7 +998,7 @@ namespace jiminy } for (int32_t i = model.njoints - 1; i > 0; --i) { - pinocchio::JointIndex parentIdx = model.parents[i]; + const pinocchio::JointIndex parentIdx = model.parents[i]; data.h[parentIdx] += data.liMi[i].act(data.h[i]); data.f[parentIdx] += data.liMi[i].act(data.f[i]); } @@ -1585,8 +1585,8 @@ namespace jiminy u = uInternal + uCustom; for (const auto & motor : systemIt->robot->getMotors()) { - const std::size_t & motorIdx = motor->getIdx(); - int32_t motorVelocityIdx = motor->getJointVelocityIdx(); + const std::size_t motorIdx = motor->getIdx(); + const int32_t motorVelocityIdx = motor->getJointVelocityIdx(); u[motorVelocityIdx] += uMotor[motorIdx]; } } @@ -1866,14 +1866,15 @@ namespace jiminy it uses the user-defined parameter dtMax. */ if (stepSize < EPS) { - float64_t controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod; + const float64_t controllerUpdatePeriod = + engineOptions_->stepper.controllerUpdatePeriod; if (controllerUpdatePeriod > EPS) { stepSize = controllerUpdatePeriod; } else { - float64_t sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; + const float64_t sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; if (sensorsUpdatePeriod > EPS) { stepSize = sensorsUpdatePeriod; @@ -1899,9 +1900,9 @@ namespace jiminy /* Avoid compounding of error using Kahan algorithm. It consists in keeping track of the cumulative rounding error to add it back to the sum when it gets larger than the numerical precision, thus avoiding it to grows unbounded. */ - float64_t stepSize_true = stepSize - stepperState_.tError; - float64_t tEnd = stepperState_.t + stepSize_true; - stepperState_.tError = (tEnd - stepperState_.t) - stepSize_true; + const float64_t stepSizeCorrected = stepSize - stepperState_.tError; + const float64_t tEnd = stepperState_.t + stepSizeCorrected; + stepperState_.tError = (tEnd - stepperState_.t) - stepSizeCorrected; // Get references to some internal stepper buffers float64_t & t = stepperState_.t; @@ -2449,10 +2450,10 @@ namespace jiminy // Update sensors data if necessary, namely if time-continuous or breakpoint if (returnCode == hresult_t::SUCCESS) { - float64_t sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; - bool_t mustUpdateSensors = sensorsUpdatePeriod < EPS; - float64_t dtNextSensorsUpdatePeriod = + const float64_t sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod; + const float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod); + bool_t mustUpdateSensors = sensorsUpdatePeriod < EPS; if (!mustUpdateSensors) { mustUpdateSensors = dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP || @@ -2828,7 +2829,7 @@ namespace jiminy // Make sure the dtMax is not out of range GenericConfig stepperOptions = boost::get(engineOptions.at("stepper")); - float64_t dtMax = boost::get(stepperOptions.at("dtMax")); + const float64_t dtMax = boost::get(stepperOptions.at("dtMax")); if (SIMULATION_MAX_TIMESTEP + EPS < dtMax || dtMax < SIMULATION_MIN_TIMESTEP) { PRINT_ERROR("'dtMax' option is out of range."); @@ -2836,7 +2837,7 @@ namespace jiminy } // Make sure successiveIterFailedMax is strictly positive - uint32_t successiveIterFailedMax = + const uint32_t successiveIterFailedMax = boost::get(stepperOptions.at("successiveIterFailedMax")); if (successiveIterFailedMax < 1) { @@ -2853,9 +2854,9 @@ namespace jiminy } // Make sure the controller and sensor update periods are valid - float64_t sensorsUpdatePeriod = + const float64_t sensorsUpdatePeriod = boost::get(stepperOptions.at("sensorsUpdatePeriod")); - float64_t controllerUpdatePeriod = + const float64_t controllerUpdatePeriod = boost::get(stepperOptions.at("controllerUpdatePeriod")); auto [isIncluded, minUpdatePeriod] = isGcdIncluded(systemsDataHolder_, controllerUpdatePeriod, sensorsUpdatePeriod); @@ -2935,7 +2936,7 @@ namespace jiminy /* Reset random number generators if setOptions is called for the first time, or if the desired random seed has changed. */ - uint32_t randomSeed = boost::get(stepperOptions.at("randomSeed")); + const uint32_t randomSeed = boost::get(stepperOptions.at("randomSeed")); if (!engineOptions_ || randomSeed != engineOptions_->stepper.randomSeed) { resetRandomGenerators(randomSeed); @@ -3245,7 +3246,7 @@ namespace jiminy // In case of slippage the contact point has actually moved and must be updated constraint->enable(); auto & frameConstraint = static_cast(*constraint.get()); - pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); + const pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); frameConstraint.setReferenceTransform( {system.robot->pncData_.oMf[frameIdx].rotation(), system.robot->pncData_.oMf[frameIdx].translation() - depth * nGround}); @@ -3278,7 +3279,7 @@ namespace jiminy // Compute the ground normal and penetration depth at the contact point const Eigen::Vector3d & posFrame = transformFrameInWorld.translation(); auto ground = engineOptions_->world.groundProfile(posFrame); - float64_t zGround = std::get(ground); + const float64_t zGround = std::get(ground); Eigen::Vector3d & nGround = std::get(ground); nGround.normalize(); // Make sure the ground normal is normalized @@ -3466,17 +3467,17 @@ namespace jiminy Eigen::VectorXd & u) { // Define some proxies for convenience - pinocchio::JointIndex jointIdx = joint.id(); - uint32_t positionIdx = joint.idx_q(); - uint32_t velocityIdx = joint.idx_v(); - float64_t qJoint = q[positionIdx]; - float64_t qJointMin = positionLimitMin[positionIdx]; - float64_t qJointMax = positionLimitMax[positionIdx]; - float64_t vJoint = v[velocityIdx]; - float64_t Ia = getSubtreeInertiaProj(joint.derived(), pncData.Ycrb[jointIdx]); - float64_t stiffness = engineOptions->joints.boundStiffness; - float64_t damping = engineOptions->joints.boundDamping; - float64_t transitionEps = engineOptions->contacts.transitionEps; + const pinocchio::JointIndex jointIdx = joint.id(); + const uint32_t positionIdx = joint.idx_q(); + const uint32_t velocityIdx = joint.idx_v(); + const float64_t qJoint = q[positionIdx]; + const float64_t qJointMin = positionLimitMin[positionIdx]; + const float64_t qJointMax = positionLimitMax[positionIdx]; + const float64_t vJoint = v[velocityIdx]; + const float64_t Ia = getSubtreeInertiaProj(joint.derived(), pncData.Ycrb[jointIdx]); + const float64_t stiffness = engineOptions->joints.boundStiffness; + const float64_t damping = engineOptions->joints.boundDamping; + const float64_t transitionEps = engineOptions->contacts.transitionEps; // Check if out-of-bounds if (contactModel == contactModel_t::SPRING_DAMPER) @@ -3595,13 +3596,13 @@ namespace jiminy Eigen::VectorXd & u) { // Define some proxies for convenience - pinocchio::JointIndex jointIdx = joint.id(); - uint32_t velocityIdx = joint.idx_v(); - float64_t vJoint = v[velocityIdx]; - float64_t vJointMin = -velocityLimitMax[velocityIdx]; - float64_t vJointMax = velocityLimitMax[velocityIdx]; - float64_t Ia = getSubtreeInertiaProj(joint.derived(), pncData.Ycrb[jointIdx]); - float64_t damping = engineOptions->joints.boundDamping; + const pinocchio::JointIndex jointIdx = joint.id(); + const uint32_t velocityIdx = joint.idx_v(); + const float64_t vJoint = v[velocityIdx]; + const float64_t vJointMin = -velocityLimitMax[velocityIdx]; + const float64_t vJointMax = velocityLimitMax[velocityIdx]; + const float64_t Ia = getSubtreeInertiaProj(joint.derived(), pncData.Ycrb[jointIdx]); + const float64_t damping = engineOptions->joints.boundDamping; // Check if out-of-bounds if (contactModel == contactModel_t::SPRING_DAMPER) @@ -3706,9 +3707,9 @@ namespace jiminy system.robot->getFlexibleJointsModelIdx(); for (std::size_t i = 0; i < flexibilityIdx.size(); ++i) { - pinocchio::JointIndex jointIdx = flexibilityIdx[i]; - uint32_t positionIdx = pncModel.joints[jointIdx].idx_q(); - uint32_t velocityIdx = pncModel.joints[jointIdx].idx_v(); + const pinocchio::JointIndex jointIdx = flexibilityIdx[i]; + const uint32_t positionIdx = pncModel.joints[jointIdx].idx_q(); + const uint32_t velocityIdx = pncModel.joints[jointIdx].idx_v(); const Eigen::Vector3d & stiffness = mdlDynOptions.flexibilityConfig[i].stiffness; const Eigen::Vector3d & damping = mdlDynOptions.flexibilityConfig[i].damping; @@ -3735,7 +3736,7 @@ namespace jiminy for (std::size_t i = 0; i < contactFramesIdx.size(); ++i) { // Compute force at the given contact frame. - pinocchio::FrameIndex frameIdx = contactFramesIdx[i]; + const pinocchio::FrameIndex frameIdx = contactFramesIdx[i]; auto & constraint = systemData.constraintsHolder.contactFrames[i].second; pinocchio::Force & fextLocal = systemData.contactFramesForces[i]; if (!isStateUpToDate) @@ -3744,7 +3745,8 @@ namespace jiminy } // Apply the force at the origin of the parent joint frame, in local joint frame - pinocchio::JointIndex parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; + const pinocchio::JointIndex parentJointIdx = + system.robot->pncModel_.frames[frameIdx].parent; fext[parentJointIdx] += fextLocal; // Convert contact force from global frame to local frame to store it in contactForces_ @@ -3762,8 +3764,9 @@ namespace jiminy { /* Compute force at the given collision body. It returns the force applied at the origin of parent joint frame in global frame. */ - pinocchio::FrameIndex frameIdx = collisionBodiesIdx[i]; - pinocchio::JointIndex parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; + const pinocchio::FrameIndex frameIdx = collisionBodiesIdx[i]; + const pinocchio::JointIndex parentJointIdx = + system.robot->pncModel_.frames[frameIdx].parent; for (std::size_t j = 0; j < collisionPairsIdx[i].size(); ++j) { pinocchio::Force & fextLocal = systemData.collisionBodiesForces[i][j]; @@ -3797,8 +3800,8 @@ namespace jiminy to get around the ambiguous t- versus t+. */ if (*forcesImpulseActiveIt) { - pinocchio::FrameIndex frameIdx = forcesImpulseIt->frameIdx; - pinocchio::JointIndex parentJointIdx = + const pinocchio::FrameIndex frameIdx = forcesImpulseIt->frameIdx; + const pinocchio::JointIndex parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; const pinocchio::Force & F = forcesImpulseIt->F; @@ -3810,8 +3813,9 @@ namespace jiminy // Add the effect of time-continuous external force profiles for (auto & forceProfile : systemData.forcesProfile) { - pinocchio::FrameIndex frameIdx = forceProfile.frameIdx; - pinocchio::JointIndex parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; + const pinocchio::FrameIndex frameIdx = forceProfile.frameIdx; + const pinocchio::JointIndex parentJointIdx = + system.robot->pncModel_.frames[frameIdx].parent; if (forceProfile.updatePeriod < EPS) { forceProfile.forcePrev = forceProfile.forceFct(t, q, v); @@ -3828,31 +3832,31 @@ namespace jiminy for (auto & forceCoupling : forcesCoupling_) { // Extract info about the first system involved - int32_t systemIdx1 = forceCoupling.systemIdx1; + const int32_t systemIdx1 = forceCoupling.systemIdx1; const systemHolder_t & system1 = systems_[systemIdx1]; const Eigen::VectorXd & q1 = qSplit[systemIdx1]; const Eigen::VectorXd & v1 = vSplit[systemIdx1]; - pinocchio::FrameIndex frameIdx1 = forceCoupling.frameIdx1; + const pinocchio::FrameIndex frameIdx1 = forceCoupling.frameIdx1; ForceVector & fext1 = systemsDataHolder_[systemIdx1].state.fExternal; // Extract info about the second system involved - int32_t systemIdx2 = forceCoupling.systemIdx2; + const int32_t systemIdx2 = forceCoupling.systemIdx2; const systemHolder_t & system2 = systems_[systemIdx2]; const Eigen::VectorXd & q2 = qSplit[systemIdx2]; const Eigen::VectorXd & v2 = vSplit[systemIdx2]; - pinocchio::FrameIndex frameIdx2 = forceCoupling.frameIdx2; + const pinocchio::FrameIndex frameIdx2 = forceCoupling.frameIdx2; ForceVector & fext2 = systemsDataHolder_[systemIdx2].state.fExternal; // Compute the coupling force pinocchio::Force force = forceCoupling.forceFct(t, q1, v1, q2, v2); - pinocchio::JointIndex parentJointIdx1 = + const pinocchio::JointIndex parentJointIdx1 = system1.robot->pncModel_.frames[frameIdx1].parent; fext1[parentJointIdx1] += convertForceGlobalFrameToJoint( system1.robot->pncModel_, system1.robot->pncData_, frameIdx1, force); // Move force from frame1 to frame2 to apply it to the second system force.toVector() *= -1; - pinocchio::JointIndex parentJointIdx2 = + const pinocchio::JointIndex parentJointIdx2 = system2.robot->pncModel_.frames[frameIdx2].parent; const Eigen::Vector3d offset = system2.robot->pncData_.oMf[frameIdx2].translation() - system1.robot->pncData_.oMf[frameIdx1].translation(); @@ -4020,8 +4024,8 @@ namespace jiminy u = uInternal + uCustom; for (const auto & motor : systemIt->robot->getMotors()) { - const std::size_t & motorIdx = motor->getIdx(); - int32_t motorVelocityIdx = motor->getJointVelocityIdx(); + const std::size_t motorIdx = motor->getIdx(); + const int32_t motorVelocityIdx = motor->getJointVelocityIdx(); u[motorVelocityIdx] += uMotor[motorIdx]; } @@ -4140,8 +4144,8 @@ namespace jiminy }); // Convert the force from local world aligned frame to local frame - pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); - auto rotationWorldInContact = data.oMf[frameIdx].rotation().transpose(); + const pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); + const auto rotationWorldInContact = data.oMf[frameIdx].rotation().transpose(); forceIt->linear().noalias() = rotationWorldInContact * fextInWorld.linear(); forceIt->angular().noalias() = rotationWorldInContact * fextInWorld.angular(); @@ -4176,8 +4180,8 @@ namespace jiminy }); // Convert the force from local world aligned to local parent joint - pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); - pinocchio::JointIndex jointIdx = model.frames[frameIdx].parent; + const pinocchio::FrameIndex frameIdx = frameConstraint.getFrameIdx(); + const pinocchio::JointIndex jointIdx = model.frames[frameIdx].parent; fext[jointIdx] += convertForceGlobalFrameToJoint(model, data, frameIdx, fextInWorld); }); diff --git a/core/src/hardware/abstract_motor.cc b/core/src/hardware/abstract_motor.cc index ea2a0dd1b..e817b91c0 100644 --- a/core/src/hardware/abstract_motor.cc +++ b/core/src/hardware/abstract_motor.cc @@ -160,23 +160,24 @@ namespace jiminy if (isInitialized_) { // Check if armature has changed - bool_t enableArmature = boost::get(motorOptions.at("enableArmature")); + const bool_t enableArmature = boost::get(motorOptions.at("enableArmature")); internalBuffersMustBeUpdated |= (baseMotorOptions_->enableArmature != enableArmature); if (enableArmature) { - float64_t armature = boost::get(motorOptions.at("armature")); + const float64_t armature = boost::get(motorOptions.at("armature")); internalBuffersMustBeUpdated |= // std::abs(armature - baseMotorOptions_->armature) > EPS; } // Check if command limit has changed - bool_t commandLimitFromUrdf = + const bool_t commandLimitFromUrdf = boost::get(motorOptions.at("commandLimitFromUrdf")); internalBuffersMustBeUpdated |= (baseMotorOptions_->commandLimitFromUrdf != commandLimitFromUrdf); if (!commandLimitFromUrdf) { - float64_t commandLimit = boost::get(motorOptions.at("commandLimit")); + const float64_t commandLimit = + boost::get(motorOptions.at("commandLimit")); internalBuffersMustBeUpdated |= std::abs(commandLimit - baseMotorOptions_->commandLimit) > EPS; } @@ -353,7 +354,7 @@ namespace jiminy return name_; } - const std::size_t & AbstractMotorBase::getIdx() const + std::size_t AbstractMotorBase::getIdx() const { return motorIdx_; } diff --git a/core/src/hardware/basic_motors.cc b/core/src/hardware/basic_motors.cc index e9e82ae1e..8e3bbd2aa 100644 --- a/core/src/hardware/basic_motors.cc +++ b/core/src/hardware/basic_motors.cc @@ -104,18 +104,17 @@ namespace jiminy It is computed on joint side instead of the motor. */ if (motorOptions_->enableFriction) { - float64_t vJoint = v; - if (vJoint > 0) + if (v > 0) { - data() += motorOptions_->frictionViscousPositive * vJoint + - motorOptions_->frictionDryPositive * - tanh(motorOptions_->frictionDrySlope * vJoint); + data() += + motorOptions_->frictionViscousPositive * v + + motorOptions_->frictionDryPositive * tanh(motorOptions_->frictionDrySlope * v); } else { - data() += motorOptions_->frictionViscousNegative * vJoint + - motorOptions_->frictionDryNegative * - tanh(motorOptions_->frictionDrySlope * vJoint); + data() += + motorOptions_->frictionViscousNegative * v + + motorOptions_->frictionDryNegative * tanh(motorOptions_->frictionDrySlope * v); } } diff --git a/core/src/hardware/basic_sensors.cc b/core/src/hardware/basic_sensors.cc index b9e6e6d5e..cd373a325 100644 --- a/core/src/hardware/basic_sensors.cc +++ b/core/src/hardware/basic_sensors.cc @@ -386,7 +386,7 @@ namespace jiminy GET_ROBOT_IF_INITIALIZED() // Get the sum of external forces applied on parent joint - pinocchio::JointIndex i = parentJointIdx_; + const pinocchio::JointIndex i = parentJointIdx_; const pinocchio::Force & fJoint = fExternal[i]; // Transform the force from joint frame to sensor frame @@ -489,12 +489,12 @@ namespace jiminy GET_ROBOT_IF_INITIALIZED() const auto & joint = robot->pncModel_.joints[jointIdx_]; - int32_t jointPositionIdx = joint.idx_q(); - int32_t jointVelocityIdx = joint.idx_v(); + const int32_t jointPositionIdx = joint.idx_q(); + const int32_t jointVelocityIdx = joint.idx_v(); if (jointType_ == JointModelType::ROTARY_UNBOUNDED) { - float64_t cosTheta = q[jointPositionIdx]; - float64_t sinTheta = q[jointPositionIdx + 1]; + const float64_t cosTheta = q[jointPositionIdx]; + const float64_t sinTheta = q[jointPositionIdx + 1]; data()[0] = std::atan2(sinTheta, cosTheta); } else @@ -564,7 +564,7 @@ namespace jiminy return motorName_; } - const std::size_t & EffortSensor::getMotorIdx() const + std::size_t EffortSensor::getMotorIdx() const { return motorIdx_; } diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index af22ae2d5..83fa46360 100644 --- a/core/src/robot/model.cc +++ b/core/src/robot/model.cc @@ -1159,10 +1159,11 @@ namespace jiminy for (const std::string & jointName : rigidJointsNames_) { - pinocchio::JointIndex jointIdx = pncModel_.getJointId(jointName); + const pinocchio::JointIndex jointIdx = pncModel_.getJointId(jointName); // Add bias to com position - float64_t comBiasStd = mdlOptions_->dynamics.centerOfMassPositionBodiesBiasStd; + const float64_t comBiasStd = + mdlOptions_->dynamics.centerOfMassPositionBodiesBiasStd; if (comBiasStd > EPS) { Eigen::Vector3d & comRelativePositionBody = @@ -1173,7 +1174,7 @@ namespace jiminy /* Add bias to body mass. It cannot be less than min(original mass, 1g) for numerical stability. */ - float64_t massBiasStd = mdlOptions_->dynamics.massBodiesBiasStd; + const float64_t massBiasStd = mdlOptions_->dynamics.massBodiesBiasStd; if (massBiasStd > EPS) { float64_t & massBody = pncModel_.inertias[jointIdx].mass(); @@ -1188,7 +1189,7 @@ namespace jiminy small rotation is applied to the principal axes based on a randomly generated rotation axis. Finally, the biased inertia matrix is obtained doing `A @ diag(M) @ A.T`. If no bias, the original inertia matrix is recovered. */ - float64_t inertiaBiasStd = mdlOptions_->dynamics.inertiaBodiesBiasStd; + const float64_t inertiaBiasStd = mdlOptions_->dynamics.inertiaBodiesBiasStd; if (inertiaBiasStd > EPS) { pinocchio::Symmetric3 & inertiaBody = pncModel_.inertias[jointIdx].inertia(); @@ -1207,7 +1208,7 @@ namespace jiminy } // Add bias to relative body position (rotation excluded !) - float64_t relativeBodyPosBiasStd = + const float64_t relativeBodyPosBiasStd = mdlOptions_->dynamics.relativePositionBodiesBiasStd; if (relativeBodyPosBiasStd > EPS) { @@ -1813,7 +1814,7 @@ namespace jiminy "centerOfMassPositionBodiesBiasStd", "relativePositionBodiesBiasStd"}}) { - float64_t value = boost::get(dynOptionsHolder.at(field)); + const float64_t value = boost::get(dynOptionsHolder.at(field)); if (0.9 < value || value < 0.0) { PRINT_ERROR( diff --git a/core/src/solver/constraint_solvers.cc b/core/src/solver/constraint_solvers.cc index 5322bd8b9..b4355f374 100644 --- a/core/src/solver/constraint_solvers.cc +++ b/core/src/solver/constraint_solvers.cc @@ -144,10 +144,10 @@ namespace jiminy const ConstraintBlock & block = constraintData.blocks[i]; const Eigen::Index * fIdx = block.fIdx; const std::uint_fast8_t & fSize = block.fSize; - Eigen::Index o = constraintData.startIdx; + const Eigen::Index o = constraintData.startIdx; const Eigen::Index i0 = o + fIdx[0]; - float64_t hi = block.hi; - float64_t lo = block.lo; + const float64_t hi = block.hi; + const float64_t lo = block.lo; float64_t & e = x[i0]; // Bypass zero-ed coefficients @@ -169,7 +169,7 @@ namespace jiminy { const Eigen::Index k = o + fIdx[j]; y_[k] = b[k] - A.col(k).dot(x); - float64_t A_kk = A(k, k); + const float64_t A_kk = A(k, k); if (A_kk > A_max) { A_max = A_kk; @@ -202,7 +202,7 @@ namespace jiminy float64_t squaredNorm = e * e; for (std::uint_fast8_t j = 1; j < fSize - 1; ++j) { - float64_t f = xConst[fIdx[j]]; + const float64_t f = xConst[fIdx[j]]; squaredNorm += f * f; } if (squaredNorm > thr * thr) diff --git a/core/src/utilities/json.cc b/core/src/utilities/json.cc index 228ed1ec0..ddab99b5f 100644 --- a/core/src/utilities/json.cc +++ b/core/src/utilities/json.cc @@ -263,7 +263,7 @@ namespace jiminy } else { - field = Eigen::VectorXd(); + field = Eigen::VectorXd{}; } } else diff --git a/core/src/utilities/pinocchio.cc b/core/src/utilities/pinocchio.cc index 517f138a0..9c5a96886 100644 --- a/core/src/utilities/pinocchio.cc +++ b/core/src/utilities/pinocchio.cc @@ -32,7 +32,7 @@ namespace jiminy ++i) { // Get joint starting and ending index in position vector. - int32_t startIdx = model.joints[i].idx_q(); + const int32_t startIdx = model.joints[i].idx_q(); const int32_t endIdx = startIdx + model.joints[i].nq(); // If idx is between start and end, we found the joint we were looking for. @@ -55,7 +55,7 @@ namespace jiminy ++i) { // Get joint starting and ending index in velocity vector. - int32_t startIdx = model.joints[i].idx_v(); + const int32_t startIdx = model.joints[i].idx_v(); const int32_t endIdx = startIdx + model.joints[i].nv(); // If idx is between start and end, we found the joint we were looking for @@ -303,9 +303,9 @@ namespace jiminy return hresult_t::ERROR_BAD_INPUT; } - pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); - int32_t jointPositionFirstIdx = model.joints[jointModelIdx].idx_q(); - int32_t jointNq = model.joints[jointModelIdx].nq(); + const pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); + const int32_t jointPositionFirstIdx = model.joints[jointModelIdx].idx_q(); + const int32_t jointNq = model.joints[jointModelIdx].nq(); jointPositionIdx.resize(static_cast(jointNq)); std::iota(jointPositionIdx.begin(), jointPositionIdx.end(), jointPositionFirstIdx); @@ -425,9 +425,9 @@ namespace jiminy return hresult_t::ERROR_BAD_INPUT; } - pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); - int32_t jointVelocityFirstIdx = model.joints[jointModelIdx].idx_v(); - int32_t jointNv = model.joints[jointModelIdx].nv(); + const pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); + const int32_t jointVelocityFirstIdx = model.joints[jointModelIdx].idx_v(); + const int32_t jointNv = model.joints[jointModelIdx].nv(); jointVelocityIdx.resize(static_cast(jointNv)); std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx); @@ -446,7 +446,7 @@ namespace jiminy return hresult_t::ERROR_BAD_INPUT; } - pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); + const pinocchio::JointIndex jointModelIdx = model.getJointId(jointName); jointVelocityFirstIdx = model.joints[jointModelIdx].idx_v(); return hresult_t::SUCCESS; @@ -694,7 +694,7 @@ namespace jiminy // Add new joint to frame list pinocchio::FrameIndex childFrameIdx; getFrameIdx(modelInOut, childJointNameIn, childFrameIdx); // Cannot fail at this point - pinocchio::FrameIndex newFrameIdx = modelInOut.addJointFrame( + const pinocchio::FrameIndex newFrameIdx = modelInOut.addJointFrame( newJointIdx, static_cast(modelInOut.frames[childFrameIdx].previousFrame)); // Update child joint previousFrame index @@ -827,7 +827,7 @@ namespace jiminy { continue; } - pinocchio::JointIndex childJointIdx = modelInOut.frames[childFrameIdx].parent; + const pinocchio::JointIndex childJointIdx = modelInOut.frames[childFrameIdx].parent; // Set child joint to be a child of the new joint modelInOut.parents[childJointIdx] = newJointIdx; diff --git a/python/jiminy_pywrap/include/jiminy/python/utilities.h b/python/jiminy_pywrap/include/jiminy/python/utilities.h index 0a881473b..2cb9cdc03 100644 --- a/python/jiminy_pywrap/include/jiminy/python/utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/utilities.h @@ -122,7 +122,7 @@ namespace jiminy::python { if (strncmp(s.basename, "void", 4) == 0) { - static const char * none = "None"; + static char constexpr none[] = "None"; return none; } const PyTypeObject * py_type = s.pytype_f ? s.pytype_f() : nullptr; @@ -132,7 +132,7 @@ namespace jiminy::python } else { - static const char * object = "object"; + static char constexpr object[] = "object"; return object; } } diff --git a/python/jiminy_pywrap/src/sensors.cc b/python/jiminy_pywrap/src/sensors.cc index 1543b1e19..b5e056f99 100644 --- a/python/jiminy_pywrap/src/sensors.cc +++ b/python/jiminy_pywrap/src/sensors.cc @@ -182,7 +182,7 @@ namespace jiminy::python for (const auto & sensorData : sensorsDataType.second) { const std::string & sensorName = sensorData.name; - const std::size_t & sensorIdx = sensorData.idx; + std::size_t sensorIdx = sensorData.idx; const Eigen::Ref & sensorDataValue = sensorData.value; s << " (" << sensorIdx << ") " << sensorName << ": " << sensorDataValue.transpose().format(HeavyFmt); @@ -275,7 +275,7 @@ namespace jiminy::python for (std::size_t i = 0; i < fieldnames.size(); ++i) { const std::string & field = fieldnames[i]; - float64_t value = sensorDataValue[i]; + const float64_t value = sensorDataValue[i]; if (i > 0) { s << ", ";