Skip to content

Commit

Permalink
Actuators message for JointPositionController. (#1954)
Browse files Browse the repository at this point in the history 
Signed-off-by: Benjamin Perseghetti <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>
  • Loading branch information
@bperseghetti @mjcarroll
bperseghetti and mjcarroll authored May 1, 2023
1 parent 568410a commit 7221109
Show file tree
Hide file tree
Showing 4 changed files with 283 additions and 3 deletions.
77 changes: 74 additions & 3 deletions src/systems/joint_position_controller/JointPositionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include "JointPositionController.hh"

#include <gz/msgs/actuators.pb.h>
#include <gz/msgs/double.pb.h>

#include <string>
Expand All @@ -29,6 +30,7 @@
#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>

#include "gz/sim/components/Actuators.hh"
#include "gz/sim/components/Joint.hh"
#include "gz/sim/components/JointForceCmd.hh"
#include "gz/sim/components/JointVelocityCmd.hh"
Expand All @@ -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;

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

Expand Down Expand Up @@ -190,9 +202,26 @@ void JointPositionController::Configure(const Entity &_entity,
this->dataPtr->jointPosCmd = _sdf->Get<double>("initial_position");
}

if (_sdf->HasElement("use_actuator_msg") &&
_sdf->Get<bool>("use_actuator_msg"))
{
if (_sdf->HasElement("actuator_number"))
{
this->dataPtr->actuatorNumber =
_sdf->Get<int>("actuator_number");
this->dataPtr->useActuatorMsg = true;
}
else
{
gzerr << "Please specify an actuator_number" <<
"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/" +
Expand All @@ -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/" +
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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<std::mutex> 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<double>(_msg.position(this->actuatorNumber));
}

GZ_ADD_PLUGIN(JointPositionController,
System,
JointPositionController::ISystemConfigure,
Expand Down
7 changes: 7 additions & 0 deletions src/systems/joint_position_controller/JointPositionController.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,13 @@ namespace systems
/// `<joint_index>` Axis of the joint to control. Optional parameter.
/// The default value is 0.
///
/// `<use_actuator_msg>` True to enable the use of actutor message
/// for position command. Relies on `<actuator_number>` for the
/// index of the position actuator and defaults to topic "/actuators".
///
/// `<actuator_number>` used with `<use_actuator_commands>` to set
/// the index of the position actuator.
///
/// `<p_gain>` The proportional gain of the PID. Optional parameter.
/// The default value is 1.
///
Expand Down
79 changes: 79 additions & 0 deletions test/integration/joint_position_controller_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <gtest/gtest.h>

#include <gz/msgs/double.pb.h>
#include <gz/msgs/actuators.pb.h>

#include <gz/common/Console.hh>
#include <gz/common/Util.hh>
Expand Down Expand Up @@ -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<double> 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<components::JointPosition>(joint))
{
_ecm.CreateComponent(joint, components::JointPosition());
}
});

testSystem.OnPostUpdate([&](const UpdateInfo &,
const EntityComponentManager &_ecm)
{
_ecm.Each<components::Joint, components::Name,
components::JointPosition>(
[&](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<msgs::Actuators>(
"/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,
Expand Down
123 changes: 123 additions & 0 deletions test/worlds/joint_position_controller_actuators.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<physics name="fast" type="ignored">
<real_time_factor>0</real_time_factor>
</physics>

<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="joint_position_controller_test">
<pose>0 0 0.005 0 0 0</pose>
<link name="base_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
<inertia>
<ixx>2.501</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2.501</iyy>
<iyz>0</iyz>
<izz>5</izz>
</inertia>
<mass>120.0</mass>
</inertial>
<visual name="base_visual">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.01</size>
</box>
</geometry>
</visual>
<collision name="base_collision">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.01</size>
</box>
</geometry>
</collision>
</link>
<link name="rotor">
<pose>0.0 0.0 1.0 0.0 0 0</pose>
<inertial>
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertia>
<ixx>0.032</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.032</iyy>
<iyz>0</iyz>
<izz>0.00012</izz>
</inertia>
<mass>0.6</mass>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.25 0.25 0.05</size>
</box>
</geometry>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.25 0.25 0.05</size>
</box>
</geometry>
</collision>
</link>

<joint name="j1" type="revolute">
<pose>0 0 -0.5 0 0 0</pose>
<parent>base_link</parent>
<child>rotor</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>j1</joint_name>
<use_actuator_msg>true</use_actuator_msg>
<actuator_number>0</actuator_number>
</plugin>
</model>
</world>
</sdf>

0 comments on commit 7221109

Please sign in to comment.