Add <initial_position> param to JointPositionController system

src/systems/joint_position_controller/JointPositionController.cc

@@ -46,13 +46,13 @@ class ignition::gazebo::systems::JointPositionControllerPrivate
   public: transport::Node node;
 
   /// \brief Joint Entity
-  public: Entity jointEntity;
+  public: Entity jointEntity{kNullEntity};
 
   /// \brief Joint name
   public: std::string jointName;
 
   /// \brief Commanded joint position
-  public: double jointPosCmd;
+  public: double jointPosCmd{0.0};
 
   /// \brief mutex to protect joint commands
   public: std::mutex jointCmdMutex;
@@ -170,6 +170,12 @@ void JointPositionController::Configure(const Entity &_entity,
 
   this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
 
+
+  if (_sdf->HasElement("initial_position"))
+  {
+    this->dataPtr->jointPosCmd = _sdf->Get<double>("initial_position");
+  }
+
   // Subscribe to commands
   std::string topic = transport::TopicUtils::AsValidTopic("/model/" +
       this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
@@ -206,6 +212,8 @@ void JointPositionController::Configure(const Entity &_entity,
   igndbg << "cmd_min: ["    << cmdMin    << "]"            << std::endl;
   igndbg << "cmd_offset: [" << cmdOffset << "]"            << std::endl;
   igndbg << "Topic: ["      << topic     << "]"            << std::endl;
+  igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]"
+         << std::endl;
 }
 
 //////////////////////////////////////////////////

src/systems/joint_position_controller/JointPositionController.hh

@@ -81,6 +81,9 @@ namespace systems
   /// `<topic>` If you wish to listen on a non-default topic you may specify it
   /// here, otherwise the controller defaults to listening on
   /// "/model/<model_name>/joint/<joint_name>/<joint_index>/cmd_pos".
+  ///
+  /// `<initial_position>` Initial position of a joint. Optional parameter.
+  ///  The default value is 0.
   class JointPositionController
       : public System,
         public ISystemConfigure,

test/integration/joint_position_controller_system.cc

@@ -178,10 +178,17 @@ TEST_F(JointPositionControllerTestFixture,
 
   server.AddSystem(testSystem.systemPtr);
 
-  const std::size_t initIters = 10;
+  // joint pos starts at 0
+  const std::size_t initIters = 1;
   server.Run(true, initIters, false);
   EXPECT_NEAR(0, currentPosition.at(0), TOL);
 
+  // joint moves to initial_position at -2.0
+  const std::size_t initPosIters = 1000;
+  server.Run(true, initPosIters, false);
+  double expectedInitialPosition = -2.0;
+  EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL);
+
   // Publish command and check that the joint position is set
   transport::Node node;
   auto pub = node.Advertise<msgs::Double>(

test/worlds/joint_position_controller_velocity.sdf

@@ -112,6 +112,7 @@
         <joint_name>j1</joint_name>
         <use_velocity_commands>true</use_velocity_commands>
         <cmd_max>1000</cmd_max>
+        <initial_position>-2.0</initial_position>
       </plugin>
     </model>
   </world>