Skip to content

Commit

Permalink
Thruster plugin: accept angular velocity and provide feedback on topic (
Browse files Browse the repository at this point in the history 
#1269)

Signed-off-by: Arjo Chakravarty <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
@arjo129 @chapulina
arjo129 and chapulina authored Jan 18, 2022
1 parent 15ed280 commit cfb83a2
Show file tree
Hide file tree
Showing 4 changed files with 288 additions and 40 deletions.
150 changes: 122 additions & 28 deletions src/systems/thruster/Thruster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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
Expand All @@ -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);
};

/////////////////////////////////////////////////
Expand Down Expand Up @@ -158,6 +179,14 @@ void Thruster::Configure(
this->dataPtr->fluidDensity = _sdf->Get<double>("fluid_density");
}

// Get the operation mode
if (_sdf->HasElement("use_angvel_cmd"))
{
this->dataPtr->opmode = _sdf->Get<bool>("use_angvel_cmd") ?
ThrusterPrivateData::OperationMode::AngVelCmd :
ThrusterPrivateData::OperationMode::ForceCmd;
}

this->dataPtr->jointEntity = model.JointByName(_ecm, jointName);
if (kNullEntity == this->dataPtr->jointEntity)
{
Expand All @@ -173,34 +202,65 @@ void Thruster::Configure(
this->dataPtr->jointPose = _ecm.Component<components::Pose>(
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<ignition::gazebo::components::ChildLinkName>(
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<msgs::Double>(
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<msgs::Double>(
feedbackTopic
);
}

// Create necessary components if not present.
enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->linkEntity);
enableComponent<components::WorldAngularVelocity>(_ecm,
Expand Down Expand Up @@ -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<std::mutex> 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)
{
Expand All @@ -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,
Expand Down Expand Up @@ -307,6 +389,7 @@ void Thruster::PreUpdate(
desiredPropellerAngVel = this->dataPtr->propellerAngVel;
}

msgs::Double angvel;
// PID control
double torque = 0.0;
if (!this->dataPtr->velocityControl)
Expand All @@ -318,6 +401,7 @@ void Thruster::PreUpdate(
torque = this->dataPtr->propellerController.Update(angularError,
_info.dt);
}
angvel.set_data(currentAngular);
}
// Velocity control
else
Expand All @@ -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(
Expand All @@ -350,4 +445,3 @@ IGNITION_ADD_PLUGIN(
Thruster::ISystemPreUpdate)

IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster")

16 changes: 14 additions & 2 deletions src/systems/thruster/Thruster.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
/// <use_angvel_cmd> is set to true it publishes force in Newtons instead to
/// `/model/{ns}/joint/{joint_name}/force`.
///
/// ## System Parameters
/// - <namespace> - 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]
/// - <joint_name> - This is the joint in the model which corresponds to the
/// propeller. [Required]
/// - <use_angvel_cmd> - 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]
/// - <fluid_density> - The fluid density of the liquid in which the thruster
/// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3]
/// - <propeller_diameter> - The diameter of the propeller in meters.
Expand Down
51 changes: 41 additions & 10 deletions test/integration/thruster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<msgs::Double>(
"/model/" + _namespace + "/joint/propeller_joint/cmd_thrust");
cmdTopic);

int sleep{0};
int maxSleep{30};
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand Down
Loading

0 comments on commit cfb83a2

Please sign in to comment.