Update sdf plugins to use actuator_number.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
gazebosim · Apr 28, 2023 · 3c3af7c · 3c3af7c
1 parent 14d5832
commit 3c3af7c
Show file tree

Hide file tree

Showing 9 changed files with 53 additions and 42 deletions.
diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf
@@ -117,7 +117,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
@@ -137,7 +137,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
@@ -157,7 +157,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
@@ -177,7 +177,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
@@ -247,7 +247,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>
@@ -267,7 +267,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>
@@ -287,7 +287,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>
@@ -307,7 +307,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>
@@ -327,7 +327,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>4</motorNumber>
+        <actuator_number>4</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>
@@ -347,7 +347,7 @@ You can use the velocity controller and command linear velocity and yaw angular
         <motorConstant>1.269e-05</motorConstant>
         <momentConstant>0.016754</momentConstant>
         <commandSubTopic>command/motor_speed</commandSubTopic>
-        <motorNumber>5</motorNumber>
+        <actuator_number>5</actuator_number>
         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
         <rotorDragCoefficient>0</rotorDragCoefficient>
         <rollingMomentCoefficient>0</rollingMomentCoefficient>

diff --git a/examples/worlds/quadcopter.sdf b/examples/worlds/quadcopter.sdf
@@ -93,7 +93,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
@@ -113,7 +113,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
@@ -133,7 +133,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
@@ -153,7 +153,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>

diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc
@@ -259,10 +259,10 @@ void AckermannSteering::Configure(const Entity &_entity,
   if (_sdf->HasElement("use_actuator_msg") &&
     _sdf->Get<bool>("use_actuator_msg"))
   {
-    if (_sdf->HasElement("actuatorNumber"))
+    if (_sdf->HasElement("actuator_number"))
     {
       this->dataPtr->actuatorNumber =
-        _sdf->Get<int>("actuatorNumber");
+        _sdf->Get<int>("actuator_number");
       this->dataPtr->useActuatorMsg = true;
       if (!this->dataPtr->steeringOnly)
       {
@@ -271,7 +271,7 @@ void AckermannSteering::Configure(const Entity &_entity,
     }
     else
     {
-      gzerr << "Please specify an actuatorNumber" <<
+      gzerr << "Please specify an actuator_number" <<
         "to use Actuator position message control." << std::endl;
     }
   }

diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh
@@ -45,10 +45,10 @@ namespace systems
   /// `<use_actuator_msg>` is True, uses defaults topic name "/actuators".
   ///
   /// `<use_actuator_msg>` True to enable the use of actutor message
-  /// for steering angle command. Relies on `<actuatorNumber>` for the
+  /// for steering angle command. Relies on `<actuator_number>` for the
   /// index of the position actuator and defaults to topic "/actuators".
   ///
-  /// `<actuatorNumber>` used with `<use_actuator_commands>` to set
+  /// `<actuator_number>` used with `<use_actuator_commands>` to set
   /// the index of the steering angle position actuator.
   ///
   /// `<steer_p_gain>`: Float used to control the steering angle P gain.

diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc
@@ -157,8 +157,8 @@ class gz::sim::systems::MulticopterMotorModelPrivate
   /// \brief Sampling time (from motor_model.hpp).
   public: double samplingTime = 0.01;
 
-  /// \brief Index of motor on multirotor_base.
-  public: int motorNumber = 0;
+  /// \brief Index of motor in Actuators msg on multirotor_base.
+  public: int actuatorNumber = 0;
 
   /// \brief Turning direction of the motor.
   public: int turningDirection = turning_direction::kCw;
@@ -283,11 +283,22 @@ void MulticopterMotorModel::Configure(const Entity &_entity,
     return;
   }
 
-  if (sdfClone->HasElement("motorNumber"))
-    this->dataPtr->motorNumber =
+  if (sdfClone->HasElement("actuator_number"))
+  {
+    this->dataPtr->actuatorNumber =
+      sdfClone->GetElement("actuator_number")->Get<int>();
+  }
+  else if (sdfClone->HasElement("motorNumber"))
+  {
+    this->dataPtr->actuatorNumber =
       sdfClone->GetElement("motorNumber")->Get<int>();
+    gzwarn << "<motorNumber> is depricated, "
+           << "please use <actuator_number>.\n";
+  }
   else
-    gzerr << "Please specify a motorNumber.\n";
+  {
+    gzerr << "Please specify a actuator_number.\n";
+  }
 
   if (sdfClone->HasElement("turningDirection"))
   {
@@ -512,9 +523,9 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments(
 
   if (msg.has_value())
   {
-    if (this->motorNumber > msg->velocity_size() - 1)
+    if (this->actuatorNumber > msg->velocity_size() - 1)
     {
-      gzerr << "You tried to access index " << this->motorNumber
+      gzerr << "You tried to access index " << this->actuatorNumber
         << " of the Actuator velocity array which is of size "
         << msg->velocity_size() << std::endl;
       return;
@@ -523,13 +534,13 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments(
     if (this->motorType == MotorType::kVelocity)
     {
       this->refMotorInput = std::min(
-          static_cast<double>(msg->velocity(this->motorNumber)),
+          static_cast<double>(msg->velocity(this->actuatorNumber)),
           static_cast<double>(this->maxRotVelocity));
     }
     //  else if (this->motorType == MotorType::kPosition)
     else  // if (this->motorType == MotorType::kForce) {
     {
-      this->refMotorInput = msg->velocity(this->motorNumber);
+      this->refMotorInput = msg->velocity(this->actuatorNumber);
     }
   }
 
@@ -554,7 +565,7 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments(
       double motorRotVel = jointVelocity->Data()[0];
       if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime))
       {
-        gzerr << "Aliasing on motor [" << this->motorNumber
+        gzerr << "Aliasing on motor [" << this->actuatorNumber
               << "] might occur. Consider making smaller simulation time "
                  "steps or raising the rotorVelocitySlowdownSim param.\n";
       }

diff --git a/test/worlds/ackermann_steering_only_actuators.sdf b/test/worlds/ackermann_steering_only_actuators.sdf
@@ -365,7 +365,7 @@
         <right_steering_joint>front_right_wheel_steering_joint</right_steering_joint>
         <steering_only>true</steering_only>
         <use_actuator_msg>true</use_actuator_msg>
-        <actuatorNumber>0</actuatorNumber>
+        <actuator_number>0</actuator_number>
         <steer_p_gain>10.0</steer_p_gain>
         <steering_limit>0.5</steering_limit>
         <wheel_base>1.0</wheel_base>

diff --git a/test/worlds/odometry_publisher_3d.sdf b/test/worlds/odometry_publisher_3d.sdf
@@ -273,7 +273,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
@@ -293,7 +293,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
@@ -313,7 +313,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
@@ -333,7 +333,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>

diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf
@@ -262,7 +262,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
@@ -282,7 +282,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
@@ -302,7 +302,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
@@ -322,7 +322,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>

diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf
@@ -262,7 +262,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
+        <actuator_number>0</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
@@ -282,7 +282,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
+        <actuator_number>1</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
@@ -302,7 +302,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
+        <actuator_number>2</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
@@ -322,7 +322,7 @@
         <motorConstant>8.54858e-06</motorConstant>
         <momentConstant>0.016</momentConstant>
         <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
+        <actuator_number>3</actuator_number>
         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>