Skip to content

Commit

Permalink
JointPosController: support nested joints
Browse files Browse the repository at this point in the history 
- Add test for joint position controller with nested models.
- Update PID gains in example worlds model.

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Jan 27, 2023
1 parent 85c9bae commit 36d8eb2
Show file tree
Hide file tree
Showing 3 changed files with 420 additions and 27 deletions.
56 changes: 31 additions & 25 deletions examples/worlds/nested_model_joint_positions.sdf
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0" ?>
<!--
Run example
gz sim -v4 -r test_nested_model.sdf
gz sim -v4 -r nested_model_joint_positions.sdf
Move rotor in model1
gz topic -t "/model1/cmd_rotor" -m gz.msgs.Double -p "data: 1"
Expand All @@ -20,11 +20,11 @@
gz topic -t "/model42/cmd_rotor" -m gz.msgs.Double -p "data: 1"
-->

<sdf version="1.7">
<sdf version="1.6">
<world name="nested_model_joint_positions">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_factor>0</real_time_factor>
</physics>

<plugin filename="gz-sim-physics-system"
Expand Down Expand Up @@ -56,8 +56,6 @@
<direction>-0.5 0.1 -0.9</direction>
</light>

<!-- <gravity>0 0 0</gravity> -->

<model name="ground_plane">
<static>true</static>
<link name="link">
Expand Down Expand Up @@ -181,7 +179,8 @@
name="gz::sim::systems::JointPositionController">
<joint_name>rotor_joint</joint_name>
<topic>/model1/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
</model>

Expand Down Expand Up @@ -373,11 +372,11 @@
<child>model21::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
<damping>1</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
Expand All @@ -387,14 +386,16 @@
name="gz::sim::systems::JointPositionController">
<joint_name>rotor_joint</joint_name>
<topic>/model2/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>model21::rotor_joint</joint_name>
<topic>/model21/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
</model>

Expand Down Expand Up @@ -675,11 +676,11 @@
<child>model31::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
<damping>1</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
Expand All @@ -689,11 +690,11 @@
<child>model32::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
<damping>1</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
Expand All @@ -703,21 +704,24 @@
name="gz::sim::systems::JointPositionController">
<joint_name>rotor_joint</joint_name>
<topic>/model3/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>model31::rotor_joint</joint_name>
<topic>/model31/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>model32::rotor_joint</joint_name>
<topic>/model32/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
</model>

Expand Down Expand Up @@ -914,11 +918,11 @@
<child>model42::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
<damping>1</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
Expand All @@ -928,14 +932,16 @@
name="gz::sim::systems::JointPositionController">
<joint_name>model41::rotor_joint</joint_name>
<topic>/model41/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>model42::rotor_joint</joint_name>
<topic>/model42/cmd_rotor</topic>
<p_gain>2</p_gain>
<p_gain>30</p_gain>
<i_gain>0.05</i_gain>
</plugin>
</model>

Expand Down
121 changes: 119 additions & 2 deletions test/integration/joint_position_controller_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,13 @@

#include "gz/sim/components/Joint.hh"
#include "gz/sim/components/JointPosition.hh"
#include "gz/sim/components/Model.hh"
#include "gz/sim/components/Name.hh"

#include "gz/sim/Server.hh"
#include "gz/sim/SystemLoader.hh"
#include "gz/sim/Util.hh"

#include "test_config.hh"

#include "../helpers/Relay.hh"
Expand Down Expand Up @@ -128,7 +131,7 @@ TEST_F(JointPositionControllerTestFixture,
/////////////////////////////////////////////////
// Tests that the JointPositionController accepts joint position commands
TEST_F(JointPositionControllerTestFixture,
GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositonVelocityCommand))
GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionVelocityCommand))
{
using namespace std::chrono_literals;

Expand Down Expand Up @@ -208,11 +211,13 @@ TEST_F(JointPositionControllerTestFixture,

EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
}

/////////////////////////////////////////////////
// Tests that the JointPositionController accepts joint position
// sub_topic commands
TEST_F(JointPositionControllerTestFixture,
GZ_UTILS_TEST_DISABLED_ON_WIN32(
JointPositonMultipleJointsSubTopicCommand))
JointPositionMultipleJointsSubTopicCommand))
{
using namespace std::chrono_literals;

Expand Down Expand Up @@ -285,3 +290,115 @@ TEST_F(JointPositionControllerTestFixture,

EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
}

/////////////////////////////////////////////////
// Tests that the JointPositionController commands joints in
// nested models.
TEST_F(JointPositionControllerTestFixture,
GZ_UTILS_TEST_DISABLED_ON_WIN32(
JointPositionNestedModels))
{
using namespace std::chrono_literals;

// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(common::joinPaths(
PROJECT_SOURCE_PATH, "test", "worlds",
"joint_position_controller_nested_models.sdf"));

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.SetUpdatePeriod(0ns);

// model2 contains two joints with the same name, one is nested.
const std::string modelName = "model2";
const std::string jointName = "rotor_joint";
const std::string scopedJointName = "model21::rotor_joint";

test::Relay testSystem;
std::vector<double> joint2Position;
std::vector<double> joint21Position;
testSystem.OnPreUpdate(
[&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm)
{
// Create a JointPosition component if it doesn't exist.
_ecm.Each<components::Joint, components::Name>(
[&](const gz::sim::Entity &_joint,
const components::Joint *,
const components::Name *_name) -> bool
{
if (_name->Data() == jointName)
{
_ecm.CreateComponent(_joint, components::JointPosition());
}
return true;
});
});

testSystem.OnPostUpdate([&](const sim::UpdateInfo &,
const sim::EntityComponentManager &_ecm)
{
// Retrieve the parent model.
Entity model{kNullEntity};
_ecm.Each<components::Model,
components::Name>(
[&](const gz::sim::Entity &_entity,
const components::Model *,
const components::Name *_name) -> bool
{
if (_name->Data() == modelName)
{
model = _entity;
}
return true;
});

// joint2
{
auto entities = entitiesFromScopedName(jointName, _ecm, model);
Entity joint = *entities.begin();
auto posComp = _ecm.Component<components::JointPosition>(joint);
joint2Position = posComp->Data();
}

// joint21
{
auto entities = entitiesFromScopedName(scopedJointName, _ecm, model);
Entity joint = *entities.begin();
auto posComp = _ecm.Component<components::JointPosition>(joint);
joint21Position = posComp->Data();
}
});

server.AddSystem(testSystem.systemPtr);

// joint pos starts at 0
const std::size_t initIters = 1;
server.Run(true, initIters, false);
EXPECT_NEAR(0, joint2Position.at(0), TOL);
EXPECT_NEAR(0, joint21Position.at(0), TOL);

// Publish command and check that the joint position is set
transport::Node node;
auto pub = node.Advertise<msgs::Double>(
"/model21/cmd_rotor");

const double targetPosition{1.0};
msgs::Double msg;
msg.set_data(targetPosition);

pub.Publish(msg);
// Wait for the message to be published
std::this_thread::sleep_for(100ms);

const std::size_t testIters = 1000;
server.Run(true, testIters , false);

// joint2 should not move
EXPECT_NEAR(0, joint2Position.at(0), TOL);

// joint21 should be at target position
EXPECT_NEAR(targetPosition, joint21Position.at(0), TOL);
}
Loading

0 comments on commit 36d8eb2

Please sign in to comment.