diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 6211434bc9..d36be2b415 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -45,6 +45,16 @@ using namespace systems; class ignition::gazebo::systems::ThrusterPrivateData { + /// \brief The mode of operation + public: enum OperationMode { + /// \brief Takes in a force commmand and spins the propeller at an + /// appropriate rate. + ForceCmd = 0, + /// \brief Takes in angular velocity commands in radians per second and + /// calculates the appropriate force. + AngVelCmd + } opmode = OperationMode::ForceCmd; + /// \brief Mutex for read/write access to class public: std::mutex mtx; @@ -55,25 +65,28 @@ class ignition::gazebo::systems::ThrusterPrivateData public: double propellerAngVel = 0.0; /// \brief The link entity which will spin - public: ignition::gazebo::Entity linkEntity; + public: Entity linkEntity; /// \brief Axis along which the propeller spins. Expressed in the joint /// frame. Addume this doesn't change during simulation. - public: ignition::math::Vector3d jointAxis; + public: math::Vector3d jointAxis; /// \brief Joint pose in the child link frame. Assume this doesn't change /// during the simulation. public: math::Pose3d jointPose; /// \brief Propeller koint entity - public: ignition::gazebo::Entity jointEntity; + public: Entity jointEntity; /// \brief ignition node for handling transport - public: ignition::transport::Node node; + public: transport::Node node; + + /// \brief Publisher for feedback of data + public: transport::Node::Publisher pub; /// \brief The PID which controls the propeller. This isn't used if /// velocityControl is true. - public: ignition::math::PID propellerController; + public: math::PID propellerController; /// \brief Velocity Control mode - this disables the propellerController /// and writes the angular velocity directly to the joint. default: false @@ -100,10 +113,18 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief callback for handling thrust update public: void OnCmdThrust(const ignition::msgs::Double &_msg); + /// \brief callback for handling angular velocity update + public: void OnCmdAngVel(const ignition::msgs::Double &_msg); + /// \brief function which computes angular velocity from thrust /// \param[in] _thrust Thrust in N /// \return Angular velocity in rad/s public: double ThrustToAngularVec(double _thrust); + + /// \brief function which computers thrust from angular velocity + /// \param[in] _angVel Angular Velocity in rad/s + /// \return Thrust in Newtons + public: double AngularVelToThrust(double _angVel); }; ///////////////////////////////////////////////// @@ -158,6 +179,14 @@ void Thruster::Configure( this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Get the operation mode + if (_sdf->HasElement("use_angvel_cmd")) + { + this->dataPtr->opmode = _sdf->Get("use_angvel_cmd") ? + ThrusterPrivateData::OperationMode::AngVelCmd : + ThrusterPrivateData::OperationMode::ForceCmd; + } + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); if (kNullEntity == this->dataPtr->jointEntity) { @@ -173,34 +202,65 @@ void Thruster::Configure( this->dataPtr->jointPose = _ecm.Component( this->dataPtr->jointEntity)->Data(); - // Keeping cmd_pos for backwards compatibility - // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions - std::string thrusterTopicOld = ignition::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); - - this->dataPtr->node.Subscribe( - thrusterTopicOld, - &ThrusterPrivateData::OnCmdThrust, - this->dataPtr.get()); - - // Subscribe to force commands - std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); - - this->dataPtr->node.Subscribe( - thrusterTopic, - &ThrusterPrivateData::OnCmdThrust, - this->dataPtr.get()); - - ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]" - << std::endl; - // Get link entity auto childLink = _ecm.Component( this->dataPtr->jointEntity); this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data()); + if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) + { + // Keeping cmd_pos for backwards compatibility + // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions + std::string thrusterTopicOld = + ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + + this->dataPtr->node.Subscribe( + thrusterTopicOld, + &ThrusterPrivateData::OnCmdThrust, + this->dataPtr.get()); + + // Subscribe to force commands + std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); + + this->dataPtr->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdThrust, + this->dataPtr.get()); + + ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]" + << std::endl; + + std::string feedbackTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/ang_vel"); + this->dataPtr->pub = this->dataPtr->node.Advertise( + feedbackTopic + ); + } + else + { + igndbg << "Using angular velocity mode" << std::endl; + // Subscribe to angvel commands + std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/cmd_vel"); + + this->dataPtr->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdAngVel, + this->dataPtr.get()); + + ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]" + << std::endl; + + std::string feedbackTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/force"); + this->dataPtr->pub = this->dataPtr->node.Advertise( + feedbackTopic + ); + } + // Create necessary components if not present. enableComponent(_ecm, this->dataPtr->linkEntity); enableComponent(_ecm, @@ -265,6 +325,19 @@ void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) this->propellerAngVel = this->ThrustToAngularVec(this->thrust); } +///////////////////////////////////////////////// +void ThrusterPrivateData::OnCmdAngVel(const ignition::msgs::Double &_msg) +{ + std::lock_guard lock(mtx); + this->propellerAngVel = + ignition::math::clamp(ignition::math::fixnan(_msg.data()), + this->cmdMin, this->cmdMax); + + // Thrust is proportional to the Rotation Rate squared + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 + this->thrust = this->AngularVelToThrust(this->propellerAngVel); +} + ///////////////////////////////////////////////// double ThrusterPrivateData::ThrustToAngularVec(double _thrust) { @@ -280,6 +353,15 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +double ThrusterPrivateData::AngularVelToThrust(double _angVel) +{ + // Thrust is proportional to the Rotation Rate squared + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 + return this->thrustCoefficient * pow(this->propellerDiameter, 4) + * abs(_angVel) * _angVel * this->fluidDensity; +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const ignition::gazebo::UpdateInfo &_info, @@ -307,6 +389,7 @@ void Thruster::PreUpdate( desiredPropellerAngVel = this->dataPtr->propellerAngVel; } + msgs::Double angvel; // PID control double torque = 0.0; if (!this->dataPtr->velocityControl) @@ -318,6 +401,7 @@ void Thruster::PreUpdate( torque = this->dataPtr->propellerController.Update(angularError, _info.dt); } + angvel.set_data(currentAngular); } // Velocity control else @@ -334,8 +418,19 @@ void Thruster::PreUpdate( { velocityComp->Data()[0] = desiredPropellerAngVel; } + angvel.set_data(desiredPropellerAngVel); } + if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) + { + this->dataPtr->pub.Publish(angvel); + } + else + { + msgs::Double force; + force.set_data(desiredThrust); + this->dataPtr->pub.Publish(force); + } // Force: thrust // Torque: propeller rotation, if using PID link.AddWorldWrench( @@ -350,4 +445,3 @@ IGNITION_ADD_PLUGIN( Thruster::ISystemPreUpdate) IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") - diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index ec1a607d21..d62a8c9f1d 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -34,16 +34,28 @@ namespace systems /// \brief This plugin simulates a maritime thruster for /// boats and underwater vehicles. It uses the equations described in Fossen's - /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin takes in + /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin has two + /// modes of operation. In the default mode it takes in a /// force in Newtons and applies it to the thruster. It also calculates the /// theoretical angular velocity of the blades and spins them accordingly. + /// Alternatively, one may send angular velocity commands to calculate the + /// force to be applied using the said equation. In the default mode the + /// plugin will publish angular velocity in radians per second on + /// `/model/{ns}/joint/{joint_name}/ang_vel` as an ignition.msgs.double. If + /// is set to true it publishes force in Newtons instead to + /// `/model/{ns}/joint/{joint_name}/force`. /// /// ## System Parameters /// - - The namespace in which the robot exists. The plugin will - /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`. + /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`or + /// `/model/{namespace}/joint/{joint_name}/cmd_vel` depending on the mode of + /// operation. /// [Optional] /// - - This is the joint in the model which corresponds to the /// propeller. [Required] + /// - - Default false, if set to true will make the thruster + /// plugin accept commands in angular velocity in radians per seconds in + /// terms of newtons. [Optional, Boolean, Default True] /// - - The fluid density of the liquid in which the thruster /// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3] /// - - The diameter of the propeller in meters. diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 591bad2580..e257fb3fc9 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -46,15 +46,19 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \param[in] _density Fluid density /// \param[in] _diameter Propeller diameter /// \param[in] _baseTol Base tolerance for most quantities + /// \param[in] _useAngVelCmd Send commands in angular velocity instead of + /// force + /// \param[in] _mass Mass of the body being propelled. public: void TestWorld(const std::string &_world, const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol); + double _diameter, double _baseTol, bool _useAngVelCmd = false, + double _mass = 100.1); }; ////////////////////////////////////////////////// void ThrusterTest::TestWorld(const std::string &_world, const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol) + double _diameter, double _baseTol, bool _useAngVelCmd, double _mass) { // Start server ServerConfig serverConfig; @@ -121,8 +125,17 @@ void ThrusterTest::TestWorld(const std::string &_world, // Publish command and check that vehicle moved transport::Node node; + std::string cmdTopic; + if (!_useAngVelCmd) + { + cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"; + } + else + { + cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_vel"; + } auto pub = node.Advertise( - "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"); + cmdTopic); int sleep{0}; int maxSleep{30}; @@ -134,8 +147,21 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_TRUE(pub.HasConnections()); double force{300.0}; + + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 + // omega = sqrt(thrust / + // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4))); + msgs::Double msg; - msg.set_data(force); + if(!_useAngVelCmd) + { + msg.set_data(force); + } + else + { + msg.set_data(omega); + } pub.Publish(msg); // Check movement @@ -155,13 +181,12 @@ void ThrusterTest::TestWorld(const std::string &_world, // s = a * t^2 / 2 // F = m * 2 * s / t^2 // s = F * t^2 / 2m - double mass{100.1}; double xTol{1e-2}; for (unsigned int i = 0; i < modelPoses.size(); ++i) { auto pose = modelPoses[i]; auto time = dt * i; - EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol); + EXPECT_NEAR(force * time * time / (2 * _mass), pose.Pos().X(), xTol); EXPECT_NEAR(0.0, pose.Pos().Y(), _baseTol); EXPECT_NEAR(0.0, pose.Pos().Z(), _baseTol); EXPECT_NEAR(0.0, pose.Rot().Pitch(), _baseTol); @@ -175,10 +200,6 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); } - // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 - // omega = sqrt(thrust / - // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4))); double omegaTol{1e-1}; for (unsigned int i = 0; i < propellerAngVels.size(); ++i) { @@ -193,6 +214,16 @@ void ThrusterTest::TestWorld(const std::string &_world, } } +///////////////////////////////////////////////// +TEST_F(ThrusterTest, AngVelCmdControl) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_ang_vel_cmd.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, "custom", 0.005, 950, 0.2, 1e-2, true, 100.01); +} + ///////////////////////////////////////////////// TEST_F(ThrusterTest, PIDControl) { diff --git a/test/worlds/thruster_ang_vel_cmd.sdf b/test/worlds/thruster_ang_vel_cmd.sdf new file mode 100644 index 0000000000..c345259735 --- /dev/null +++ b/test/worlds/thruster_ang_vel_cmd.sdf @@ -0,0 +1,111 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.0000354167 + 0 + 0 + 0.0000021667 + 0 + 0.0000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + custom + propeller_joint + 0.005 + 950 + 0.2 + true + true + + + + +