From 2b8344789b055a0fa48120e961303b98ef2c464d Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 29 Jun 2022 01:54:20 +0800
Subject: [PATCH 01/11] Allow SDF model to be constructed in a single shot

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 src/systems/physics/Physics.cc | 77 ++++++++++++++++++++++++++++++++--
 1 file changed, 73 insertions(+), 4 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index c19c019117..81034e7046 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1066,8 +1066,17 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
         }
         // TODO(anyone) Don't load models unless they have collisions
 
+
         // Check if parent world / model exists
         sdf::Model model;
+        if (const auto *modelSdfComp =
+            _ecm.Component<components::ModelSdf>(_entity))
+        {
+          model = modelSdfComp->Data();
+        }
+
+        // Component values should override whatever values were put into the
+        // ModelSdf component.
         model.SetName(_name->Data());
         model.SetRawPose(_pose->Data());
         auto staticComp = _ecm.Component<components::Static>(_entity);
@@ -1223,8 +1232,31 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm,
                   << "] not found on model map." << std::endl;
           return true;
         }
+
+        auto basicModelPtrPhys = this->entityModelMap.Get(_parent->Data());
+
+        if (const auto existingLink = basicModelPtrPhys->GetLink(_name->Data()))
+        {
+          // No need to create this link because it was already created when
+          // parsing the model (links in models are required to have unique
+          // names). Instead we will register its existence and move along.
+          this->entityLinkMap.AddEntity(_entity, existingLink);
+          this->topLevelModelMap.insert(
+            std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+          return true;
+        }
+
         auto modelPtrPhys =
-            this->entityModelMap.Get(_parent->Data());
+            this->entityModelMap
+              .EntityCast<ConstructSdfLinkFeatureList>(_parent->Data());
+
+        if (!modelPtrPhys)
+        {
+          gzwarn << "Cannot create a new link [" << _name->Data() << "] "
+                 << "because the physics engine plugin does not support "
+                 << "link construction during runtime" << std::endl;
+          return true;
+        }
 
         sdf::Link link;
         link.SetName(_name->Data());
@@ -1312,6 +1344,29 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm,
         }
         auto linkPtrPhys = this->entityLinkMap.Get(_parent->Data());
 
+        if (const auto existingShape = linkPtrPhys->GetShape(_name->Data()))
+        {
+          // No need to create this collision shape because it was already
+          // created when parsing the model.
+
+          // TODO(MXG): There's some significant design inconsistency in this
+          // CreateCollisionEntities function. There's a mix of using the
+          // ConstructSdfCollision feature and other collision attachment
+          // features like AttachHeightMapFeature and AttachMeshShape. This
+          // makes it confusing for physics plugin implementers to know where
+          // and how to implement the construction of collision shapes.
+          //
+          // In the current implementation, if a physics engine implements
+          // constructing a mesh or heightmap in ConstructSdfCollision then that
+          // ability won't actually get leveraged by gz-sim because it
+          // specifically uses AttachMeshShapeFeature and AttachHeightMapFeature
+          // for those shapes. The physics plugin implementer needs to know to
+          // implement those specific features instead.
+          this->topLevelModelMap.insert(
+            std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+          return true;
+        }
+
         // Make a copy of the collision DOM so we can set its pose which has
         // been resolved and is now expressed w.r.t the parent link of the
         // collision.
@@ -1601,12 +1656,16 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                   << "] not found on model map." << std::endl;
           return true;
         }
-        auto modelPtrPhys = this->entityModelMap.Get(_parentModel->Data());
 
+<<<<<<< HEAD
         auto modelJointFeature =
             this->entityModelMap.EntityCast<ConstructSdfJointFeatureList>(
+=======
+        auto modelPtrPhys =
+            this->entityModelMap.EntityCast<JointFeatureList>(
+>>>>>>> e578c37f7 (Allow SDF model to be constructed in a single shot)
                 _parentModel->Data());
-        if (!modelJointFeature)
+        if (!modelPtrPhys)
         {
           static bool informed{false};
           if (!informed)
@@ -1621,6 +1680,16 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           return false;
         }
 
+        if (const auto existingJoint = modelPtrPhys->GetJoint(_name->Data()))
+        {
+          // No need to create this joint because it was already created when
+          // parsing the model.
+          this->entityJointMap.AddEntity(_entity, existingJoint);
+          this->topLevelModelMap.insert(
+            std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+          return true;
+        }
+
         sdf::Joint joint;
         joint.SetName(_name->Data());
         joint.SetType(_jointType->Data());
@@ -1642,7 +1711,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           joint.SetAxis(1, jointAxis2->Data());
 
         // Use the parent link's parent model as the model of this joint
-        auto jointPtrPhys = modelJointFeature->ConstructJoint(joint);
+        auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint);
 
         if (jointPtrPhys.Valid())
         {

From 748960e83ef1771f784924080cd5d82878cf12af Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 30 Jun 2022 15:51:32 +0800
Subject: [PATCH 02/11] Fix bug that can cause segfault

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../joint_position_controller/JointPositionController.cc        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc
index 136d7eba06..b51c8f1280 100644
--- a/src/gui/plugins/joint_position_controller/JointPositionController.cc
+++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc
@@ -274,7 +274,7 @@ void JointPositionController::Update(const UpdateInfo &,
     // Value
     double value = 0.0;
     auto posComp = _ecm.Component<components::JointPosition>(jointEntity);
-    if (posComp)
+    if (posComp && !posComp->Data().empty())
     {
       value = posComp->Data()[0];
     }

From 0426381a53d20219f2f3c3808fe6dec517d6fa1e Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 30 Jun 2022 15:51:50 +0800
Subject: [PATCH 03/11] Fix joint entity creation behavior

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 src/systems/physics/Physics.cc | 55 +++++++++++++++++++---------------
 1 file changed, 31 insertions(+), 24 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 81034e7046..4a97a3b7e7 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1626,12 +1626,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           const components::ChildLinkName *_childLinkName) -> bool
       {
         // If the parent model is scheduled for recreation, then do not
-        // try to create a new link. This situation can occur when a link
+        // try to create a new joint. This situation can occur when a joint
         // is added to a model from the GUI model editor.
         if (_ecm.EntityHasComponentType(_parentModel->Data(),
               components::Recreate::typeId))
         {
-          // Add this entity to the set of newly added links to existing
+          // Add this entity to the set of newly added joints to existing
           // models.
           this->jointAddedToModel.insert(_entity);
           return true;
@@ -1652,35 +1652,31 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         // Check if parent model exists
         if (!this->entityModelMap.HasEntity(_parentModel->Data()))
         {
-          gzwarn << "Joint's parent entity [" << _parentModel->Data()
+          gzerr << "Joint's parent model entity [" << _parentModel->Data()
                   << "] not found on model map." << std::endl;
           return true;
         }
 
-<<<<<<< HEAD
-        auto modelJointFeature =
-            this->entityModelMap.EntityCast<ConstructSdfJointFeatureList>(
-=======
-        auto modelPtrPhys =
-            this->entityModelMap.EntityCast<JointFeatureList>(
->>>>>>> e578c37f7 (Allow SDF model to be constructed in a single shot)
-                _parentModel->Data());
-        if (!modelPtrPhys)
+        auto basicModelPtrPhys = this->entityModelMap
+            .EntityCast<JointFeatureList>(_parentModel->Data());
+        if (!basicModelPtrPhys)
         {
           static bool informed{false};
           if (!informed)
           {
-            gzdbg << "Attempting to process joints, but the physics "
-                   << "engine doesn't support joint features. "
-                   << "Joints will be ignored." << std::endl;
+            gzerr << "Attempting to create a new joint [" <<_name->Data()
+                  << "] but the chosen physics engine does not support the "
+                  << "minimal joint features, so no joints will be created."
+                  << std::endl;
             informed = true;
           }
 
-          // Break Each call since no joints can be processed
+          // Skip all other attempts to create joints
           return false;
         }
 
-        if (const auto existingJoint = modelPtrPhys->GetJoint(_name->Data()))
+        if (const auto existingJoint =
+            basicModelPtrPhys->GetJoint(_name->Data()))
         {
           // No need to create this joint because it was already created when
           // parsing the model.
@@ -1690,6 +1686,17 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           return true;
         }
 
+        auto modelPtrPhys =
+            this->entityModelMap.EntityCast<ConstructSdfJointFeatureList>(
+                _parentModel->Data());
+        if (!modelPtrPhys)
+        {
+          gzerr << "Attempting to create a new joint [" << _name->Data()
+                << "], but the physics engine doesn't support constructing "
+                << "joints at runtime." << std::endl;
+          return true;
+        }
+
         sdf::Joint joint;
         joint.SetName(_name->Data());
         joint.SetType(_jointType->Data());
@@ -1752,9 +1759,9 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
             this->entityLinkMap.Get(_jointInfo->Data().parentLink);
         if (!parentLinkPhys)
         {
-          gzwarn << "DetachableJoint's parent link entity ["
-                  << _jointInfo->Data().parentLink << "] not found in link map."
-                  << std::endl;
+          gzerr << "DetachableJoint's parent link entity ["
+                << _jointInfo->Data().parentLink << "] not found in link map."
+                << std::endl;
           return true;
         }
 
@@ -1764,8 +1771,8 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         auto childLinkPhys = this->entityLinkMap.Get(childLinkEntity);
         if (!childLinkPhys)
         {
-          gzwarn << "Failed to find joint's child link [" << childLinkEntity
-                  << "]." << std::endl;
+          gzerr << "Failed to find joint's child link [" << childLinkEntity
+                << "]." << std::endl;
           return true;
         }
 
@@ -1777,7 +1784,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           static bool informed{false};
           if (!informed)
           {
-            gzdbg << "Attempting to create a detachable joint, but the physics"
+            gzerr << "Attempting to create a detachable joint, but the physics"
                    << " engine doesn't support feature "
                    << "[AttachFixedJointFeature]. Detachable joints will be "
                    << "ignored." << std::endl;
@@ -1810,7 +1817,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         }
         else
         {
-          gzwarn << "DetachableJoint could not be created." << std::endl;
+          gzerr << "DetachableJoint could not be created." << std::endl;
         }
         return true;
       });

From c4f55033b61ae56d6544b1fe9305e6a9102b67ba Mon Sep 17 00:00:00 2001
From: Michael Carroll <michael@openrobotics.org>
Date: Fri, 29 Jul 2022 19:01:49 -0500
Subject: [PATCH 04/11] Just the ABI breaking parts of #1560

Signed-off-by: Michael Carroll <michael@openrobotics.org>
---
 src/systems/physics/Physics.cc | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 4a97a3b7e7..781122af46 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1657,9 +1657,10 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           return true;
         }
 
-        auto basicModelPtrPhys = this->entityModelMap
-            .EntityCast<JointFeatureList>(_parentModel->Data());
-        if (!basicModelPtrPhys)
+        auto modelJointFeature =
+            this->entityModelMap.EntityCast<ConstructSdfJointFeatureList>(
+                _parentModel->Data());
+        if (!modelJointFeature)
         {
           static bool informed{false};
           if (!informed)

From 2d9ae7e37783b0439b3bb81691935745ef6c515d Mon Sep 17 00:00:00 2001
From: Michael Carroll <michael@openrobotics.org>
Date: Fri, 16 Sep 2022 12:34:19 -0500
Subject: [PATCH 05/11] Add ability to resolve URIs

Signed-off-by: Michael Carroll <michael@openrobotics.org>
---
 src/Server.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/Server.cc b/src/Server.cc
index ea0c7b5bbf..c8754386fa 100644
--- a/src/Server.cc
+++ b/src/Server.cc
@@ -73,6 +73,8 @@ Server::Server(const ServerConfig &_config)
 
   sdf::Errors errors;
 
+  sdf::ParserConfig::GlobalConfig().SetStoreResovledURIs(true);
+
   switch (_config.Source())
   {
     // Load a world if specified. Check SDF string first, then SDF file

From 22e42ea286b8104c3a6971df1984ae38805caf23 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Fri, 30 Sep 2022 10:40:43 +0200
Subject: [PATCH 06/11] Add collision to entityCollisionMap

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 src/systems/physics/Physics.cc | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 781122af46..46d03a640d 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1362,6 +1362,11 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm,
           // specifically uses AttachMeshShapeFeature and AttachHeightMapFeature
           // for those shapes. The physics plugin implementer needs to know to
           // implement those specific features instead.
+          auto linkCollisionFeature =
+              this->entityLinkMap.EntityCast<CollisionFeatureList>(
+                  _parent->Data());
+          this->entityCollisionMap.AddEntity(
+            _entity, linkCollisionFeature->GetCollision(_name->Data()));
           this->topLevelModelMap.insert(
             std::make_pair(_entity, topLevelModel(_entity, _ecm)));
           return true;

From 1c6ee95ae1c3081d0a6deb6c410a4c55d64fa041 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Thu, 6 Oct 2022 17:39:59 +0200
Subject: [PATCH 07/11] Add bullet params to diff drive vehicle

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 examples/worlds/breadcrumbs.sdf         | 20 ++++++++
 examples/worlds/detachable_joint.sdf    | 20 ++++++++
 examples/worlds/diff_drive.sdf          | 68 +++++++++++++++++++++++++
 examples/worlds/performer_detector.sdf  | 20 ++++++++
 examples/worlds/triggered_publisher.sdf | 20 ++++++++
 test/worlds/breadcrumbs.sdf             | 20 ++++++++
 test/worlds/performer_detector.sdf      | 20 ++++++++
 7 files changed, 188 insertions(+)

diff --git a/examples/worlds/breadcrumbs.sdf b/examples/worlds/breadcrumbs.sdf
index 3eb587456f..bb9a40a80f 100644
--- a/examples/worlds/breadcrumbs.sdf
+++ b/examples/worlds/breadcrumbs.sdf
@@ -152,6 +152,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -197,6 +202,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -242,6 +252,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -287,6 +302,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf
index 9f2bb2f38d..f036956dca 100644
--- a/examples/worlds/detachable_joint.sdf
+++ b/examples/worlds/detachable_joint.sdf
@@ -154,6 +154,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -199,6 +204,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -244,6 +254,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -289,6 +304,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/diff_drive.sdf b/examples/worlds/diff_drive.sdf
index f40941667e..a85ae7f112 100644
--- a/examples/worlds/diff_drive.sdf
+++ b/examples/worlds/diff_drive.sdf
@@ -144,6 +144,23 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <slip1>0.035</slip1>
+                <slip2>0</slip2>
+                <fdir1>0 0 1</fdir1>
+              </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+                <spinning_friction>0.1</spinning_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
@@ -178,6 +195,23 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <slip1>0.035</slip1>
+                <slip2>0</slip2>
+                <fdir1>0 0 1</fdir1>
+              </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+                <spinning_friction>0.1</spinning_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
@@ -332,6 +366,23 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <slip1>0.035</slip1>
+                <slip2>0</slip2>
+                <fdir1>0 0 1</fdir1>
+              </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+                <spinning_friction>0.1</spinning_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
@@ -366,6 +417,23 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <slip1>0.035</slip1>
+                <slip2>0</slip2>
+                <fdir1>0 0 1</fdir1>
+              </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+                <spinning_friction>0.1</spinning_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
diff --git a/examples/worlds/performer_detector.sdf b/examples/worlds/performer_detector.sdf
index 036248851f..4060cf72d0 100644
--- a/examples/worlds/performer_detector.sdf
+++ b/examples/worlds/performer_detector.sdf
@@ -151,6 +151,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -196,6 +201,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -241,6 +251,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -286,6 +301,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf
index 2c7135ac0e..1457f55d9d 100644
--- a/examples/worlds/triggered_publisher.sdf
+++ b/examples/worlds/triggered_publisher.sdf
@@ -157,6 +157,11 @@ start falling.
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -202,6 +207,11 @@ start falling.
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -247,6 +257,11 @@ start falling.
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -292,6 +307,11 @@ start falling.
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/breadcrumbs.sdf b/test/worlds/breadcrumbs.sdf
index 7790557dba..8fe6818a26 100644
--- a/test/worlds/breadcrumbs.sdf
+++ b/test/worlds/breadcrumbs.sdf
@@ -187,6 +187,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -232,6 +237,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -277,6 +287,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -322,6 +337,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/performer_detector.sdf b/test/worlds/performer_detector.sdf
index de36510e55..9b1cae5d51 100644
--- a/test/worlds/performer_detector.sdf
+++ b/test/worlds/performer_detector.sdf
@@ -119,6 +119,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -164,6 +169,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -209,6 +219,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -254,6 +269,11 @@
                 <slip2>0</slip2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>

From 55096620b5f7e45952fdc4724bdeaf3c2593e96d Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Fri, 7 Oct 2022 13:38:23 +0200
Subject: [PATCH 08/11] Added more bullet params to examples

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 examples/worlds/ackermann_steering.sdf        | 24 ++++++++++++++
 examples/worlds/diff_drive_skid.sdf           | 25 ++++++++++++++
 examples/worlds/pendulum_links.sdf            | 33 +++++++++++++++----
 examples/worlds/skid_steer_mecanum.sdf        | 25 ++++++++++++++
 examples/worlds/velocity_control.sdf          | 27 +++++++++++++++
 test/worlds/ackermann_steering.sdf            |  5 +++
 .../ackermann_steering_custom_frame_id.sdf    | 25 ++++++++++++++
 .../ackermann_steering_custom_topics.sdf      | 25 ++++++++++++++
 .../ackermann_steering_limited_joints_pub.sdf | 25 ++++++++++++++
 test/worlds/ackermann_steering_slow_odom.sdf  |  5 +++
 10 files changed, 212 insertions(+), 7 deletions(-)

diff --git a/examples/worlds/ackermann_steering.sdf b/examples/worlds/ackermann_steering.sdf
index 5de5fa13ac..b4fdc88d80 100644
--- a/examples/worlds/ackermann_steering.sdf
+++ b/examples/worlds/ackermann_steering.sdf
@@ -60,6 +60,10 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -156,6 +160,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -201,6 +210,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.5</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -246,6 +260,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -291,6 +310,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.5</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/diff_drive_skid.sdf b/examples/worlds/diff_drive_skid.sdf
index da43686013..fe1fdbe833 100644
--- a/examples/worlds/diff_drive_skid.sdf
+++ b/examples/worlds/diff_drive_skid.sdf
@@ -60,6 +60,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -154,6 +159,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -197,6 +207,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -240,6 +255,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -283,6 +303,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf
index 7b189e0d83..5eecd1893a 100644
--- a/examples/worlds/pendulum_links.sdf
+++ b/examples/worlds/pendulum_links.sdf
@@ -46,11 +46,12 @@
       <static>true</static>
       <link name="link">
         <collision name="collision">
-        <geometry>
-          <plane>
-            <normal>0 0 1</normal>
-          </plane>
-        </geometry>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
         </collision>
         <visual name="visual">
           <geometry>
@@ -71,7 +72,16 @@
     <model name="double_pendulum_with_base">
       <link name="base">
         <inertial>
-          <mass>100</mass>
+          <pose>-0.188749 0 0.75813399999999997 -0.13567899999999999 0 1.5708</pose>
+          <mass>256.42500000000001</mass>
+          <inertia>
+            <ixx>154.202</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>152.286</iyy>
+            <iyz>0</iyz>
+            <izz>28.8249</izz>
+          </inertia>
         </inertial>
         <visual name="vis_plate_on_ground">
           <pose>0 0 0.01 0 0 0</pose>
@@ -123,7 +133,16 @@
         <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>
+          <pose>-0.0107143 -4.4408900000000002e-16 0.46428599999999998 0.038729600000000003 -3.4694469519536142e-18 1.5708</pose>
+          <mass>87.964600000000004</mass>
+          <inertia>
+            <ixx>11.999000000000001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>11.8742</iyy>
+            <iyz>0</iyz>
+            <izz>0.56461300000000003</izz>
+          </inertia>
         </inertial>
         <visual name="vis_upper_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
diff --git a/examples/worlds/skid_steer_mecanum.sdf b/examples/worlds/skid_steer_mecanum.sdf
index 637be5faa8..05dfc150b6 100644
--- a/examples/worlds/skid_steer_mecanum.sdf
+++ b/examples/worlds/skid_steer_mecanum.sdf
@@ -60,6 +60,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -159,6 +164,11 @@
                 <mu2>0.0</mu2>
                 <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -203,6 +213,11 @@
                 <mu2>0.0</mu2>
                 <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -247,6 +262,11 @@
                 <mu2>0.0</mu2>
                 <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -291,6 +311,11 @@
                 <mu2>0.0</mu2>
                 <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf
index 58d124fb2a..f66efea55f 100644
--- a/examples/worlds/velocity_control.sdf
+++ b/examples/worlds/velocity_control.sdf
@@ -138,6 +138,15 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
@@ -172,6 +181,15 @@
               <radius>0.3</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
@@ -206,6 +224,15 @@
               <radius>0.2</radius>
             </sphere>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
       </link>
 
diff --git a/test/worlds/ackermann_steering.sdf b/test/worlds/ackermann_steering.sdf
index 1930cf3145..ea75b3b70d 100644
--- a/test/worlds/ackermann_steering.sdf
+++ b/test/worlds/ackermann_steering.sdf
@@ -40,6 +40,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/ackermann_steering_custom_frame_id.sdf b/test/worlds/ackermann_steering_custom_frame_id.sdf
index e1e318ba72..8e553c9769 100644
--- a/test/worlds/ackermann_steering_custom_frame_id.sdf
+++ b/test/worlds/ackermann_steering_custom_frame_id.sdf
@@ -48,6 +48,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -144,6 +149,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -189,6 +199,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -234,6 +249,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -279,6 +299,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/ackermann_steering_custom_topics.sdf b/test/worlds/ackermann_steering_custom_topics.sdf
index 1923027d07..a80f065e58 100644
--- a/test/worlds/ackermann_steering_custom_topics.sdf
+++ b/test/worlds/ackermann_steering_custom_topics.sdf
@@ -48,6 +48,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -144,6 +149,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -189,6 +199,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -234,6 +249,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -279,6 +299,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/ackermann_steering_limited_joints_pub.sdf b/test/worlds/ackermann_steering_limited_joints_pub.sdf
index f11bb3ad23..59c2eae3c1 100644
--- a/test/worlds/ackermann_steering_limited_joints_pub.sdf
+++ b/test/worlds/ackermann_steering_limited_joints_pub.sdf
@@ -48,6 +48,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -144,6 +149,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -189,6 +199,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -234,6 +249,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
@@ -279,6 +299,11 @@
                 <mu2>1.0</mu2>
                 <fdir1>0 0 1</fdir1>
               </ode>
+              <bullet>
+                <friction>0.5</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>
diff --git a/test/worlds/ackermann_steering_slow_odom.sdf b/test/worlds/ackermann_steering_slow_odom.sdf
index 1de374c7f9..5847ddacb4 100644
--- a/test/worlds/ackermann_steering_slow_odom.sdf
+++ b/test/worlds/ackermann_steering_slow_odom.sdf
@@ -40,6 +40,11 @@
               <ode>
                 <mu>50</mu>
               </ode>
+              <bullet>
+                <friction>1</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.1</rolling_friction>
+              </bullet>
             </friction>
           </surface>
         </collision>

From bbda0ab06e872617449fd4a3c1491039579e44d8 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Thu, 27 Oct 2022 14:53:15 +0200
Subject: [PATCH 09/11] Improved some example sdf files

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 examples/worlds/acoustic_comms_demo.sdf   |  8 +++----
 examples/worlds/apply_link_wrench.sdf     | 18 +++++++++++++++
 examples/worlds/buoyancy_engine.sdf       |  4 ++--
 examples/worlds/spherical_coordinates.sdf |  4 ++--
 examples/worlds/trajectory_follower.sdf   | 27 +++++++++++++++++++++++
 test/worlds/collada_world_exporter.sdf    |  4 ++++
 6 files changed, 57 insertions(+), 8 deletions(-)

diff --git a/examples/worlds/acoustic_comms_demo.sdf b/examples/worlds/acoustic_comms_demo.sdf
index d5ebe9e0e5..6e1be4c4b9 100644
--- a/examples/worlds/acoustic_comms_demo.sdf
+++ b/examples/worlds/acoustic_comms_demo.sdf
@@ -66,12 +66,12 @@
 
     <include>
       <pose>-5 0 0 0 0 0</pose>
-      <uri>https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator</uri>
+      <uri>https://fuel.gazebosim.org/1.0/mabelzhang/models/Turquoise turbidity generator</uri>
     </include>
 
     <include>
       <pose>0 0 1 0 0 1.57</pose>
-      <uri>https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
+      <uri>https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
 
       <!-- Acoustic comms endpoint -->
       <plugin
@@ -172,7 +172,7 @@
 
     <include>
       <pose>15 0 1 0 0 1.57</pose>
-      <uri>https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
+      <uri>https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
       <name>triton</name>
 
       <!-- Acoustic comms endpoint -->
@@ -274,7 +274,7 @@
 
     <include>
       <pose>-15 0 1 0 0 1.57</pose>
-      <uri>https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
+      <uri>https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>
       <name>daphne</name>
 
       <!-- Acoustic comms endpoint -->
diff --git a/examples/worlds/apply_link_wrench.sdf b/examples/worlds/apply_link_wrench.sdf
index 03509d9766..9b9bb229b4 100644
--- a/examples/worlds/apply_link_wrench.sdf
+++ b/examples/worlds/apply_link_wrench.sdf
@@ -104,6 +104,15 @@
               <size>1 1 1</size>
             </box>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>0.7</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.2</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
 
         <visual name="visual">
@@ -142,6 +151,15 @@
               <length>1.0</length>
             </cylinder>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>0.7</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.2</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
 
         <visual name="visual">
diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf
index 7f7dfae298..1a68f4d323 100644
--- a/examples/worlds/buoyancy_engine.sdf
+++ b/examples/worlds/buoyancy_engine.sdf
@@ -41,13 +41,13 @@ until it breaches and then start oscillating around the surface. On the other
 hand the box on the right will rise forever as no surface is set.
 Enter the following in a separate terminal:
 ```
-gz topic -t  /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double
+gz topic -t  /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \
    -p "data: 0.003"
 ```
 The boxes will float up. Note that the box on the left will start oscillating
 once it breaches the surface.
 ```
-gz topic -t  /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double
+gz topic -t  /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \
    -p "data: 0.001"
 ```
 The boxes will go down.
diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf
index d9a9a1ba45..1519bd64d7 100644
--- a/examples/worlds/spherical_coordinates.sdf
+++ b/examples/worlds/spherical_coordinates.sdf
@@ -230,14 +230,14 @@ gz service -s /world/spherical_coordinates/set_spherical_coordinates \
       </plugin>
 
       <!-- Map -->
-      <plugin filename="NavSatMap" name="NavSat Map">
+      <!-- <plugin filename="NavSatMap" name="NavSat Map">
         <gz-gui>
           <title>NavSat Map</title>
           <property key="state" type="string">docked</property>
         </gz-gui>
         <topic>/navsat</topic>
         <topic_picker>true</topic_picker>
-      </plugin>
+      </plugin> -->
     </gui>
 
     <light type="directional" name="sun">
diff --git a/examples/worlds/trajectory_follower.sdf b/examples/worlds/trajectory_follower.sdf
index 8af0e3e9b8..54aff9d9a2 100644
--- a/examples/worlds/trajectory_follower.sdf
+++ b/examples/worlds/trajectory_follower.sdf
@@ -93,6 +93,15 @@
               <size>1 1 1</size>
             </box>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>0.7</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.2</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
 
         <visual name="box_visual">
@@ -144,6 +153,15 @@
               <size>1 1 1</size>
             </box>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>0.7</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.2</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
 
         <visual name="box_visual">
@@ -195,6 +213,15 @@
               <size>1 1 1</size>
             </box>
           </geometry>
+          <surface>
+            <friction>
+              <bullet>
+                <friction>0.7</friction>
+                <friction2>1</friction2>
+                <rolling_friction>0.2</rolling_friction>
+              </bullet>
+            </friction>
+          </surface>
         </collision>
 
         <visual name="box_visual">
diff --git a/test/worlds/collada_world_exporter.sdf b/test/worlds/collada_world_exporter.sdf
index fdfd1e834f..b8c67d6dfa 100644
--- a/test/worlds/collada_world_exporter.sdf
+++ b/test/worlds/collada_world_exporter.sdf
@@ -1,3 +1,7 @@
+<!--
+Launch the world and check the folder ./shapes
+-->
+
 <?xml version="1.0" ?>
 <sdf version="1.6">
   <world name="collada_world_exporter_box_test">

From 55ebcd3f186422db748a872e5bd819c5ad4c43c7 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Thu, 27 Oct 2022 15:07:02 +0200
Subject: [PATCH 10/11] Change API call

Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: Michael Carroll <michael@openrobotics.org>
---
 src/Server.cc                                 |  2 -
 src/systems/physics/Physics.cc                | 47 ++++++++-----------
 src/systems/user_commands/UserCommands.cc     |  3 ++
 test/integration/physics_system.cc            |  4 +-
 test/integration/thermal_system.cc            |  8 ++--
 test/worlds/CMakeLists.txt                    |  3 ++
 test/worlds/collada_world_exporter.sdf        |  3 +-
 .../{heightmap.sdf => heightmap.sdf.in}       |  2 +-
 test/worlds/{mesh.sdf => mesh.sdf.in}         |  8 ++--
 test/worlds/{thermal.sdf => thermal.sdf.in}   |  8 ++--
 10 files changed, 41 insertions(+), 47 deletions(-)
 rename test/worlds/{heightmap.sdf => heightmap.sdf.in} (95%)
 rename test/worlds/{mesh.sdf => mesh.sdf.in} (92%)
 rename test/worlds/{thermal.sdf => thermal.sdf.in} (96%)

diff --git a/src/Server.cc b/src/Server.cc
index c8754386fa..ea0c7b5bbf 100644
--- a/src/Server.cc
+++ b/src/Server.cc
@@ -73,8 +73,6 @@ Server::Server(const ServerConfig &_config)
 
   sdf::Errors errors;
 
-  sdf::ParserConfig::GlobalConfig().SetStoreResovledURIs(true);
-
   switch (_config.Source())
   {
     // Load a world if specified. Check SDF string first, then SDF file
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 46d03a640d..461fb605ad 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1066,7 +1066,6 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
         }
         // TODO(anyone) Don't load models unless they have collisions
 
-
         // Check if parent world / model exists
         sdf::Model model;
         if (const auto *modelSdfComp =
@@ -1079,6 +1078,8 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
         // ModelSdf component.
         model.SetName(_name->Data());
         model.SetRawPose(_pose->Data());
+        model.SetPoseRelativeTo("");
+
         auto staticComp = _ecm.Component<components::Static>(_entity);
         if (staticComp && staticComp->Data())
         {
@@ -1116,17 +1117,22 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
               return true;
             }
             auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
-            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
-            this->topLevelModelMap.insert(std::make_pair(_entity,
-                topLevelModel(_entity, _ecm)));
+            if (modelPtrPhys)
+            {
+              this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+              this->topLevelModelMap.insert(std::make_pair(_entity,
+                  topLevelModel(_entity, _ecm)));
+            }
           }
           else
           {
             auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
-
-            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
-            this->topLevelModelMap.insert(std::make_pair(_entity,
-                topLevelModel(_entity, _ecm)));
+            if (modelPtrPhys)
+            {
+              this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+              this->topLevelModelMap.insert(std::make_pair(_entity,
+                  topLevelModel(_entity, _ecm)));
+            }
           }
         }
         // check if parent is a model (nested model)
@@ -1232,7 +1238,6 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm,
                   << "] not found on model map." << std::endl;
           return true;
         }
-
         auto basicModelPtrPhys = this->entityModelMap.Get(_parent->Data());
 
         if (const auto existingLink = basicModelPtrPhys->GetLink(_name->Data()))
@@ -1261,6 +1266,7 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm,
         sdf::Link link;
         link.SetName(_name->Data());
         link.SetRawPose(_pose->Data());
+        link.SetPoseRelativeTo("");
 
         if (this->staticEntities.find(_parent->Data()) !=
             this->staticEntities.end())
@@ -1348,25 +1354,11 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm,
         {
           // No need to create this collision shape because it was already
           // created when parsing the model.
-
-          // TODO(MXG): There's some significant design inconsistency in this
-          // CreateCollisionEntities function. There's a mix of using the
-          // ConstructSdfCollision feature and other collision attachment
-          // features like AttachHeightMapFeature and AttachMeshShape. This
-          // makes it confusing for physics plugin implementers to know where
-          // and how to implement the construction of collision shapes.
-          //
-          // In the current implementation, if a physics engine implements
-          // constructing a mesh or heightmap in ConstructSdfCollision then that
-          // ability won't actually get leveraged by gz-sim because it
-          // specifically uses AttachMeshShapeFeature and AttachHeightMapFeature
-          // for those shapes. The physics plugin implementer needs to know to
-          // implement those specific features instead.
           auto linkCollisionFeature =
               this->entityLinkMap.EntityCast<CollisionFeatureList>(
                   _parent->Data());
           this->entityCollisionMap.AddEntity(
-            _entity, linkCollisionFeature->GetCollision(_name->Data()));
+            _entity, linkCollisionFeature->GetShape(_name->Data()));
           this->topLevelModelMap.insert(
             std::make_pair(_entity, topLevelModel(_entity, _ecm)));
           return true;
@@ -1662,10 +1654,9 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           return true;
         }
 
-        auto modelJointFeature =
-            this->entityModelMap.EntityCast<ConstructSdfJointFeatureList>(
-                _parentModel->Data());
-        if (!modelJointFeature)
+        auto basicModelPtrPhys = this->entityModelMap
+            .EntityCast<JointFeatureList>(_parentModel->Data());
+        if (!basicModelPtrPhys)
         {
           static bool informed{false};
           if (!informed)
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index f51daef673..7218473587 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -1220,6 +1220,7 @@ bool CreateCommand::Execute()
   // Spherical coordinates
   if (createMsg->has_spherical_coordinates())
   {
+    gzerr << "HasSphericalCoordinates" << std::endl;
     auto scComp = this->iface->ecm->Component<components::SphericalCoordinates>(
         this->iface->worldEntity);
     if (nullptr == scComp)
@@ -1257,6 +1258,8 @@ bool CreateCommand::Execute()
   {
     auto poseComp = this->iface->ecm->Component<components::Pose>(entity);
     *poseComp = components::Pose(createPose.value());
+    gzdbg << poseComp->Data().X() << " " << poseComp->Data().Y() << " " << poseComp->Data().Z() << std::endl;
+    gzdbg << poseComp->Data().Roll() << " " << poseComp->Data().Pitch() << " " << poseComp->Data().Yaw() << std::endl;
   }
 
   gzdbg << "Created entity [" << entity << "] named [" << desiredName << "]"
diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
index 542335801f..0511fc4d36 100644
--- a/test/integration/physics_system.cc
+++ b/test/integration/physics_system.cc
@@ -2000,8 +2000,8 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(Heightmap))
 {
   gz::sim::ServerConfig serverConfig;
 
-  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/heightmap.sdf";
+  const auto sdfFile = common::joinPaths(PROJECT_BINARY_PATH,
+    "test", "worlds", "heightmap.sdf");
   serverConfig.SetSdfFile(sdfFile);
 
   sdf::Root root;
diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc
index 26e7ffa68b..8244f781fa 100644
--- a/test/integration/thermal_system.cc
+++ b/test/integration/thermal_system.cc
@@ -53,8 +53,8 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent))
 {
   // Start server
   ServerConfig serverConfig;
-  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
-      "/test/worlds/thermal.sdf");
+  serverConfig.SetSdfFile(common::joinPaths(PROJECT_BINARY_PATH,
+      "test", "worlds", "thermal.sdf"));
 
   Server server(serverConfig);
   EXPECT_FALSE(server.Running());
@@ -177,8 +177,8 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem))
 {
   // Start server
   ServerConfig serverConfig;
-  serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH,
-        "test/worlds/thermal.sdf"));
+  serverConfig.SetSdfFile(common::joinPaths(PROJECT_BINARY_PATH,
+      "test", "worlds", "thermal.sdf"));
 
   Server server(serverConfig);
   EXPECT_FALSE(server.Running());
diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt
index ea81f11e65..70e61f51f3 100644
--- a/test/worlds/CMakeLists.txt
+++ b/test/worlds/CMakeLists.txt
@@ -1 +1,4 @@
 configure_file (buoyancy.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/buoyancy.sdf)
+configure_file (mesh.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/mesh.sdf)
+configure_file (thermal.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/thermal.sdf)
+configure_file (heightmap.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/heightmap.sdf)
diff --git a/test/worlds/collada_world_exporter.sdf b/test/worlds/collada_world_exporter.sdf
index b8c67d6dfa..fff778f3be 100644
--- a/test/worlds/collada_world_exporter.sdf
+++ b/test/worlds/collada_world_exporter.sdf
@@ -1,8 +1,7 @@
+<?xml version="1.0" ?>
 <!--
 Launch the world and check the folder ./shapes
 -->
-
-<?xml version="1.0" ?>
 <sdf version="1.6">
   <world name="collada_world_exporter_box_test">
     <physics name="fast" type="ignored">
diff --git a/test/worlds/heightmap.sdf b/test/worlds/heightmap.sdf.in
similarity index 95%
rename from test/worlds/heightmap.sdf
rename to test/worlds/heightmap.sdf.in
index 6cc2834cfe..db573ea196 100644
--- a/test/worlds/heightmap.sdf
+++ b/test/worlds/heightmap.sdf.in
@@ -11,7 +11,7 @@
         <collision name="collision">
           <geometry>
             <heightmap>
-              <uri>../media/heightmap_bowl.png</uri>
+              <uri>file://@CMAKE_SOURCE_DIR@/test/media/heightmap_bowl.png</uri>
               <size>129 129 10</size>
               <pos>0 0 0</pos>
             </heightmap>
diff --git a/test/worlds/mesh.sdf b/test/worlds/mesh.sdf.in
similarity index 92%
rename from test/worlds/mesh.sdf
rename to test/worlds/mesh.sdf.in
index cdf0453685..abe7df140a 100644
--- a/test/worlds/mesh.sdf
+++ b/test/worlds/mesh.sdf.in
@@ -87,7 +87,7 @@
           <geometry>
             <mesh>
               <scale>0.5 0.5 0.5</scale>
-              <uri>file://test/media/duck_collider.dae</uri>
+              <uri>file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae</uri>
             </mesh>
           </geometry>
         </collision>
@@ -97,7 +97,7 @@
           <geometry>
             <mesh>
               <scale>0.5 0.5 0.5</scale>
-              <uri>file://test/media/duck.dae</uri>
+              <uri>file://@CMAKE_SOURCE_DIR@/test/media/duck.dae</uri>
             </mesh>
           </geometry>
         </visual>
@@ -122,7 +122,7 @@
           <pose>0 0 -0.4 1.57 0.0 0.0</pose>
           <geometry>
             <mesh>
-              <uri>file://test/media/duck_collider.dae</uri>
+              <uri>file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae</uri>
             </mesh>
           </geometry>
         </collision>
@@ -131,7 +131,7 @@
           <pose>0 0 -0.4 1.57 0.0 0.0</pose>
           <geometry>
             <mesh>
-              <uri>file://test/media/duck.dae</uri>
+              <uri>file://@CMAKE_SOURCE_DIR@/test/media/duck.dae</uri>
             </mesh>
           </geometry>
         </visual>
diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf.in
similarity index 96%
rename from test/worlds/thermal.sdf
rename to test/worlds/thermal.sdf.in
index 5cb3adf13f..2ec333e6e5 100644
--- a/test/worlds/thermal.sdf
+++ b/test/worlds/thermal.sdf.in
@@ -204,7 +204,7 @@
           <plugin
             filename="gz-sim-thermal-system"
             name="gz::sim::systems::Thermal">
-            <heat_signature>../media/duck.png</heat_signature>
+            <heat_signature>file://@CMAKE_SOURCE_DIR@/test/media/duck.png</heat_signature>
             <temperature>400.0</temperature>
           </plugin>
         </visual>
@@ -248,7 +248,7 @@
           <plugin
             filename="gz-sim-thermal-system"
             name="gz::sim::systems::Thermal">
-            <heat_signature>../media/duck.png</heat_signature>
+            <heat_signature>file://@CMAKE_SOURCE_DIR@/test/media/duck.png</heat_signature>
             <max_temp>500.0</max_temp>
           </plugin>
         </visual>
@@ -295,7 +295,7 @@
           <plugin
             filename="gz-sim-thermal-system"
             name="gz::sim::systems::Thermal">
-            <heat_signature>../media/duck.png</heat_signature>
+            <heat_signature>file://@CMAKE_SOURCE_DIR@/test/media/duck.png</heat_signature>
             <min_temp>400.0</min_temp>
           </plugin>
         </visual>
@@ -317,7 +317,7 @@
           </material>
           <plugin filename="gz-sim-thermal-system"
                   name="gz::sim::systems::Thermal">
-          <heat_signature>../media/RescueRandy_Thermal.png</heat_signature>
+            <heat_signature>file://@CMAKE_SOURCE_DIR@/test/media/RescueRandy_Thermal.png</heat_signature>
             <max_temp>310</max_temp>
           </plugin>
         </visual>

From 79fe3bae6a8f7b0de1dc76f79e3cc539ac2be3ef Mon Sep 17 00:00:00 2001
From: Michael Carroll <michael@openrobotics.org>
Date: Fri, 4 Nov 2022 11:24:14 -0500
Subject: [PATCH 11/11] Rebuild frame graph

Signed-off-by: Michael Carroll <michael@openrobotics.org>
---
 src/systems/physics/Physics.cc            | 9 +++++++--
 src/systems/user_commands/UserCommands.cc | 2 --
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 461fb605ad..0c5b0c6028 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1080,6 +1080,10 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
         model.SetRawPose(_pose->Data());
         model.SetPoseRelativeTo("");
 
+        sdf::Root root;
+        root.SetModel(model);
+        root.UpdateGraphs();
+
         auto staticComp = _ecm.Component<components::Static>(_entity);
         if (staticComp && staticComp->Data())
         {
@@ -1116,7 +1120,8 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
               }
               return true;
             }
-            auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
+            auto modelPtrPhys =
+              nestedModelFeature->ConstructNestedModel(*root.Model());
             if (modelPtrPhys)
             {
               this->entityModelMap.AddEntity(_entity, modelPtrPhys);
@@ -1126,7 +1131,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
           }
           else
           {
-            auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
+            auto modelPtrPhys = worldPtrPhys->ConstructModel(*root.Model());
             if (modelPtrPhys)
             {
               this->entityModelMap.AddEntity(_entity, modelPtrPhys);
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index 7218473587..b3adede8f6 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -1258,8 +1258,6 @@ bool CreateCommand::Execute()
   {
     auto poseComp = this->iface->ecm->Component<components::Pose>(entity);
     *poseComp = components::Pose(createPose.value());
-    gzdbg << poseComp->Data().X() << " " << poseComp->Data().Y() << " " << poseComp->Data().Z() << std::endl;
-    gzdbg << poseComp->Data().Roll() << " " << poseComp->Data().Pitch() << " " << poseComp->Data().Yaw() << std::endl;
   }
 
   gzdbg << "Created entity [" << entity << "] named [" << desiredName << "]"