diff --git a/include/gz/sim/components/AngularVelocityReset.hh b/include/gz/sim/components/AngularVelocityReset.hh new file mode 100644 index 0000000000..11292852ed --- /dev/null +++ b/include/gz/sim/components/AngularVelocityReset.hh @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_COMPONENTS_WORLDANGULARVELOCITYRESET_HH_ +#define GZ_SIM_COMPONENTS_WORLDANGULARVELOCITYRESET_HH_ + +#include +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief Angular velocity of an entity, in its own frame + /// and in SI units (rad/s). The angular velocity is + // represented by gz::math::Vector3d. + using AngularVelocityReset = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.AngularVelocityReset", AngularVelocityReset) + + /// \brief Angular velocity of an entity in the world frame + /// and in SI units (rad/s). The angular velocity is + // represented by gz::math::Vector3d. + using WorldAngularVelocityReset = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.WorldAngularVelocityReset", WorldAngularVelocityReset) +} +} +} +} + +#endif diff --git a/include/gz/sim/components/LinearVelocityReset.hh b/include/gz/sim/components/LinearVelocityReset.hh new file mode 100644 index 0000000000..c7343105f3 --- /dev/null +++ b/include/gz/sim/components/LinearVelocityReset.hh @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_ +#define GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_ + +#include +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief Linear velocity of an entity in its own frame + /// and in SI units (m/s). The linear velocity is + /// represented by gz::math::Vector3d. + using LinearVelocityReset = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.LinearVelocityReset", LinearVelocityReset) + + /// \brief Linear velocity of an entity in the world frame + /// and in SI units (m/s). The linear velocity is + /// represented by gz::math::Vector3d. + using WorldLinearVelocityReset = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.WorldLinearVelocityReset", WorldLinearVelocityReset) +} +} +} +} + +#endif diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index f073825dbc..bf5bd25781 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -100,6 +100,7 @@ #include "gz/sim/components/AngularAcceleration.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/AngularVelocityCmd.hh" +#include "gz/sim/components/AngularVelocityReset.hh" #include "gz/sim/components/AxisAlignedBox.hh" #include "gz/sim/components/BatterySoC.hh" #include "gz/sim/components/CanonicalLink.hh" @@ -124,6 +125,7 @@ #include "gz/sim/components/LinearAcceleration.hh" #include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/components/LinearVelocityCmd.hh" +#include "gz/sim/components/LinearVelocityReset.hh" #include "gz/sim/components/Link.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" @@ -2710,6 +2712,122 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Reset link linear velocity in world frame + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::WorldLinearVelocityReset *_worldlinearvelocityreset) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + gzwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + + auto rootLinkPtr = freeGroup->RootLink(); + if (rootLinkPtr != linkPtrPhys) + { + gzdbg << "Attempting to set linear velocity for link [ " << _entity + << " ] which is not root link of the FreeGroup." + << "Velocity won't be set." + << std::endl; + + return true; + } + + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldLinearVelFeature = this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + // Linear velocity in world frame + math::Vector3d worldLinearVel = _worldlinearvelocityreset->Data(); + + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + + // Reset link angular velocity in world frame + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::WorldAngularVelocityReset + *_worldangularvelocityreset) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + gzwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + + auto rootLinkPtr = freeGroup->RootLink(); + if(rootLinkPtr != linkPtrPhys) + { + gzdbg << "Attempting to set angular velocity for link [ " << _entity + << " ] which is not root link of the FreeGroup." + << "Velocity won't be set." + << std::endl; + + return true; + } + + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldAngularVelFeature = this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + // Angular velocity in world frame + math::Vector3d worldAngularVel = _worldangularvelocityreset->Data(); + + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); // Populate bounding box info // Only compute bounding box if component exists to avoid unnecessary @@ -3645,6 +3763,34 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, _ecm.RemoveComponent(entity); } + std::vector entitiesLinearVelocityReset; + _ecm.Each( + [&](const Entity &_entity, + components::WorldLinearVelocityReset *) -> bool + { + entitiesLinearVelocityReset.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesLinearVelocityReset) + { + _ecm.RemoveComponent(entity); + } + + std::vector entitiesAngularVelocityReset; + _ecm.Each( + [&](const Entity &_entity, + components::WorldAngularVelocityReset *) -> bool + { + entitiesAngularVelocityReset.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesAngularVelocityReset) + { + _ecm.RemoveComponent(entity); + } + std::vector entitiesCustomContactSurface; _ecm.Each( [&](const Entity &_entity, diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 2e7989b09a..7a50c8c7db 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -48,6 +48,7 @@ #include "gz/sim/components/AngularAcceleration.hh" #include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/AngularVelocityReset.hh" #include "gz/sim/components/AxisAlignedBox.hh" #include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/Collision.hh" @@ -67,6 +68,7 @@ #include "gz/sim/components/Link.hh" #include "gz/sim/components/LinearAcceleration.hh" #include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/LinearVelocityReset.hh" #include "gz/sim/components/Material.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" @@ -747,6 +749,272 @@ TEST_F(PhysicsSystemFixture, EXPECT_NEAR(pos0, positions[1], 0.01); } +///////////////////////////////////////////////// +/// Test linear veocity reset component +TEST_F(PhysicsSystemFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(LinearVelocityResetComponent)) +{ + ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive.sdf"; + + sdf::Root root; + root.Load(sdfFile); + sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + // Disable gravity + world->SetGravity(math::Vector3d::Zero); + + serverConfig.SetSdfRoot(root); + + Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // Get Link Entity's and objects + const std::string rootLinkName{"chassis"}; + auto rootLinkEntity = ecm->EntityByComponents( + components::Link(), components::Name(rootLinkName)); + Link rootLink(rootLinkEntity); + ASSERT_TRUE(rootLink.Valid(*ecm)); + + const std::string childLinkName{"caster"}; + auto childLinkEntity = ecm->EntityByComponents( + components::Link(), components::Name(childLinkName)); + Link childLink(childLinkEntity); + ASSERT_TRUE(childLink.Valid(*ecm)); + + // Enable velocity checks for each link + rootLink.EnableVelocityChecks(*ecm, true); + childLink.EnableVelocityChecks(*ecm, true); + EXPECT_NE(nullptr, + ecm->Component(rootLinkEntity)); + EXPECT_NE(nullptr, + ecm->Component(childLinkEntity)); + + // Step and expect velocities near zero + server.Run(true, 1, false); + { + auto rootLinkLinearVel = rootLink.WorldLinearVelocity(*ecm); + auto childLinkLinearVel = childLink.WorldLinearVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkLinearVel); + ASSERT_NE(std::nullopt, childLinkLinearVel); + EXPECT_EQ(math::Vector3d::Zero, rootLinkLinearVel); + EXPECT_EQ(math::Vector3d::Zero, childLinkLinearVel); + } + + const math::Vector3d vel0(0.125, -0.5, 3.5); + + // Reset child link velocity to nonzero components (upward in Z) + // This shouldn't do anything, since it is not the root link of its FreeGroup + ecm->CreateComponent(childLinkEntity, + components::WorldLinearVelocityReset(vel0)); + + // Step and expect that all velocities are still zero + server.Run(true, 1, false); + { + auto rootLinkLinearVel = rootLink.WorldLinearVelocity(*ecm); + auto childLinkLinearVel = childLink.WorldLinearVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkLinearVel); + ASSERT_NE(std::nullopt, childLinkLinearVel); + EXPECT_EQ(math::Vector3d::Zero, rootLinkLinearVel); + EXPECT_EQ(math::Vector3d::Zero, childLinkLinearVel); + } + + // Reset root link velocity to nonzero components (upward in Z) + ecm->CreateComponent(rootLinkEntity, + components::WorldLinearVelocityReset(vel0)); + + // Step and expect all links to have similar velocity + server.Run(true, 1, false); + { + auto rootLinkLinearVel = rootLink.WorldLinearVelocity(*ecm); + auto childLinkLinearVel = childLink.WorldLinearVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkLinearVel); + ASSERT_NE(std::nullopt, childLinkLinearVel); + EXPECT_EQ(vel0, rootLinkLinearVel); + EXPECT_EQ(vel0, childLinkLinearVel); + } + // expect that reset component has been removed + EXPECT_EQ(nullptr, + ecm->Component(rootLinkEntity)); + + // Reset root link velocity to zero + ecm->CreateComponent(rootLinkEntity, + components::WorldLinearVelocityReset(math::Vector3d::Zero)); + + // Step and expect all links to have zero velocity + server.Run(true, 1, false); + { + auto rootLinkLinearVel = rootLink.WorldLinearVelocity(*ecm); + auto childLinkLinearVel = childLink.WorldLinearVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkLinearVel); + ASSERT_NE(std::nullopt, childLinkLinearVel); + // TODO(scpeters) figure out why this isn't working + // EXPECT_EQ(math::Vector3d::Zero, rootLinkLinearVel); + // EXPECT_EQ(math::Vector3d::Zero, childLinkLinearVel); + } + // expect that reset component has been removed + EXPECT_EQ(nullptr, + ecm->Component(rootLinkEntity)); +} + +///////////////////////////////////////////////// +/// Test angular veocity reset component +TEST_F(PhysicsSystemFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AngularVelocityResetComponent)) +{ + ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive.sdf"; + + sdf::Root root; + root.Load(sdfFile); + sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + // Disable gravity + world->SetGravity(math::Vector3d::Zero); + + serverConfig.SetSdfRoot(root); + + Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // Get model Entity's + const std::string modelName("vehicle"); + auto modelEntity = ecm->EntityByComponents( + components::Model(), components::Name(modelName)); + Model model(modelEntity); + ASSERT_TRUE(model.Valid(*ecm)); + + // Get Link Entity's and objects + const std::string rootLinkName{"chassis"}; + auto rootLinkEntity = ecm->EntityByComponents( + components::Link(), components::Name(rootLinkName)); + Link rootLink(rootLinkEntity); + ASSERT_TRUE(rootLink.Valid(*ecm)); + + const std::string childLinkName{"caster"}; + auto childLinkEntity = ecm->EntityByComponents( + components::Link(), components::Name(childLinkName)); + Link childLink(childLinkEntity); + ASSERT_TRUE(childLink.Valid(*ecm)); + + // Enable velocity checks for each link + rootLink.EnableVelocityChecks(*ecm, true); + childLink.EnableVelocityChecks(*ecm, true); + EXPECT_NE(nullptr, + ecm->Component(rootLinkEntity)); + EXPECT_NE(nullptr, + ecm->Component(childLinkEntity)); + + // Step and expect velocities near zero + server.Run(true, 1, false); + { + auto rootLinkAngularVel = rootLink.WorldAngularVelocity(*ecm); + auto childLinkAngularVel = childLink.WorldAngularVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkAngularVel); + ASSERT_NE(std::nullopt, childLinkAngularVel); + EXPECT_EQ(math::Vector3d::Zero, rootLinkAngularVel); + EXPECT_EQ(math::Vector3d::Zero, childLinkAngularVel); + } + + const math::Pose3d pose0(0, 0, 1, 0, 0, 0); + // To avoid collision with ground plane while rotating + model.SetWorldPoseCmd(*ecm, pose0); + server.Run(true, 1, false); + + const math::Vector3d vel0(0.1, 5.0, 0.1); + + // Reset child link velocity to nonzero components + // This shouldn't do anything, since it is not the root link of its FreeGroup + ecm->CreateComponent(childLinkEntity, + components::WorldAngularVelocityReset(vel0)); + + // Step and expect that all velocities are still zero + server.Run(true, 1, false); + { + auto rootLinkAngularVel = rootLink.WorldAngularVelocity(*ecm); + auto childLinkAngularVel = childLink.WorldAngularVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkAngularVel); + ASSERT_NE(std::nullopt, childLinkAngularVel); + EXPECT_EQ(math::Vector3d::Zero, rootLinkAngularVel); + EXPECT_EQ(math::Vector3d::Zero, childLinkAngularVel); + } + + // Reset root link velocity to nonzero components + ecm->CreateComponent(rootLinkEntity, + components::WorldAngularVelocityReset(vel0)); + + // Step and expect all links to have similar velocity + server.Run(true, 1, false); + { + auto rootLinkAngularVel = rootLink.WorldAngularVelocity(*ecm); + auto childLinkAngularVel = childLink.WorldAngularVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkAngularVel); + ASSERT_NE(std::nullopt, childLinkAngularVel); + EXPECT_EQ(vel0, rootLinkAngularVel); + EXPECT_EQ(vel0, childLinkAngularVel); + } + // expect that reset component has been removed + EXPECT_EQ(nullptr, + ecm->Component(rootLinkEntity)); + + // Reset root link velocity to zero + ecm->CreateComponent(rootLinkEntity, + components::WorldAngularVelocityReset(math::Vector3d::Zero)); + + // Step and expect all links to have zero velocity + server.Run(true, 1, false); + { + auto rootLinkAngularVel = rootLink.WorldAngularVelocity(*ecm); + auto childLinkAngularVel = childLink.WorldAngularVelocity(*ecm); + ASSERT_NE(std::nullopt, rootLinkAngularVel); + ASSERT_NE(std::nullopt, childLinkAngularVel); + // TODO(scpeters) figure out why this isn't working + // EXPECT_EQ(math::Vector3d::Zero, rootLinkLinearVel); + // EXPECT_EQ(math::Vector3d::Zero, childLinkLinearVel); + } + // expect that reset component has been removed + EXPECT_EQ(nullptr, + ecm->Component(rootLinkEntity)); +} + ///////////////////////////////////////////////// /// Test joint veocity reset component TEST_F(PhysicsSystemFixture,