Skip to content

Gravity set command fixed (backport #3189) #3193

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

Open
mergify wants to merge 1 commit into
base: ign-gazebo6
Choose a base branch
from
Open

Gravity set command fixed (backport #3189) #3193

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
34 changes: 33 additions & 1 deletion src/SimulationRunner.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,21 @@

#include <algorithm>

<<<<<<< HEAD
=======
#include <gz/math/Vector3.hh>
#include <gz/msgs/boolean.pb.h>
#include <gz/msgs/clock.pb.h>
#include <gz/msgs/gui.pb.h>
#include <gz/msgs/log_playback_control.pb.h>
#include <gz/msgs/sdf_generator_config.pb.h>
#include <gz/msgs/stringmsg.pb.h>
#include <gz/msgs/world_control.pb.h>
#include <gz/msgs/world_control_state.pb.h>
#include <gz/msgs/world_stats.pb.h>

#include <sdf/Physics.hh>
>>>>>>> d2780bb0 (Gravity set command fixed (#3189))
#include <sdf/Root.hh>
#include <vector>

Expand All @@ -28,6 +43,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"
Expand Down Expand Up @@ -362,10 +378,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();
Expand Down
42 changes: 41 additions & 1 deletion src/systems/physics/Physics.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include <gz/msgs/Utility.hh>

#include <algorithm>
#include <iostream>
#include <deque>
#include <map>
#include <set>
Expand Down Expand Up @@ -425,6 +424,13 @@ class ignition::gazebo::systems::PhysicsPrivate
//////////////////////////////////////////////////

//////////////////////////////////////////////////

// Gravity
public: struct GravityFeatureList
: physics::FeatureList<
MinimumFeatureList,
physics::Gravity>{};

// Slip Compliance

/// \brief Feature list to process `FrictionPyramidSlipCompliance` components.
Expand Down Expand Up @@ -593,6 +599,11 @@ class ignition::gazebo::systems::PhysicsPrivate
MinimumFeatureList,
CollisionFeatureList,
ContactFeatureList,
<<<<<<< HEAD
=======
GravityFeatureList,
RayIntersectionFeatureList,
>>>>>>> d2780bb0 (Gravity set command fixed (#3189))
SetContactPropertiesCallbackFeatureList,
NestedModelFeatureList,
CollisionDetectorFeatureList,
Expand Down Expand Up @@ -1763,7 +1774,36 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
//////////////////////////////////////////////////
void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
{
<<<<<<< HEAD
IGN_PROFILE("PhysicsPrivate::UpdatePhysics");
=======
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;
});


>>>>>>> d2780bb0 (Gravity set command fixed (#3189))
// Battery state
_ecm.Each<components::BatterySoC>(
[&](const Entity & _entity, const components::BatterySoC *_bat)
Expand Down
30 changes: 30 additions & 0 deletions test/integration/user_commands.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,13 @@
*
*/

<<<<<<< HEAD
=======
#include <future>
#include <gz/msgs/details/vector3d.pb.h>
#include <string>

>>>>>>> d2780bb0 (Gravity set command fixed (#3189))
#include <gtest/gtest.h>

#include <gz/msgs/entity_factory.pb.h>
Expand All @@ -28,6 +35,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"
Expand Down Expand Up @@ -1064,15 +1072,30 @@ TEST_F(UserCommandsTest, IGN_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;
Expand All @@ -1087,6 +1110,13 @@ TEST_F(UserCommandsTest, IGN_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());
Expand Down
Loading