Skip to content

Commit 7221109

Browse files
bperseghettimjcarroll
bperseghetti
and
mjcarroll
authored
Actuators message for JointPositionController. (#1954)
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com> Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
1 parent 568410a commit 7221109

File tree

4 files changed

+283
-3
lines changed

4 files changed

+283
-3
lines changed

‎src/systems/joint_position_controller/JointPositionController.cc

Lines changed: 74 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
#include "JointPositionController.hh"
2020

21+
#include <gz/msgs/actuators.pb.h>
2122
#include <gz/msgs/double.pb.h>
2223

2324
#include <string>
@@ -29,6 +30,7 @@
2930
#include <gz/plugin/Register.hh>
3031
#include <gz/transport/Node.hh>
3132

33+
#include "gz/sim/components/Actuators.hh"
3234
#include "gz/sim/components/Joint.hh"
3335
#include "gz/sim/components/JointForceCmd.hh"
3436
#include "gz/sim/components/JointVelocityCmd.hh"
@@ -46,6 +48,10 @@ class gz::sim::systems::JointPositionControllerPrivate
4648
/// \param[in] _msg Position message
4749
public: void OnCmdPos(const msgs::Double &_msg);
4850

51+
/// \brief Callback for actuator position subscription
52+
/// \param[in] _msg Position message
53+
public: void OnActuatorPos(const msgs::Actuators &_msg);
54+
4955
/// \brief Gazebo communication node.
5056
public: transport::Node node;
5157

@@ -58,12 +64,18 @@ class gz::sim::systems::JointPositionControllerPrivate
5864
/// \brief Commanded joint position
5965
public: double jointPosCmd{0.0};
6066

67+
/// \brief Index of position actuator.
68+
public: int actuatorNumber = 0;
69+
6170
/// \brief mutex to protect joint commands
6271
public: std::mutex jointCmdMutex;
6372

6473
/// \brief Model interface
6574
public: Model model{kNullEntity};
6675

76+
/// \brief True if using Actuator msg to control joint position.
77+
public: bool useActuatorMsg{false};
78+
6779
/// \brief Position PID controller.
6880
public: math::PID posPid;
6981

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

205+
if (_sdf->HasElement("use_actuator_msg") &&
206+
_sdf->Get<bool>("use_actuator_msg"))
207+
{
208+
if (_sdf->HasElement("actuator_number"))
209+
{
210+
this->dataPtr->actuatorNumber =
211+
_sdf->Get<int>("actuator_number");
212+
this->dataPtr->useActuatorMsg = true;
213+
}
214+
else
215+
{
216+
gzerr << "Please specify an actuator_number" <<
217+
"to use Actuator position message control." << std::endl;
218+
}
219+
}
220+
193221
// Subscribe to commands
194222
std::string topic;
195-
if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")))
223+
if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))
224+
&& (!this->dataPtr->useActuatorMsg))
196225
{
197226
topic = transport::TopicUtils::AsValidTopic("/model/" +
198227
this->dataPtr->model.Name(_ecm) + "/joint/" +
@@ -206,6 +235,18 @@ void JointPositionController::Configure(const Entity &_entity,
206235
return;
207236
}
208237
}
238+
if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))
239+
&& (this->dataPtr->useActuatorMsg))
240+
{
241+
topic = transport::TopicUtils::AsValidTopic("/actuators");
242+
if (topic.empty())
243+
{
244+
gzerr << "Failed to create Actuator topic for joint ["
245+
<< this->dataPtr->jointNames[0]
246+
<< "]" << std::endl;
247+
return;
248+
}
249+
}
209250
if (_sdf->HasElement("sub_topic"))
210251
{
211252
topic = transport::TopicUtils::AsValidTopic("/model/" +
@@ -235,8 +276,24 @@ void JointPositionController::Configure(const Entity &_entity,
235276
return;
236277
}
237278
}
238-
this->dataPtr->node.Subscribe(
239-
topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get());
279+
if (this->dataPtr->useActuatorMsg)
280+
{
281+
this->dataPtr->node.Subscribe(topic,
282+
&JointPositionControllerPrivate::OnActuatorPos,
283+
this->dataPtr.get());
284+
285+
gzmsg << "JointPositionController subscribing to Actuator messages on ["
286+
<< topic << "]" << std::endl;
287+
}
288+
else
289+
{
290+
this->dataPtr->node.Subscribe(topic,
291+
&JointPositionControllerPrivate::OnCmdPos,
292+
this->dataPtr.get());
293+
294+
gzmsg << "JointPositionController subscribing to Double messages on ["
295+
<< topic << "]" << std::endl;
296+
}
240297

241298
gzdbg << "[JointPositionController] system parameters:" << std::endl;
242299
gzdbg << "p_gain: [" << p << "]" << std::endl;
@@ -426,6 +483,20 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg)
426483
this->jointPosCmd = _msg.data();
427484
}
428485

486+
void JointPositionControllerPrivate::OnActuatorPos(const msgs::Actuators &_msg)
487+
{
488+
std::lock_guard<std::mutex> lock(this->jointCmdMutex);
489+
if (this->actuatorNumber > _msg.position_size() - 1)
490+
{
491+
gzerr << "You tried to access index " << this->actuatorNumber
492+
<< " of the Actuator position array which is of size "
493+
<< _msg.position_size() << std::endl;
494+
return;
495+
}
496+
497+
this->jointPosCmd = static_cast<double>(_msg.position(this->actuatorNumber));
498+
}
499+
429500
GZ_ADD_PLUGIN(JointPositionController,
430501
System,
431502
JointPositionController::ISystemConfigure,

‎src/systems/joint_position_controller/JointPositionController.hh

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,13 @@ namespace systems
5252
/// `<joint_index>` Axis of the joint to control. Optional parameter.
5353
/// The default value is 0.
5454
///
55+
/// `<use_actuator_msg>` True to enable the use of actutor message
56+
/// for position command. Relies on `<actuator_number>` for the
57+
/// index of the position actuator and defaults to topic "/actuators".
58+
///
59+
/// `<actuator_number>` used with `<use_actuator_commands>` to set
60+
/// the index of the position actuator.
61+
///
5562
/// `<p_gain>` The proportional gain of the PID. Optional parameter.
5663
/// The default value is 1.
5764
///

‎test/integration/joint_position_controller_system.cc

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <gtest/gtest.h>
2020

2121
#include <gz/msgs/double.pb.h>
22+
#include <gz/msgs/actuators.pb.h>
2223

2324
#include <gz/common/Console.hh>
2425
#include <gz/common/Util.hh>
@@ -128,6 +129,84 @@ TEST_F(JointPositionControllerTestFixture,
128129
EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
129130
}
130131

132+
/////////////////////////////////////////////////
133+
// Tests that the JointPositionController accepts joint position commands
134+
// See https://github.com/gazebosim/gz-sim/issues/1175
135+
TEST_F(JointPositionControllerTestFixture,
136+
GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionActuatorsCommand))
137+
{
138+
using namespace std::chrono_literals;
139+
140+
// Start server
141+
ServerConfig serverConfig;
142+
serverConfig.SetSdfFile(common::joinPaths(
143+
PROJECT_SOURCE_PATH, "test", "worlds",
144+
"joint_position_controller_actuators.sdf"));
145+
146+
Server server(serverConfig);
147+
EXPECT_FALSE(server.Running());
148+
EXPECT_FALSE(*server.Running(0));
149+
150+
server.SetUpdatePeriod(0ns);
151+
152+
const std::string jointName = "j1";
153+
154+
test::Relay testSystem;
155+
std::vector<double> currentPosition;
156+
testSystem.OnPreUpdate(
157+
[&](const UpdateInfo &, EntityComponentManager &_ecm)
158+
{
159+
auto joint = _ecm.EntityByComponents(components::Joint(),
160+
components::Name(jointName));
161+
// Create a JointPosition component if it doesn't exist. This signals
162+
// physics system to populate the component
163+
if (nullptr == _ecm.Component<components::JointPosition>(joint))
164+
{
165+
_ecm.CreateComponent(joint, components::JointPosition());
166+
}
167+
});
168+
169+
testSystem.OnPostUpdate([&](const UpdateInfo &,
170+
const EntityComponentManager &_ecm)
171+
{
172+
_ecm.Each<components::Joint, components::Name,
173+
components::JointPosition>(
174+
[&](const Entity &,
175+
const components::Joint *,
176+
const components::Name *_name,
177+
const components::JointPosition *_position) -> bool
178+
{
179+
EXPECT_EQ(_name->Data(), jointName);
180+
currentPosition = _position->Data();
181+
return true;
182+
});
183+
});
184+
185+
server.AddSystem(testSystem.systemPtr);
186+
187+
const std::size_t initIters = 10;
188+
server.Run(true, initIters, false);
189+
EXPECT_NEAR(0, currentPosition.at(0), TOL);
190+
191+
// Publish command and check that the joint position is set
192+
transport::Node node;
193+
auto pub = node.Advertise<msgs::Actuators>(
194+
"/actuators");
195+
196+
const double targetPosition{1.0};
197+
msgs::Actuators msg;
198+
msg.add_position(targetPosition);
199+
200+
pub.Publish(msg);
201+
// Wait for the message to be published
202+
std::this_thread::sleep_for(100ms);
203+
204+
const std::size_t testIters = 3000;
205+
server.Run(true, testIters , false);
206+
207+
EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
208+
}
209+
131210
/////////////////////////////////////////////////
132211
// Tests that the JointPositionController accepts joint position commands
133212
TEST_F(JointPositionControllerTestFixture,

‎test/worlds/joint_position_controller_actuators.sdf

Lines changed: 123 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<world name="default">
4+
<physics name="fast" type="ignored">
5+
<real_time_factor>0</real_time_factor>
6+
</physics>
7+
8+
<plugin
9+
filename="gz-sim-physics-system"
10+
name="gz::sim::systems::Physics">
11+
</plugin>
12+
<plugin
13+
filename="gz-sim-scene-broadcaster-system"
14+
name="gz::sim::systems::SceneBroadcaster">
15+
</plugin>
16+
17+
<model name="ground_plane">
18+
<static>true</static>
19+
<link name="link">
20+
<collision name="collision">
21+
<geometry>
22+
<plane>
23+
<normal>0 0 1</normal>
24+
<size>100 100</size>
25+
</plane>
26+
</geometry>
27+
</collision>
28+
<visual name="visual">
29+
<geometry>
30+
<plane>
31+
<normal>0 0 1</normal>
32+
<size>100 100</size>
33+
</plane>
34+
</geometry>
35+
<material>
36+
<ambient>0.8 0.8 0.8 1</ambient>
37+
<diffuse>0.8 0.8 0.8 1</diffuse>
38+
<specular>0.8 0.8 0.8 1</specular>
39+
</material>
40+
</visual>
41+
</link>
42+
</model>
43+
44+
<model name="joint_position_controller_test">
45+
<pose>0 0 0.005 0 0 0</pose>
46+
<link name="base_link">
47+
<pose>0.0 0.0 0.0 0 0 0</pose>
48+
<inertial>
49+
<inertia>
50+
<ixx>2.501</ixx>
51+
<ixy>0</ixy>
52+
<ixz>0</ixz>
53+
<iyy>2.501</iyy>
54+
<iyz>0</iyz>
55+
<izz>5</izz>
56+
</inertia>
57+
<mass>120.0</mass>
58+
</inertial>
59+
<visual name="base_visual">
60+
<pose>0.0 0.0 0.0 0 0 0</pose>
61+
<geometry>
62+
<box>
63+
<size>0.5 0.5 0.01</size>
64+
</box>
65+
</geometry>
66+
</visual>
67+
<collision name="base_collision">
68+
<pose>0.0 0.0 0.0 0 0 0</pose>
69+
<geometry>
70+
<box>
71+
<size>0.5 0.5 0.01</size>
72+
</box>
73+
</geometry>
74+
</collision>
75+
</link>
76+
<link name="rotor">
77+
<pose>0.0 0.0 1.0 0.0 0 0</pose>
78+
<inertial>
79+
<pose>0.0 0.0 0.0 0 0 0</pose>
80+
<inertia>
81+
<ixx>0.032</ixx>
82+
<ixy>0</ixy>
83+
<ixz>0</ixz>
84+
<iyy>0.032</iyy>
85+
<iyz>0</iyz>
86+
<izz>0.00012</izz>
87+
</inertia>
88+
<mass>0.6</mass>
89+
</inertial>
90+
<visual name="visual">
91+
<geometry>
92+
<box>
93+
<size>0.25 0.25 0.05</size>
94+
</box>
95+
</geometry>
96+
</visual>
97+
<collision name="collision">
98+
<geometry>
99+
<box>
100+
<size>0.25 0.25 0.05</size>
101+
</box>
102+
</geometry>
103+
</collision>
104+
</link>
105+
106+
<joint name="j1" type="revolute">
107+
<pose>0 0 -0.5 0 0 0</pose>
108+
<parent>base_link</parent>
109+
<child>rotor</child>
110+
<axis>
111+
<xyz>0 0 1</xyz>
112+
</axis>
113+
</joint>
114+
<plugin
115+
filename="gz-sim-joint-position-controller-system"
116+
name="gz::sim::systems::JointPositionController">
117+
<joint_name>j1</joint_name>
118+
<use_actuator_msg>true</use_actuator_msg>
119+
<actuator_number>0</actuator_number>
120+
</plugin>
121+
</model>
122+
</world>
123+
</sdf>

0 commit comments

Comments
 (0)