Skip to content

Commit

Permalink
Merge branch 'gazebosim:gz-sim7' into apply_force_torque
Browse files Browse the repository at this point in the history
  • Loading branch information
@Henrique-BO
Henrique-BO committed Jun 14, 2023
2 parents 5d63bdf + 064f69e commit 5af8dfc
Show file tree
Hide file tree
Showing 4 changed files with 187 additions and 2 deletions.
42 changes: 42 additions & 0 deletions src/LevelManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <sdf/Actor.hh>
#include <sdf/Atmosphere.hh>
#include <sdf/Joint.hh>
#include <sdf/Light.hh>
#include <sdf/Model.hh>
#include <sdf/World.hh>
Expand All @@ -35,6 +36,7 @@
#include "gz/sim/components/Atmosphere.hh"
#include "gz/sim/components/Geometry.hh"
#include "gz/sim/components/Gravity.hh"
#include "gz/sim/components/Joint.hh"
#include "gz/sim/components/Level.hh"
#include "gz/sim/components/Model.hh"
#include "gz/sim/components/Light.hh"
Expand Down Expand Up @@ -508,6 +510,21 @@ void LevelManager::ConfigureDefaultLevel()
entityNamesInDefault.insert(light->Name());
}
}

// Joints
// We assume no performers are joints
for (uint64_t jointIndex = 0;
jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex)
{
auto joint = this->runner->sdfWorld->JointByIndex(jointIndex);

if (this->entityNamesInLevels.find(joint->Name()) ==
this->entityNamesInLevels.end())
{
entityNamesInDefault.insert(joint->Name());
}
}

// Components
this->runner->entityCompMgr.CreateComponent(
defaultLevel, components::Level());
Expand Down Expand Up @@ -847,6 +864,20 @@ void LevelManager::LoadActiveEntities(const std::set<std::string> &_namesToLoad)
}
}

// Joints
for (uint64_t jointIndex = 0;
jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex)
{
auto joint = this->runner->sdfWorld->JointByIndex(jointIndex);
if (_namesToLoad.find(joint->Name()) != _namesToLoad.end())
{
Entity jointEntity = this->entityCreator->CreateEntities(joint);

this->entityCreator->SetParent(jointEntity, this->worldEntity);
}
}


this->activeEntityNames.insert(_namesToLoad.begin(), _namesToLoad.end());
}

Expand Down Expand Up @@ -887,6 +918,17 @@ void LevelManager::UnloadInactiveEntities(
return true;
});

this->runner->entityCompMgr.Each<components::Joint, components::Name>(
[&](const Entity &_entity, const components::Joint *,
const components::Name *_name) -> bool
{
if (_namesToUnload.find(_name->Data()) != _namesToUnload.end())
{
this->entityCreator->RequestRemoveEntity(_entity, true);
}
return true;
});

for (const auto &name : _namesToUnload)
{
this->activeEntityNames.erase(name);
Expand Down
19 changes: 18 additions & 1 deletion src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -623,6 +623,12 @@ class gz::sim::systems::PhysicsPrivate
MinimumFeatureList,
physics::sdf::ConstructSdfNestedModel>{};

//////////////////////////////////////////////////
// World models (used for world joints)
public: struct WorldModelFeatureList : physics::FeatureList<
MinimumFeatureList,
physics::WorldModelFeature>{};

//////////////////////////////////////////////////
/// \brief World EntityFeatureMap
public: using WorldEntityMap = EntityFeatureMap3d<
Expand All @@ -633,7 +639,9 @@ class gz::sim::systems::PhysicsPrivate
SetContactPropertiesCallbackFeatureList,
NestedModelFeatureList,
CollisionDetectorFeatureList,
SolverFeatureList>;
SolverFeatureList,
WorldModelFeatureList
>;

/// \brief A map between world entity ids in the ECM to World Entities in
/// gz-physics.
Expand Down Expand Up @@ -1035,6 +1043,15 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm,
}
}

// World Model proxy (used for joints directly under <world> in SDF)
auto worldModelFeature =
this->entityWorldMap.EntityCast<WorldModelFeatureList>(_entity);
if (worldModelFeature)
{
auto modelPtrPhys = worldModelFeature->GetWorldModel();
this->entityModelMap.AddEntity(_entity, modelPtrPhys);
}

return true;
});
}
Expand Down
84 changes: 83 additions & 1 deletion test/integration/physics_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,6 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld)
server.Run(true, 1, false);
EXPECT_FALSE(server.Running());
}
// TODO(addisu) add useful EXPECT calls
}

////////////////////////////////////////////////
Expand Down Expand Up @@ -2539,3 +2538,86 @@ TEST_F(PhysicsSystemFixture,
<< "World Angular Velocity should be increasing.";
}
}

//////////////////////////////////////////////////
/// Tests that joints inside <world> are supported by computing the position of
/// a pendulum bob from the joint angle. This also tests that commands such as
/// JointPositionReset work as expected.
TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld))
{
ServerConfig serverConfig;

const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds",
"joints_in_world.sdf");

serverConfig.SetSdfFile(sdfFile);
Server server(serverConfig);
server.SetUpdatePeriod(1ns);

const int kIters = 1000;
test::Relay testSystem;
testSystem.OnPreUpdate(
[&](const UpdateInfo &_info, EntityComponentManager &_ecm)
{
_ecm.EachNew<components::Joint, components::ParentEntity>(
[&](const Entity &_entity, const components::Joint *,
const components::ParentEntity *) -> bool
{
enableComponent<components::JointPosition>(_ecm, _entity);
enableComponent<components::JointVelocity>(_ecm, _entity);
return true;
});

if (_info.iterations == kIters / 2)
{
// After half the iterations, reset the joint position and velocity so
// that the bob at its equilibrium point and at rest.
auto jointEntity = _ecm.EntityByComponents(components::Name("j1"),
components::Joint());
ASSERT_NE(jointEntity, kNullEntity);
_ecm.SetComponentData<components::JointVelocityReset>(jointEntity,
{0});
_ecm.SetComponentData<components::JointPositionReset>(jointEntity,
{GZ_PI_2});
}
});
testSystem.OnPostUpdate(
[&](const UpdateInfo &_info, const EntityComponentManager &_ecm)
{
auto jointEntity = _ecm.EntityByComponents(components::Name("j1"),
components::Joint());
ASSERT_NE(jointEntity, kNullEntity);

auto m2Entity = _ecm.EntityByComponents(components::Name("m2"),
components::Model());
ASSERT_NE(m2Entity, kNullEntity);

math::Pose3d m2Pose = worldPose(m2Entity, _ecm);

auto jointPos =
_ecm.ComponentData<components::JointPosition>(jointEntity);
ASSERT_TRUE(jointPos.has_value());
ASSERT_EQ(1u, jointPos->size());

if (_info.iterations < kIters / 2)
{
// If the joint is properly working, the position of the model can be
// determined from the joint angle with the equations:
// x = L*cos(θ)
// z = -L*sin(θ)
// where L is the length of the model from the pivot (2m for this
// test).
const double theta = (*jointPos)[0];
const double kLength = 2.0;
EXPECT_NEAR(m2Pose.X(), kLength * cos(theta), 1e-2);
EXPECT_NEAR(m2Pose.Z(), -kLength * sin(theta), 1e-2);
}
else
{
EXPECT_NEAR((*jointPos)[0], GZ_PI_2, 1e-2);
}
});

server.AddSystem(testSystem.systemPtr);
server.Run(true, 1000, false);
}
44 changes: 44 additions & 0 deletions test/worlds/joints_in_world.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
<?xml version="1.0" ?>
<sdf version="1.10">
<world name="joints_in_world">
<joint name="fix_to_world" type="fixed">
<parent>world</parent>
<child>m1</child>
</joint>

<model name="m1">
<link name="link1">
<visual name="visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</visual>
</link>
</model>

<joint name="j1" type="revolute">
<pose relative_to="m1"/>
<parent>m1</parent>
<child>m2</child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>

<model name="m2">
<pose>2 0 0 0 0 0</pose>
<link name="link2">
<visual name="visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</visual>
</link>
</model>

</world>
</sdf>

0 comments on commit 5af8dfc

Please sign in to comment.