diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 2c6e0094a60..02c87294054 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -103,8 +103,10 @@ void JointController::Configure(const Entity &_entity, { ignerr << "Joint with name[" << this->dataPtr->jointName << "] not found. " << "The JointController will not control this joint.\n"; + return; } + validConfig = true; if (_sdf->HasElement("initial_velocity")) { this->dataPtr->jointVelCmd = _sdf->Get("initial_velocity"); @@ -185,17 +187,10 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { ignwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - // If the joint hasn't been identified yet, look for it - if (this->dataPtr->jointEntity == kNullEntity) - { - this->dataPtr->jointEntity = - this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); + << "s]. System will not work properly." << std::endl; } - if (this->dataPtr->jointEntity == kNullEntity) + if (!validConfig) return; // Nothing left to do if paused. diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh index d3a1fb65876..76c62679270 100644 --- a/src/systems/joint_controller/JointController.hh +++ b/src/systems/joint_controller/JointController.hh @@ -98,6 +98,9 @@ namespace systems /// \brief Private data pointer private: std::unique_ptr dataPtr; + + /// \brief Private tells us if the configuration is considered valid + bool validConfig = false; }; } }