diff --git a/scenario/src/plugins/Physics/Physics.cc b/scenario/src/plugins/Physics/Physics.cc index 011b44fb4..dee160a1a 100644 --- a/scenario/src/plugins/Physics/Physics.cc +++ b/scenario/src/plugins/Physics/Physics.cc @@ -15,6 +15,7 @@ * */ +// clang-format off #include "Physics.hh" #include @@ -131,6 +132,13 @@ #include "CanonicalLinkModelTracker.hh" #include "EntityFeatureMap.hh" +// Extra components +#include "scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h" +#include "scenario/gazebo/components/HistoryOfAppliedJointForces.h" +#include "scenario/gazebo/components/JointAcceleration.h" +#include +#include "scenario/gazebo/components/SimulatedTime.h" + using namespace ignition; using namespace ignition::gazebo; using namespace ignition::gazebo::systems; @@ -211,7 +219,8 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Update physics from components /// \param[in] _ecm Mutable reference to ECM. - public: void UpdatePhysics(EntityComponentManager &_ecm); + public: void UpdatePhysics(EntityComponentManager &_ecm, + const ignition::gazebo::UpdateInfo& _info); /// \brief Step the simulation for each world /// \param[in] _dt Duration @@ -263,7 +272,8 @@ class ignition::gazebo::systems::PhysicsPrivate /// most recent physics step. The key is the entity of the link, and the /// value is the updated frame data corresponding to that entity. public: void UpdateSim(EntityComponentManager &_ecm, - std::map &_linkFrameData); + std::map &_linkFrameData, + const ignition::gazebo::UpdateInfo &_info); /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. @@ -585,6 +595,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Boolean value that is true only the first call of Configure and + /// PreUpdate. + bool firstRun = true; }; ////////////////////////////////////////////////// @@ -721,10 +735,22 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) << "s]. System may not work properly." << std::endl; } + // Update the component with the time in seconds that the simulation + // will have after the step + _ecm.Each( + [&](const Entity& worldEntity, + const components::World*, + components::SimulatedTime*) { + + scenario::gazebo::utils::setExistingComponentData< + components::SimulatedTime>(&_ecm, worldEntity, _info.simTime); + return true; + }); + if (this->dataPtr->engine) { this->dataPtr->CreatePhysicsEntities(_ecm); - this->dataPtr->UpdatePhysics(_ecm); + this->dataPtr->UpdatePhysics(_ecm, _info); ignition::physics::ForwardStep::Output stepOutput; // Only step if not paused. if (!_info.paused) @@ -732,7 +758,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) stepOutput = this->dataPtr->Step(_info.dt); } auto changedLinks = this->dataPtr->ChangedLinks(_ecm, stepOutput); - this->dataPtr->UpdateSim(_ecm, changedLinks); + this->dataPtr->UpdateSim(_ecm, changedLinks, _info); // Entities scheduled to be removed should be removed from physics after the // simulation step. Otherwise, since the to-be-removed entity still shows up @@ -751,13 +777,20 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) this->CreateCollisionEntities(_ecm); this->CreateJointEntities(_ecm); this->CreateBatteryEntities(_ecm); + + // Make sure that entities are processed by the plugin + // in the first iteration. This is necessary because + // the physics plugin can be loaded programmatically + // after the simulation has started and stepped. + if (this->firstRun) + this->firstRun = false; } ////////////////////////////////////////////////// void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) { // Get all the new worlds - _ecm.EachNew( + auto processEntities = [&](const Entity &_entity, const components::World * /* _world */, const components::Name *_name, @@ -831,14 +864,24 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) } return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each( + processEntities); + } + else + { + _ecm.EachNew( + processEntities); + } } ////////////////////////////////////////////////// void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) { - _ecm.EachNew( + auto processEntities = [&](const Entity &_entity, const components::Model *, const components::Name *_name, @@ -963,14 +1006,28 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) } return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } } ////////////////////////////////////////////////// void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) { - _ecm.EachNew( + auto processEntities = [&](const Entity &_entity, const components::Link * /* _link */, const components::Name *_name, @@ -1021,15 +1078,28 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) topLevelModel(_entity, _ecm))); return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } } ////////////////////////////////////////////////// void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) { - _ecm.EachNew( + auto processEntities = [&](const Entity &_entity, const components::Collision *, const components::Name *_name, @@ -1212,16 +1282,32 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) this->topLevelModelMap.insert(std::make_pair(_entity, topLevelModel(_entity, _ecm))); return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } } ////////////////////////////////////////////////// void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) { - _ecm.EachNew( + auto processJointEntities = [&](const Entity &_entity, const components::Joint * /* _joint */, const components::Name *_name, @@ -1300,10 +1386,10 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) topLevelModel(_entity, _ecm))); } return true; - }); + }; // Detachable joints - _ecm.EachNew( + auto processDetachableJointEntities = [&](const Entity &_entity, const components::DetachableJoint *_jointInfo) -> bool { @@ -1388,20 +1474,54 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) ignwarn << "DetachableJoint could not be created." << std::endl; } return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each(processJointEntities); + _ecm.Each(processDetachableJointEntities); + } + else + { + _ecm.EachNew(processJointEntities); + _ecm.EachNew(processDetachableJointEntities); + } } ////////////////////////////////////////////////// void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm) { - _ecm.EachNew( + auto processEntities = [&](const Entity & _entity, const components::BatterySoC *)->bool { // Parent entity of battery is model entity this->entityOffMap.insert(std::make_pair( _ecm.ParentEntity(_entity), false)); return true; - }); + }; + + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } } ////////////////////////////////////////////////// @@ -1492,7 +1612,8 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) +void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, + const ignition::gazebo::UpdateInfo &_info) { IGN_PROFILE("PhysicsPrivate::UpdatePhysics"); // Battery state @@ -1700,6 +1821,54 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Link wrenches with duration + if (!_info.paused) + { + _ecm.Each( + [&](const Entity& _entity, + components::ExternalWorldWrenchCmdWithDuration* + _wrenchWithDurComp) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity << "]." + << std::endl; + return true; + } + + auto linkForceFeature = + this->entityLinkMap.EntityCast(_entity); + if (!linkForceFeature) { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to apply a wrench, but the physics " + << "engine doesn't support feature " + << "[AddLinkExternalForceTorque]. Wrench will be ignored." + << std::endl; + informed = true; + } + + // Break Each call since no ExternalWorldWrenchCmd's can be processed + return false; + } + + const auto& totalWrench = _wrenchWithDurComp->Data().totalWrench(); + const math::Vector3 force = msgs::Convert(totalWrench.force()); + const math::Vector3 torque = msgs::Convert(totalWrench.torque()); + + linkForceFeature->AddExternalForce(math::eigen3::convert(force)); + linkForceFeature->AddExternalTorque(math::eigen3::convert(torque)); + + // NOTE: Cleaning could be moved to UpdateSim, but let's + // keep things all together for now + auto simTimeAfterStep = _info.simTime; + _wrenchWithDurComp->Data().cleanExpired(simTimeAfterStep); + + return true; + }); + } + // Update model pose auto olderWorldPoseCmdsToRemove = std::move(this->worldPoseCmdsToRemove); this->worldPoseCmdsToRemove.clear(); @@ -2314,7 +2483,8 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, - std::map &_linkFrameData) + std::map &_linkFrameData, + const ignition::gazebo::UpdateInfo &_info) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); @@ -2550,6 +2720,37 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, } IGN_PROFILE_END(); + // history of applied joint forces (commands) + _ecm.Each( + [&](const Entity& /*_entity*/, + components::Joint* /*_joint*/, + components::JointForceCmd* _forceCmd, + components::HistoryOfAppliedJointForces* _history) -> bool + { + // Since the operation is an append, we have to perform it only when + // the physics step is actually performed + if (_info.paused) + { + return true; + } + + // Get the history queue + auto& history = _history->Data(); + + // Get the data from the component + auto jointForceCmdData = _forceCmd->Data(); + + // Append values to the queue + for (const auto& jointForceCmd : jointForceCmdData) + { + history.push(jointForceCmd); + } + + return true; + }); + // pose/velocity/acceleration of non-link entities such as sensors / // collisions. These get updated only if another system has created a // components::WorldPose component for the entity. @@ -2762,6 +2963,41 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, } return true; }); + + // Update joint accelerations + _ecm.Each( + [&](const Entity& _entity, + components::Joint*, + components::JointAcceleration* _jointAcc) -> bool + { + if (auto jointPhys = this->entityJointMap.Get(_entity)) + { + _jointAcc->Data().resize(jointPhys->GetDegreesOfFreedom()); + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) + { + _jointAcc->Data()[i] = jointPhys->GetAcceleration(i); + } + } + return true; + }); + + // Update joint forces + _ecm.Each( + [&](const Entity& _entity, + components::Joint*, + components::JointForce* _jointForce) -> bool + { + if (auto jointPhys = this->entityJointMap.Get(_entity)) + { + _jointForce->Data().resize(jointPhys->GetDegreesOfFreedom()); + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) + { + _jointForce->Data()[i] = jointPhys->GetForce(i); + } + } + return true; + }); + IGN_PROFILE_END(); // TODO(louise) Skip this if there are no collision features @@ -2781,6 +3017,24 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) // capture the world Entity in a Configure call Entity worldEntity = _ecm.EntityByComponents(components::World()); + // Check if there is at least one collision that requires contact data. + // The previous check does not block the scenario where all the collisions + // start enabled and then get disabled. + bool hasContactSensorData = false; + _ecm.Each( + [&](const Entity&, + components::ContactSensorData*, + components::Collision*) -> bool + { + hasContactSensorData = true; + return false; + }); + + // Quit early if the ContactData component was created and then removed. + // This means there are no systems that need contact information. + if (!hasContactSensorData) + return; + if (worldEntity == kNullEntity) { ignerr << "Missing world entity.\n"; @@ -2809,12 +3063,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) return; } + // Struct containing pointer of contact data + struct AllContactData + { + const WorldShapeType::ContactPoint *point; + const WorldShapeType::ExtraContactData *extra; + }; + // Each contact object we get from ign-physics contains the EntityPtrs of the // two colliding entities and other data about the contact such as the // position. This map groups contacts so that it is easy to query all the // contacts of one entity. using EntityContactMap = std::unordered_map>; + std::deque>; // This data structure is essentially a mapping between a pair of entities and // a list of pointers to their contact object. We use a map inside a map to @@ -2827,17 +3088,32 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) auto allContacts = worldCollisionFeature->GetContactsFromLastStep(); for (const auto &contactComposite : allContacts) { + // Get the RequireData const auto &contact = contactComposite.Get(); auto coll1Entity = this->entityCollisionMap.Get(ShapePtrType(contact.collision1)); auto coll2Entity = this->entityCollisionMap.Get(ShapePtrType(contact.collision2)); + // Check the ExpectData + const auto* extraContactData = + contactComposite.Query(); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - entityContactMap[coll1Entity][coll2Entity].push_back(&contact); - entityContactMap[coll2Entity][coll1Entity].push_back(&contact); + AllContactData allContactData; + allContactData.point = &contact; + + if (extraContactData) + allContactData.extra = extraContactData; + + // Note that the ExtraContactData is valid only when the first + // collision is the first body. Quantities like the force and + // the normal must be flipped in the second case. + entityContactMap[coll1Entity][coll2Entity].push_back( + allContactData); + entityContactMap[coll2Entity][coll1Entity].push_back( + allContactData); } } @@ -2868,12 +3144,73 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); contactMsg->mutable_collision2()->set_id(collEntity2); + for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + position->set_x(contact.point->point.x()); + position->set_y(contact.point->point.y()); + position->set_z(contact.point->point.z()); + + if (contact.extra) + { + // Add the penetration depth + contactMsg->add_depth(contact.extra->depth); + + // Get the name of the collisions + auto collisionName1 = + _ecm.Component(_collEntity1)->Data(); + auto collisionName2 = + _ecm.Component(collEntity2)->Data(); + + // Add the wrench (only the force component) + auto* wrench = contactMsg->add_wrench(); + wrench->set_body_1_name(collisionName1); + wrench->set_body_2_name(collisionName2); + auto* wrench1 = wrench->mutable_body_1_wrench(); + auto* wrench2 = wrench->mutable_body_2_wrench(); + + auto* force1 = wrench1->mutable_force(); + auto* force2 = wrench2->mutable_force(); + auto* torque1 = wrench1->mutable_torque(); + auto* torque2 = wrench2->mutable_torque(); + + // The same ContactPoint and ExtraContactData are + // used for the contact between collision1 and + // collision2. In those structures there is some + // data, like the force and normal, that cannot + // commute. + if (_collEntity1 == this->entityCollisionMap.Get( + ShapePtrType(contact.point->collision1))) + { + assert(collEntity2 == this->entityCollisionMap.Get( + ShapePtrType(contact.point->collision2))); + // Use the data as it is + *force1 = msgs::Convert(math::eigen3::convert(contact.extra->force)); + *force2 = msgs::Convert(-math::eigen3::convert(contact.extra->force)); + // Add the wrench normal + auto* normal = contactMsg->add_normal(); + normal->set_x(contact.extra->normal.x()); + normal->set_y(contact.extra->normal.y()); + normal->set_z(contact.extra->normal.z()); + } + else + { + assert(collEntity2 == this->entityCollisionMap.Get( + ShapePtrType(contact.point->collision1))); + // Flip the force + *force1 = msgs::Convert(-math::eigen3::convert(contact.extra->force)); + *force2 = msgs::Convert(math::eigen3::convert(contact.extra->force)); + // Flip the normal + auto* normal = contactMsg->add_normal(); + normal->set_x(-contact.extra->normal.x()); + normal->set_y(-contact.extra->normal.y()); + normal->set_z(-contact.extra->normal.z()); + } + + *torque1 = msgs::Convert(math::Vector3d::Zero); + *torque2 = msgs::Convert(math::Vector3d::Zero); + } } } @@ -2903,3 +3240,4 @@ IGNITION_ADD_PLUGIN(Physics, Physics::ISystemUpdate) IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics") +IGNITION_ADD_PLUGIN_ALIAS(Physics, "scenario::plugins::gazebo::Physics") diff --git a/scenario/src/plugins/Physics/Physics.hh b/scenario/src/plugins/Physics/Physics.hh index 2877bef28..057fe71b3 100644 --- a/scenario/src/plugins/Physics/Physics.hh +++ b/scenario/src/plugins/Physics/Physics.hh @@ -13,10 +13,11 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_ #define IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_ +// clang-format off #include #include #include