Gravity set command fixed (#3189) (#3190)

This pull requests solves the [issue](gazebosim/gz-physics#766) in gz-physics but since it requires changes in this repo instead of gz-physics I am opening this pr here.
With this gazebo now supports changing gravity at runtime by running
'gz service -s /world/world_demo/set_physics   --reqtype gz.msgs.Physics   --reptype gz.msgs.Boolean   --timeout 2000   --req 'gravity { x: 0 y: 0 z: -2.8 }'
---------




(cherry picked from commit d2780bb)

Signed-off-by: Shivang <[email protected]>
Signed-off-by: Arjo Chakravarty <[email protected]>
Co-authored-by: Shivang <[email protected]>
Co-authored-by: Arjo Chakravarty <[email protected]>

Author: 3 people

src/SimulationRunner.cc

@@ -25,6 +25,7 @@
 #include <pybind11/pybind11.h>
 #endif
 
+#include <gz/math/Vector3.hh>
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/clock.pb.h>
 #include <gz/msgs/gui.pb.h>
@@ -46,6 +47,7 @@
 #include "gz/sim/components/Sensor.hh"
 #include "gz/sim/components/Visual.hh"
 #include "gz/sim/components/World.hh"
+#include "gz/sim/components/Gravity.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/Physics.hh"
 #include "gz/sim/components/PhysicsCmd.hh"
@@ -403,10 +405,26 @@ void SimulationRunner::UpdatePhysicsParams()
   {
     return;
   }
+  const auto& physicsParams = physicsCmdComp->Data();
+
+  auto gravityComp =
+    this->entityCompMgr.Component<components::Gravity>(worldEntity);
+  if (gravityComp)
+  {
+    const  gz::math::Vector3<double>  newGravity =
+    {
+        physicsParams.gravity().x(),
+        physicsParams.gravity().y(),
+        physicsParams.gravity().z()
+    };
+    gravityComp->Data() = newGravity;
+    this->entityCompMgr.SetChanged(worldEntity, components::Gravity::typeId,
+          ComponentState::OneTimeChange);
+  }
+
   auto physicsComp =
     this->entityCompMgr.Component<components::Physics>(worldEntity);
 
-  const auto& physicsParams = physicsCmdComp->Data();
   const auto newStepSize =
     std::chrono::duration<double>(physicsParams.max_step_size());
   const double newRTF = physicsParams.real_time_factor();

src/systems/physics/Physics.cc

@@ -23,7 +23,6 @@
 #include <gz/msgs/Utility.hh>
 
 #include <algorithm>
-#include <iostream>
 #include <deque>
 #include <map>
 #include <set>
@@ -483,6 +482,13 @@ class gz::sim::systems::PhysicsPrivate
   //////////////////////////////////////////////////
 
   //////////////////////////////////////////////////
+
+  // Gravity
+  public: struct GravityFeatureList
+    : physics::FeatureList<
+          MinimumFeatureList,
+          physics::Gravity>{};
+
   // Slip Compliance
 
   /// \brief Feature list to process `FrictionPyramidSlipCompliance` components.
@@ -694,6 +700,7 @@ class gz::sim::systems::PhysicsPrivate
           MinimumFeatureList,
           CollisionFeatureList,
           ContactFeatureList,
+          GravityFeatureList,
           RayIntersectionFeatureList,
           SetContactPropertiesCallbackFeatureList,
           NestedModelFeatureList,
@@ -2197,6 +2204,31 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
 void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 {
   GZ_PROFILE("PhysicsPrivate::UpdatePhysics");
+  // Gravity state
+  _ecm.Each<components::Gravity>(
+      [&](const Entity & _entity, const components::Gravity *_gravity)
+      {
+        auto gravityFeature =
+          this->entityWorldMap.EntityCast<GravityFeatureList>(_entity);
+        if (!gravityFeature)
+        {
+            static bool informed{false};
+            if (!informed)
+            {
+              gzdbg << "Attempting to set physics options, but the "
+                     << "physics engine doesn't support feature "
+                     << "[GravityFeature]. Options will be ignored."
+                     << std::endl;
+              informed = true;
+            }
+            return false;
+        }
+        auto new_grav = _gravity->Data();
+        gravityFeature->SetGravity({new_grav.X(), new_grav.Y(), new_grav.Z()});
+        return true;
+  });
+
+
   // Battery state
   _ecm.Each<components::BatterySoC>(
       [&](const Entity & _entity, const components::BatterySoC *_bat)

test/integration/user_commands.cc

@@ -16,6 +16,7 @@
  */
 
 #include <future>
+#include <gz/msgs/details/vector3d.pb.h>
 #include <string>
 
 #include <gtest/gtest.h>
@@ -37,6 +38,7 @@
 #include <gz/transport/Node.hh>
 #include <gz/utils/ExtraTestMacros.hh>
 
+#include "gz/sim/components/Gravity.hh"
 #include "gz/sim/components/Light.hh"
 #include "gz/sim/components/Link.hh"
 #include "gz/sim/components/Material.hh"
@@ -1584,15 +1586,30 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics))
   auto worldEntity = ecm->EntityByComponents(components::World());
   EXPECT_NE(kNullEntity, worldEntity);
   auto physicsComp = ecm->Component<components::Physics>(worldEntity);
+  auto gravityComp = ecm->Component<components::Gravity>(worldEntity);
   ASSERT_NE(nullptr, physicsComp);
+  ASSERT_NE(nullptr, gravityComp);
   EXPECT_DOUBLE_EQ(0.001, physicsComp->Data().MaxStepSize());
   EXPECT_DOUBLE_EQ(0.0, physicsComp->Data().RealTimeFactor());
 
+  auto gravity = gravityComp->Data();
+  EXPECT_DOUBLE_EQ(0.0, gravity.X());
+  EXPECT_DOUBLE_EQ(0.0, gravity.Y());
+  EXPECT_DOUBLE_EQ(-9.8, gravity.Z());
+
   // Set physics properties
   msgs::Physics req;
   req.set_max_step_size(0.123);
   req.set_real_time_factor(4.567);
 
+  gz::msgs::Vector3d new_gravity;
+  new_gravity.set_x(1.0);
+  new_gravity.set_y(3.0);
+  new_gravity.set_z(5.0);
+  req.mutable_gravity()->set_x(new_gravity.x());
+  req.mutable_gravity()->set_y(new_gravity.y());
+  req.mutable_gravity()->set_z(new_gravity.z());
+
   msgs::Boolean res;
   bool result;
   unsigned int timeout = 5000;
@@ -1607,6 +1624,13 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics))
   // in the second one it is processed
   server.Run(true, 2, false);
 
+  // Check updated gravity
+  gravityComp = ecm->Component<components::Gravity>(worldEntity);
+  gravity = gravityComp->Data();
+  EXPECT_DOUBLE_EQ(new_gravity.x(), gravity.X());
+  EXPECT_DOUBLE_EQ(new_gravity.y(), gravity.Y());
+  EXPECT_DOUBLE_EQ(new_gravity.z(), gravity.Z());
+
   // Check updated physics properties
   physicsComp = ecm->Component<components::Physics>(worldEntity);
   EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize());