diff --git a/Changelog.md b/Changelog.md index 67303f56cb..0e42ed70e3 100644 --- a/Changelog.md +++ b/Changelog.md @@ -573,6 +573,21 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.14.0 (2021-12-20) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Disable user commands light test on macOS + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + ### Ignition Gazebo 4.13.0 (2021-11-15) 1. Prevent creation of spurious `` elements when saving worlds diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 6211434bc9..885f5ca209 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -79,12 +79,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// and writes the angular velocity directly to the joint. default: false public: bool velocityControl = false; - /// \brief Maximum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Maximum input force [N] for the propellerController, + /// default: 1000N public: double cmdMax = 1000; - /// \brief Minimum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Minimum input force [N] for the propellerController, + /// default: -1000N public: double cmdMin = -1000; /// \brief Thrust coefficient relating the propeller angular velocity to the @@ -206,6 +206,29 @@ void Thruster::Configure( enableComponent(_ecm, this->dataPtr->linkEntity); + double minThrustCmd = this->dataPtr->cmdMin; + double maxThrustCmd = this->dataPtr->cmdMax; + if (_sdf->HasElement("max_thrust_cmd")) + { + maxThrustCmd = _sdf->Get("max_thrust_cmd"); + } + if (_sdf->HasElement("min_thrust_cmd")) + { + minThrustCmd = _sdf->Get("min_thrust_cmd"); + } + if (maxThrustCmd < minThrustCmd) + { + ignerr << " must be greater than or equal to " + << ". Revert to using default values: " + << "min: " << this->dataPtr->cmdMin << ", " + << "max: " << this->dataPtr->cmdMax << std::endl; + } + else + { + this->dataPtr->cmdMax = maxThrustCmd; + this->dataPtr->cmdMin = minThrustCmd; + } + if (_sdf->HasElement("velocity_control")) { this->dataPtr->velocityControl = _sdf->Get("velocity_control"); diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index ec1a607d21..78eafb849b 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -64,6 +64,10 @@ namespace systems /// no units, defaults to 0.0] /// - - Derivative gain for joint PID controller. [Optional, /// no units, defaults to 0.0] + /// - - Maximum thrust command. [Optional, + /// defaults to 1000N] + /// - - Minimum thrust command. [Optional, + /// defaults to -1000N] /// /// ## Example /// An example configuration is installed with Gazebo. The example diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index af1ec159b8..d2333bc3f1 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); - double force{300.0}; + // input force cmd - this should be capped to 0 + double forceCmd{-1000.0}; msgs::Double msg; - msg.set_data(force); + msg.set_data(forceCmd); + pub.Publish(msg); + + // Check no movement + fixture.Server()->Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); + EXPECT_EQ(100u, modelPoses.size()); + EXPECT_EQ(100u, propellerAngVels.size()); + modelPoses.clear(); + propellerAngVels.clear(); + + // input force cmd this should be capped to 300 + forceCmd = 1000.0; + msg.set_data(forceCmd); pub.Publish(msg); // Check movement @@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + // max allowed force + double force{300.0}; + // F = m * a // s = a * t^2 / 2 // F = m * 2 * s / t^2 diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf index c18becc7d0..540c73170b 100644 --- a/test/worlds/thruster_pid.sdf +++ b/test/worlds/thruster_pid.sdf @@ -104,6 +104,8 @@ 0.004 1000 0.2 + 300 + 0 diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf index f78236a8ca..1850eac8f4 100644 --- a/test/worlds/thruster_vel_cmd.sdf +++ b/test/worlds/thruster_vel_cmd.sdf @@ -103,6 +103,8 @@ 950 0.25 true + 300 + 0