diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index fbb5a986d0..5f7ca8a8a8 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/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("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; } ////////////////////////////////////////////////// diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index 6c6a6c91b2..3dcaeac3f7 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -81,6 +81,9 @@ namespace systems /// `` If you wish to listen on a non-default topic you may specify it /// here, otherwise the controller defaults to listening on /// "/model//joint///cmd_pos". + /// + /// `` Initial position of a joint. Optional parameter. + /// The default value is 0. class JointPositionController : public System, public ISystemConfigure, diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 1068c21174..7e6f4e4afa 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/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( diff --git a/test/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf index f730f6069e..b7f5fdc774 100644 --- a/test/worlds/joint_position_controller_velocity.sdf +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -112,6 +112,7 @@ j1 true 1000 + -2.0