Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Joint Position Controller Behaviour Described in #1997 #2001

Merged
merged 15 commits into from
Aug 3, 2023
Merged

Fix Joint Position Controller Behaviour Described in #1997 #2001

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
ddb62f9
Fix Joint Position Controller Behaviour Described in #1997
arjo129 May 31, 2023
ee304ae
Address PR feedback with regards to style.
arjo129 May 31, 2023
a85fb68
Just one more thing
arjo129 May 31, 2023
2bc513d
style
arjo129 Jun 1, 2023
4c17b45
More minor style fixes
arjo129 Jun 1, 2023
19d70c3
More minor style fixes
arjo129 Jun 1, 2023
47e9474
Tracks periodic changes in scene broadcaster (#2010)
arjo129 Jul 18, 2023
23ed226
Merge branch 'ign-gazebo6' of github.com:gazebosim/gz-sim into arjo/f…
arjo129 Jul 19, 2023
1a88270
Migrate to new header.
arjo129 Jul 19, 2023
bf05a0f
Update test paths
arjo129 Jul 20, 2023
b9030d5
Update include/gz/sim/EntityComponentManager.hh
arjo129 Jul 26, 2023
24533ae
Rename methods
arjo129 Jul 26, 2023
91a758e
style
arjo129 Jul 26, 2023
08298b6
Flipped data structure around
arjo129 Aug 1, 2023
a6e85ba
Fix GCC warning
arjo129 Aug 2, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 6 additions & 2 deletions src/systems/joint_position_controller/JointPositionController.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,9 @@ class ignition::gazebo::systems::JointPositionControllerPrivate
/// \brief mutex to protect joint commands
public: std::mutex jointCmdMutex;

/// \brief Is the maximum PID gain set.
public: bool isMaxSet {false};

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

Expand Down Expand Up @@ -149,6 +152,7 @@ void JointPositionController::Configure(const Entity &_entity,
if (_sdf->HasElement("cmd_max"))
{
cmdMax = _sdf->Get<double>("cmd_max");
this->dataPtr->isMaxSet = true;
}
if (_sdf->HasElement("cmd_min"))
{
Expand Down Expand Up @@ -306,14 +310,14 @@ void JointPositionController::PreUpdate(
auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;

// Limit the maximum change to maxMovement
if (abs(error) > maxMovement)
if (abs(error) > maxMovement && this->dataPtr->isMaxSet)
{
targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() :
-this->dataPtr->posPid.CmdMax();
}
else
{
targetVel = -error;
targetVel = - error / dt;
}

// Set velocity and return
Expand Down
110 changes: 104 additions & 6 deletions test/integration/joint_position_controller_system.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,8 +170,8 @@ TEST_F(JointPositionControllerTestFixture,
const components::Name *_name,
const components::JointPosition *_position) -> bool
{
EXPECT_EQ(_name->Data(), jointName);
currentPosition = _position->Data();
if (_name->Data() == jointName)
currentPosition = _position->Data();
return true;
});
});
Expand All @@ -184,7 +184,7 @@ TEST_F(JointPositionControllerTestFixture,
EXPECT_NEAR(0, currentPosition.at(0), TOL);

// joint moves to initial_position at -2.0
const std::size_t initPosIters = 1000;
const std::size_t initPosIters = 1;
server.Run(true, initPosIters, false);
double expectedInitialPosition = -2.0;
EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL);
Expand All @@ -200,10 +200,108 @@ TEST_F(JointPositionControllerTestFixture,

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

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

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


/////////////////////////////////////////////////
// Tests that the JointPositionController accepts joint position commands
TEST_F(JointPositionControllerTestFixture,
IGN_UTILS_TEST_DISABLED_ON_WIN32(JointPositonVelocityCommandWithMax))
{
using namespace std::chrono_literals;

// Start server
ServerConfig serverConfig;
const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/joint_position_controller_velocity.sdf";
serverConfig.SetSdfFile(sdfFile);

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

server.SetUpdatePeriod(0ns);

const std::string jointName = "j2";

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
{
if(_name->Data() == jointName)
currentPosition = _position->Data();
return true;
});
});

server.AddSystem(testSystem.systemPtr);

// 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 = 2;
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<msgs::Double>(
"/model/joint_position_controller_test_with_max/joint/j2/0/cmd_pos");

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

int sleep{0};
int maxSleep{30};
for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) {
std::this_thread::sleep_for(100ms);
}

pub.Publish(msg);

// Wait for the message to be published
std::this_thread::sleep_for(1ms);

const std::size_t testInitialIters = 1;
server.Run(true, testInitialIters , false);

// We should not have reached our target yet.
EXPECT_GT(fabs(currentPosition.at(0) - targetPosition), TOL);

// Eventually reach target
const std::size_t testIters = 1000;
server.Run(true, testIters , false);
EXPECT_NEAR(currentPosition.at(0), targetPosition, TOL);
}
81 changes: 81 additions & 0 deletions test/worlds/joint_position_controller_velocity.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,87 @@
name="ignition::gazebo::systems::JointPositionController">
<joint_name>j1</joint_name>
<use_velocity_commands>true</use_velocity_commands>
<!--<cmd_max>1000</cmd_max>-->
<initial_position>-2.0</initial_position>
</plugin>
</model>


<model name="joint_position_controller_test_with_max">
<pose>100 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="j2" 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="ignition-gazebo-joint-position-controller-system"
name="ignition::gazebo::systems::JointPositionController">
<joint_name>j2</joint_name>
<use_velocity_commands>true</use_velocity_commands>
<cmd_max>1000</cmd_max>
<initial_position>-2.0</initial_position>
</plugin>
Expand Down