From 61d4b0701bf4cbe9ec725d2de650c6316016bd94 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sat, 1 Apr 2023 01:02:48 -0400 Subject: [PATCH 1/3] Actuators message for JointPositionController. Allows for position control of joints with actuators msg. Signed-off-by: Benjamin Perseghetti --- .../JointPositionController.cc | 77 ++++++++++++++++++- 1 file changed, 74 insertions(+), 3 deletions(-) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 270ab83027..b0539fc7a0 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -18,6 +18,7 @@ #include "JointPositionController.hh" +#include #include #include @@ -29,6 +30,7 @@ #include #include +#include "gz/sim/components/Actuators.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -46,6 +48,10 @@ class gz::sim::systems::JointPositionControllerPrivate /// \param[in] _msg Position message public: void OnCmdPos(const msgs::Double &_msg); + /// \brief Callback for actuator position subscription + /// \param[in] _msg Position message + public: void OnActuatorPos(const msgs::Actuators &_msg); + /// \brief Gazebo communication node. public: transport::Node node; @@ -58,12 +64,18 @@ class gz::sim::systems::JointPositionControllerPrivate /// \brief Commanded joint position public: double jointPosCmd{0.0}; + /// \brief Index of position actuator. + public: int actuatorNumber = 0; + /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; /// \brief Model interface public: Model model{kNullEntity}; + /// \brief True if using Actuator msg to control joint position. + public: bool useActuatorMsg{false}; + /// \brief Position PID controller. public: math::PID posPid; @@ -190,9 +202,26 @@ void JointPositionController::Configure(const Entity &_entity, this->dataPtr->jointPosCmd = _sdf->Get("initial_position"); } + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) + { + if (_sdf->HasElement("actuatorNumber")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuatorNumber"); + this->dataPtr->useActuatorMsg = true; + } + else + { + gzerr << "Please specify an actuatorNumber" << + "to use Actuator position message control." << std::endl; + } + } + // Subscribe to commands std::string topic; - if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))) + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (!this->dataPtr->useActuatorMsg)) { topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + @@ -206,6 +235,18 @@ void JointPositionController::Configure(const Entity &_entity, return; } } + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (this->dataPtr->useActuatorMsg)) + { + topic = transport::TopicUtils::AsValidTopic("/actuators"); + if (topic.empty()) + { + gzerr << "Failed to create Actuator topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } if (_sdf->HasElement("sub_topic")) { topic = transport::TopicUtils::AsValidTopic("/model/" + @@ -235,8 +276,24 @@ void JointPositionController::Configure(const Entity &_entity, return; } } - this->dataPtr->node.Subscribe( - topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnActuatorPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Actuator messages on [" + << topic << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnCmdPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Double messages on [" + << topic << "]" << std::endl; + } gzdbg << "[JointPositionController] system parameters:" << std::endl; gzdbg << "p_gain: [" << p << "]" << std::endl; @@ -426,6 +483,20 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) this->jointPosCmd = _msg.data(); } +void JointPositionControllerPrivate::OnActuatorPos(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->jointCmdMutex); + if (this->actuatorNumber > _msg.position_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator position array which is of size " + << _msg.position_size() << std::endl; + return; + } + + this->jointPosCmd = static_cast(_msg.position(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(JointPositionController, System, JointPositionController::ISystemConfigure, From 12babc97b19456a206186abf822d4f671c387431 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 10 Apr 2023 15:45:14 -0400 Subject: [PATCH 2/3] Add JointPositionController test for Actuators. Signed-off-by: Benjamin Perseghetti --- .../JointPositionController.hh | 7 + .../joint_position_controller_system.cc | 79 +++++++++++ .../joint_position_controller_actuators.sdf | 123 ++++++++++++++++++ 3 files changed, 209 insertions(+) create mode 100644 test/worlds/joint_position_controller_actuators.sdf diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index dc21602f6b..74cb58e36d 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -52,6 +52,13 @@ namespace systems /// `` Axis of the joint to control. Optional parameter. /// The default value is 0. /// + /// `` True to enable the use of actutor message + /// for position command. Relies on `` for the + /// index of the position actuator and defaults to topic "/actuators". + /// + /// `` used with `` to set + /// the index of the position actuator. + /// /// `` The proportional gain of the PID. Optional parameter. /// The default value is 1. /// diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 757e9588aa..0df56e165f 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -128,6 +129,84 @@ TEST_F(JointPositionControllerTestFixture, EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } +///////////////////////////////////////////////// +// Tests that the JointPositionController accepts joint position commands +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionActuatorsCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_actuators.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j1"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + EXPECT_EQ(_name->Data(), jointName); + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double targetPosition{1.0}; + msgs::Actuators msg; + msg.add_position(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 3000; + server.Run(true, testIters , false); + + EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); +} + ///////////////////////////////////////////////// // Tests that the JointPositionController accepts joint position commands TEST_F(JointPositionControllerTestFixture, diff --git a/test/worlds/joint_position_controller_actuators.sdf b/test/worlds/joint_position_controller_actuators.sdf new file mode 100644 index 0000000000..e65eb698c3 --- /dev/null +++ b/test/worlds/joint_position_controller_actuators.sdf @@ -0,0 +1,123 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + j1 + true + 0 + + + + From b7671af959743aef99bbff667ed7bbec951cd4a7 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 27 Apr 2023 20:09:32 -0400 Subject: [PATCH 3/3] Use actuator_number for JPC. Signed-off-by: Benjamin Perseghetti Co-authored-by: Michael Carroll --- .../joint_position_controller/JointPositionController.cc | 6 +++--- .../joint_position_controller/JointPositionController.hh | 4 ++-- test/worlds/joint_position_controller_actuators.sdf | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index b0539fc7a0..0d0d42d161 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -205,15 +205,15 @@ void JointPositionController::Configure(const Entity &_entity, if (_sdf->HasElement("use_actuator_msg") && _sdf->Get("use_actuator_msg")) { - if (_sdf->HasElement("actuatorNumber")) + if (_sdf->HasElement("actuator_number")) { this->dataPtr->actuatorNumber = - _sdf->Get("actuatorNumber"); + _sdf->Get("actuator_number"); this->dataPtr->useActuatorMsg = true; } 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/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index 74cb58e36d..af139641d7 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -53,10 +53,10 @@ namespace systems /// The default value is 0. /// /// `` True to enable the use of actutor message - /// for position command. Relies on `` for the + /// for position command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// `` used with `` to set /// the index of the position actuator. /// /// `` The proportional gain of the PID. Optional parameter. diff --git a/test/worlds/joint_position_controller_actuators.sdf b/test/worlds/joint_position_controller_actuators.sdf index e65eb698c3..5818a79ec7 100644 --- a/test/worlds/joint_position_controller_actuators.sdf +++ b/test/worlds/joint_position_controller_actuators.sdf @@ -116,7 +116,7 @@ name="gz::sim::systems::JointPositionController"> j1 true - 0 + 0