Skip to content

Test case to check if velocity limits are applied to joints #1445

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

Merged
merged 15 commits into from
May 4, 2022
Merged

Test case to check if velocity limits are applied to joints #1445

Show file tree
Hide file tree
Changes from 13 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
18c7d93
Added test case
adityapande-1995 Apr 13, 2022
80cc9d3
World file cleanup
adityapande-1995 Apr 13, 2022
c5ac0a8
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 Apr 13, 2022
3a25435
Fixed compiler warning in perfect_comms.cc, use EXPECT_LT in test
adityapande-1995 Apr 14, 2022
f231a8a
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 Apr 14, 2022
6768f59
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 Apr 21, 2022
ff5012c
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 Apr 27, 2022
de795fe
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 May 3, 2022
0710aab
Minor refactor
adityapande-1995 May 3, 2022
fd89170
Changed 1.0 to 1
adityapande-1995 May 3, 2022
464d014
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 May 3, 2022
3c229c4
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
scpeters May 3, 2022
7bdc57c
Refactor expectation and add tolerance
scpeters May 3, 2022
bf37511
Merge branch 'ign-gazebo6' into aditya/vel_limit_test
adityapande-1995 May 4, 2022
5d56b19
Added comment to exlain velocity limits, fixed linting errors in scen…
adityapande-1995 May 4, 2022
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
65 changes: 65 additions & 0 deletions test/integration/physics_system.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2196,3 +2196,68 @@ TEST_F(PhysicsSystemFixture,
EXPECT_NEAR(0.0, wrench.torque().z(), 1e-3);
}
}

/////////////////////////////////////////////////
// Test that joint velocity limit is applied
TEST_F(PhysicsSystemFixtureWithDart6_10,
IGN_UTILS_TEST_DISABLED_ON_WIN32(JointVelocityLimitTest))
{
// Start server
ServerConfig serverConfig;
const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/joint_velocity_limit.sdf";
serverConfig.SetSdfFile(sdfFile);

auto server = std::make_unique<Server>(serverConfig);
EXPECT_FALSE(server->Running());
EXPECT_FALSE(*server->Running(0));

server->SetUpdatePeriod(1ns);

test::Relay testSystem;

const std::size_t nIters{600};
testSystem.OnPreUpdate(
[&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm)
{
// Create components, if they don't exist on the first iteration
if (_info.iterations == 1)
{
for (const auto &e : _ecm.EntitiesByComponents(components::Joint()))
{
if (!_ecm.Component<components::JointVelocity>(e))
{
_ecm.CreateComponent(e, components::JointVelocity());
}
}
}
});

testSystem.OnPostUpdate(
[&](const gazebo::UpdateInfo &_info,
const gazebo::EntityComponentManager &_ecm)
{
// At nIters iterations, check angular velocity of each of the joints
if (_info.iterations == nIters)
{
int count = 0;
for (const auto &e : _ecm.EntitiesByComponents(components::Joint()))
{
auto *jointVel = _ecm.Component<components::JointVelocity>(e);
EXPECT_NE(nullptr, jointVel);
EXPECT_FALSE(jointVel->Data().empty());
if (jointVel->Data().size() > 0)
{
++count;
const double kVelocityLimit = 1.0;
const double kTolerance = 1e-6;
EXPECT_NEAR(jointVel->Data()[0], 0, kVelocityLimit + kTolerance);
}
}
EXPECT_EQ(count, 2);
}
});

server->AddSystem(testSystem.systemPtr);
server->Run(true, nIters, false);
}
265 changes: 265 additions & 0 deletions test/worlds/joint_velocity_limit.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,265 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="double_pendulum">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</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="double_pendulum_with_base">
<pose>3 0 0 0 0 0</pose>
<link name="base">
<inertial>
<mass>100</mass>
</inertial>
<visual name="vis_plate_on_ground">
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</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>
<visual name="vis_pole">
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</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>
<collision name="col_plate_on_ground">
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</geometry>
</collision>
<collision name="col_pole">
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</geometry>
</collision>
</link>
<!-- upper link, length 1, IC -90 degrees -->
<link name="upper_link">
<pose>0 0 2.1 -1.5708 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
</inertial>
<visual name="vis_upper_joint">
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</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>
<visual name="vis_lower_joint">
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</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>
<visual name="vis_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</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>
<collision name="col_upper_joint">
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name="col_lower_joint">
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
<collision name="col_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- lower link, length 1, IC ~-120 degrees more -->
<link name="lower_link">
<pose>0.25 1.0 2.1 -2 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
</inertial>
<visual name="vis_lower_joint">
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</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>
<visual name="vis_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</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>
<collision name="col_lower_joint">
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name="col_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- pin joint for upper link, at origin of upper link -->
<joint name="upper_joint" type="revolute">
<parent>base</parent>
<child>upper_link</child>
<axis>
<xyz>1.0 0 0</xyz>
<limit>
<velocity>1</velocity>
</limit>
</axis>
</joint>
<!-- pin joint for lower link, at origin of child link -->
<joint name="lower_joint" type="revolute">
<parent>upper_link</parent>
<child>lower_link</child>
<axis>
<xyz>1.0 0 0</xyz>
<limit>
<velocity>1</velocity>
</limit>
</axis>
</joint>

</model>

</world>
</sdf>