[core] Move 'enable(Velocity|Effort)Limit' options from AbstractMotor…

… to SimpleMotor.

core/include/jiminy/core/hardware/abstract_motor.h

@@ -43,10 +43,8 @@ namespace jiminy
         {
             GenericConfig config;
             config["mechanicalReduction"] = 1.0;
-            config["enableVelocityLimit"] = false;
             config["velocityLimitFromUrdf"] = true;
             config["velocityLimit"] = 0.0;
-            config["enableEffortLimit"] = true;
             config["effortLimitFromUrdf"] = true;
             config["effortLimit"] = 0.0;
             config["enableArmature"] = false;
@@ -61,10 +59,8 @@ namespace jiminy
         {
             /// \brief Mechanical reduction ratio of transmission (joint/motor), usually >= 1.0.
             const double mechanicalReduction;
-            const bool enableVelocityLimit;
             const bool velocityLimitFromUrdf;
             const double velocityLimit;
-            const bool enableEffortLimit;
             const bool effortLimitFromUrdf;
             const double effortLimit;
             const bool enableArmature;
@@ -74,10 +70,8 @@ namespace jiminy
 
             AbstractMotorOptions(const GenericConfig & options) :
             mechanicalReduction(boost::get<double>(options.at("mechanicalReduction"))),
-            enableVelocityLimit(boost::get<bool>(options.at("enableVelocityLimit"))),
             velocityLimitFromUrdf(boost::get<bool>(options.at("velocityLimitFromUrdf"))),
             velocityLimit(boost::get<double>(options.at("velocityLimit"))),
-            enableEffortLimit(boost::get<bool>(options.at("enableEffortLimit"))),
             effortLimitFromUrdf(boost::get<bool>(options.at("effortLimitFromUrdf"))),
             effortLimit(boost::get<double>(options.at("effortLimit"))),
             enableArmature(boost::get<bool>(options.at("enableArmature"))),

core/include/jiminy/core/hardware/basic_motors.h

@@ -17,6 +17,8 @@ namespace jiminy
             // Add extra options or update default values
             GenericConfig config = AbstractMotorBase::getDefaultMotorOptions();
 
+            config["enableVelocityLimit"] = false;
+            config["enableEffortLimit"] = true;
             config["enableFriction"] = false;
             config["frictionViscousPositive"] = 0.0;
             config["frictionViscousNegative"] = 0.0;
@@ -29,9 +31,11 @@ namespace jiminy
 
         struct SimpleMotorOptions : public AbstractMotorOptions
         {
-            /// \brief Flag to enable the joint friction.
-            ///
-            /// \pre Must be negative.
+            /// \brief Wether velocity limit is enabled.
+            const bool enableVelocityLimit;
+            /// \brief Wether effort limit is enabled.
+            const bool enableEffortLimit;
+            /// \brief Wether joint friction is enabled.
             const bool enableFriction;
             /// \brief Viscous coefficient of the joint friction for positive velocity.
             ///
@@ -58,6 +62,8 @@ namespace jiminy
 
             SimpleMotorOptions(const GenericConfig & options) :
             AbstractMotorOptions(options),
+            enableVelocityLimit(boost::get<bool>(options.at("enableVelocityLimit"))),
+            enableEffortLimit(boost::get<bool>(options.at("enableEffortLimit"))),
             enableFriction{boost::get<bool>(options.at("enableFriction"))},
             frictionViscousPositive{boost::get<double>(options.at("frictionViscousPositive"))},
             frictionViscousNegative{boost::get<double>(options.at("frictionViscousNegative"))},

core/src/hardware/abstract_motor.cc

@@ -199,15 +199,9 @@ namespace jiminy
         // Check if the internal buffers must be updated
         if (isInitialized_)
         {
-            // Check if armature has changed
-            const bool enableArmature = boost::get<bool>(motorOptions.at("enableArmature"));
-            mustNotifyRobot_ |= (baseMotorOptions_->enableArmature != enableArmature);
-            if (enableArmature)
-            {
-                const double armature = boost::get<double>(motorOptions.at("armature"));
-                mustNotifyRobot_ |=  //
-                    std::abs(armature - baseMotorOptions_->armature) > EPS;
-            }
+            // Check if mechanical reduction ratio has changed
+            mustNotifyRobot_ |= abs(baseMotorOptions_->mechanicalReduction - mechanicalReduction) >
+                                EPS;
 
             // Check if backlash has changed
             const bool enableBacklash = boost::get<bool>(motorOptions.at("enableBacklash"));
@@ -219,30 +213,14 @@ namespace jiminy
                     std::abs(backlash - baseMotorOptions_->backlash) > EPS;
             }
 
-            // Check if mechanical reduction ratio has changed
-            mustNotifyRobot_ |= abs(baseMotorOptions_->mechanicalReduction - mechanicalReduction) >
-                                EPS;
-
-            // Check if velocity limit has changed
-            const bool velocityLimitFromUrdf =
-                boost::get<bool>(motorOptions.at("velocityLimitFromUrdf"));
-            mustNotifyRobot_ |=
-                (baseMotorOptions_->velocityLimitFromUrdf != velocityLimitFromUrdf);
-            if (!velocityLimitFromUrdf)
-            {
-                const double velocityLimit = boost::get<double>(motorOptions.at("velocityLimit"));
-                mustNotifyRobot_ |= std::abs(velocityLimit - baseMotorOptions_->velocityLimit) >
-                                    EPS;
-            }
-
-            // Check if effort limit has changed
-            const bool effortLimitFromUrdf =
-                boost::get<bool>(motorOptions.at("effortLimitFromUrdf"));
-            mustNotifyRobot_ |= (baseMotorOptions_->effortLimitFromUrdf != effortLimitFromUrdf);
-            if (!effortLimitFromUrdf)
+            // Check if armature has changed
+            const bool enableArmature = boost::get<bool>(motorOptions.at("enableArmature"));
+            mustNotifyRobot_ |= (baseMotorOptions_->enableArmature != enableArmature);
+            if (enableArmature)
             {
-                const double effortLimit = boost::get<double>(motorOptions.at("effortLimit"));
-                mustNotifyRobot_ |= std::abs(effortLimit - baseMotorOptions_->effortLimit) > EPS;
+                const double armature = boost::get<double>(motorOptions.at("armature"));
+                mustNotifyRobot_ |=  //
+                    std::abs(armature - baseMotorOptions_->armature) > EPS;
             }
         }
 
@@ -252,8 +230,8 @@ namespace jiminy
         // Update inherited polymorphic accessor
         deepUpdate(motorOptionsGeneric_, motorOptions);
 
-        // Refresh the proxies if the attached robot must be notified if any
-        if (robot && isAttached_ && mustNotifyRobot_)
+        // Refresh proxies systematically if motor is attached
+        if (robot && isAttached_)
         {
             refreshProxies();
         }
@@ -330,23 +308,16 @@ namespace jiminy
         }
 
         // Get the motor effort limits on motor side from the URDF or the user options
-        if (baseMotorOptions_->enableEffortLimit)
+        if (baseMotorOptions_->effortLimitFromUrdf)
         {
-            if (baseMotorOptions_->effortLimitFromUrdf)
-            {
-                const Eigen::Index mechanicalJointVelocityIndex =
-                    getJointVelocityFirstIndex(robot->pinocchioModelTh_, jointName_);
-                effortLimit_ = robot->pinocchioModelTh_.effortLimit[mechanicalJointVelocityIndex] /
-                               mechanicalReduction;
-            }
-            else
-            {
-                effortLimit_ = baseMotorOptions_->effortLimit;
-            }
+            const Eigen::Index mechanicalJointVelocityIndex =
+                getJointVelocityFirstIndex(robot->pinocchioModelTh_, jointName_);
+            effortLimit_ = robot->pinocchioModelTh_.effortLimit[mechanicalJointVelocityIndex] /
+                           mechanicalReduction;
         }
         else
         {
-            effortLimit_ = INF;
+            effortLimit_ = baseMotorOptions_->effortLimit;
         }
 
         // Get the motor velocity limits on motor side from the URDF or the user options

core/src/hardware/basic_motors.cc

@@ -61,6 +61,14 @@ namespace jiminy
             JIMINY_THROW(std::invalid_argument, "'frictionDrySlope' must be positive.");
         }
 
+        // Check that velocity limit is not enabled with effort limit
+        if (boost::get<bool>(motorOptions.at("enableVelocityLimit")) &&
+            !boost::get<bool>(motorOptions.at("enableEffortLimit")))
+        {
+            JIMINY_THROW(std::invalid_argument,
+                         "'enableVelocityLimit' cannot be enabled without 'enableEffortLimit'.");
+        }
+
         // Update class-specific "strongly typed" accessor for fast and convenient access
         motorOptions_ = std::make_unique<const SimpleMotorOptions>(motorOptions);
 
@@ -85,17 +93,22 @@ namespace jiminy
 
         /* Compute the motor effort, taking into account velocity and effort limits.
            It is the output of the motor on joint side, ie after the transmission. */
-        double effortMin = -effortLimit_;
-        double effortMax = effortLimit_;
-        if (motorOptions_->enableVelocityLimit)
+        double effortMin = -INF;
+        double effortMax = INF;
+        if (motorOptions_->enableEffortLimit)
         {
-            if (vMotor < -velocityLimit_)
-            {
-                effortMin = 0.0;
-            }
-            if (vMotor > velocityLimit_)
+            effortMin = -effortLimit_;
+            effortMax = effortLimit_;
+            if (motorOptions_->enableVelocityLimit)
             {
-                effortMax = 0.0;
+                if (vMotor < -velocityLimit_)
+                {
+                    effortMin = 0.0;
+                }
+                if (vMotor > velocityLimit_)
+                {
+                    effortMax = 0.0;
+                }
             }
         }
         data() = motorOptions_->mechanicalReduction * std::clamp(command, effortMin, effortMax);

core/unit/engine_sanity_check.cc

@@ -79,7 +79,7 @@ TEST(EngineSanity, EnergyConservation)
     {
         GenericConfig & motorOptions = boost::get<GenericConfig>(motorOptionsItem.second);
         boost::get<bool>(motorOptions.at("enableVelocityLimit")) = false;
-        boost::get<bool>(motorOptions.at("enableCommandLimit")) = false;
+        boost::get<bool>(motorOptions.at("enableEffortLimit")) = false;
     }
 
     // Set all robot options
@@ -93,13 +93,17 @@ TEST(EngineSanity, EnergyConservation)
     Engine engine{};
     engine.addRobot(robot);
 
-    // Configure engine: High accuracy + Continuous-time integration
+    // Configure engine: High accuracy + Continuous-time integration + telemetry
     GenericConfig simuOptions = engine.getDefaultEngineOptions();
     {
         GenericConfig & stepperOptions = boost::get<GenericConfig>(simuOptions.at("stepper"));
         boost::get<double>(stepperOptions.at("tolAbs")) = TOLERANCE * 1.0e-2;
         boost::get<double>(stepperOptions.at("tolRel")) = TOLERANCE * 1.0e-2;
     }
+    {
+        GenericConfig & telemetryOptions = boost::get<GenericConfig>(simuOptions.at("telemetry"));
+        boost::get<bool>(telemetryOptions.at("enableEnergy")) = true;
+    }
     engine.setOptions(simuOptions);
 
     // Run simulation
@@ -134,6 +138,10 @@ TEST(EngineSanity, EnergyConservation)
         boost::get<double>(stepperOptions.at("sensorsUpdatePeriod")) = 1.0e-3;
         boost::get<double>(stepperOptions.at("controllerUpdatePeriod")) = 1.0e-3;
     }
+    {
+        GenericConfig & telemetryOptions = boost::get<GenericConfig>(simuOptions.at("telemetry"));
+        boost::get<bool>(telemetryOptions.at("enableEnergy")) = true;
+    }
     engine.setOptions(simuOptions);
 
     // Run simulation

core/unit/model_test.cc

@@ -30,11 +30,12 @@ TEST_P(ModelTestFixture, CreateFlexible)
     model->initialize(urdfPath, hasFreeflyer, std::vector<std::string>(), true);
 
     // We now have a rigid robot: perform rigid computation on this
-    auto q = pinocchio::randomConfiguration(model->pinocchioModel_);
     if (hasFreeflyer)
     {
-        q.head<3>().setZero();
+        model->pinocchioModel_.lowerPositionLimit.head<3>().setZero();
+        model->pinocchioModel_.upperPositionLimit.head<3>().setZero();
     }
+    auto q = pinocchio::randomConfiguration(model->pinocchioModel_);
     auto pinocchioData = pinocchio::Data(model->pinocchioModel_);
     pinocchio::framesForwardKinematics(model->pinocchioModel_, pinocchioData, q);
 

python/jiminy_py/unit_py/utilities.py

@@ -52,7 +52,7 @@ def load_urdf_default(urdf_name: str,
     motor_options = robot_options["motors"]
     for motor in robot.motors:
         motor_options[motor.name]["enableVelocityLimit"] = False
-        motor_options[motor.name]['enableCommandLimit'] = False
+        motor_options[motor.name]['enableEffortLimit'] = False
         motor_options[motor.name]['enableArmature'] = False
     robot.set_options(robot_options)