diff --git a/plugins/Physics/Physics.cpp b/plugins/Physics/Physics.cpp index b48b51bc1..8050df0b0 100644 --- a/plugins/Physics/Physics.cpp +++ b/plugins/Physics/Physics.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -113,25 +114,30 @@ using namespace ignition::gazebo::components; class Physics::Impl { public: - using MinimumFeatureList = ignition::physics::FeatureList< // - ignition::physics::FindFreeGroupFeature, - ignition::physics::SetFreeGroupWorldPose, - ignition::physics::SetFreeGroupWorldVelocity, - ignition::physics::AddLinkExternalForceTorque, - ignition::physics::ForwardStep, - ignition::physics::GetEntities, - ignition::physics::GetContactsFromLastStepFeature, - ignition::physics::RemoveEntities, - ignition::physics::mesh::AttachMeshShapeFeature, - ignition::physics::GetBasicJointProperties, - ignition::physics::GetBasicJointState, - ignition::physics::SetBasicJointState, - ignition::physics::SetJointVelocityCommandFeature, - ignition::physics::sdf::ConstructSdfCollision, - ignition::physics::sdf::ConstructSdfJoint, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfWorld>; + struct MinimumFeatureList + : ignition::physics::FeatureList< // + ignition::physics::FindFreeGroupFeature, + ignition::physics::SetFreeGroupWorldPose, + ignition::physics::FreeGroupFrameSemantics, + ignition::physics::LinkFrameSemantics, + ignition::physics::SetFreeGroupWorldVelocity, + ignition::physics::AddLinkExternalForceTorque, + ignition::physics::ForwardStep, + ignition::physics::GetEntities, + ignition::physics::GetContactsFromLastStepFeature, + ignition::physics::RemoveEntities, + ignition::physics::mesh::AttachMeshShapeFeature, + ignition::physics::GetBasicJointProperties, + ignition::physics::GetBasicJointState, + ignition::physics::SetBasicJointState, + ignition::physics::SetJointVelocityCommandFeature, + ignition::physics::sdf::ConstructSdfCollision, + ignition::physics::sdf::ConstructSdfJoint, + ignition::physics::sdf::ConstructSdfLink, + ignition::physics::sdf::ConstructSdfModel, + ignition::physics::sdf::ConstructSdfVisual, + ignition::physics::sdf::ConstructSdfWorld> + {}; using EnginePtrType = ignition::physics::EnginePtr; @@ -200,8 +206,12 @@ class Physics::Impl /// ign-physics. std::unordered_map entityLinkMap; - /// \brief A map between collision entity ids in the ECM to Shape Entities in - /// ign-physics. + /// \brief Reverse of entityLinkMap. This is used for finding the Entity + /// associated with a physics Link + std::unordered_map linkEntityMap; + + /// \brief A map between collision entity ids in the ECM to Shape Entities + /// in ign-physics. std::unordered_map entityCollisionMap; /// \brief A map between shape entities in ign-physics to collision entities @@ -282,9 +292,10 @@ void Physics::Update(const UpdateInfo& _info, EntityComponentManager& _ecm) this->pImpl->UpdateSim(_ecm); } - // Entities scheduled to be removed should be removed from physics after the - // simulation step. Otherwise, since the to-be-removed entity still shows up - // in the ECM::Each the UpdatePhysics and UpdateSim calls will have an error + // Entities scheduled to be removed should be removed from physics after + // the simulation step. Otherwise, since the to-be-removed entity still + // shows up in the ECM::Each the UpdatePhysics and UpdateSim calls will + // have an error this->pImpl->RemovePhysicsEntities(_ecm); } } @@ -388,6 +399,7 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) auto linkPtrPhys = modelPtrPhys->ConstructLink(link); this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys)); + this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity)); return true; }); @@ -421,7 +433,12 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) } auto linkPtrPhys = this->entityLinkMap.at(_parent->Data()); - const sdf::Collision& collision = _collElement->Data(); + // 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. + sdf::Collision collision = _collElement->Data(); + collision.SetRawPose(_pose->Data()); + collision.SetPoseRelativeTo(""); ShapePtrType collisionPtrPhys; if (_geom->Data().Type() == sdf::GeometryType::MESH) { @@ -433,9 +450,10 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) } auto& meshManager = *ignition::common::MeshManager::Instance(); - auto* mesh = meshManager.Load(meshSdf->Uri()); + auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); + auto* mesh = meshManager.Load(fullPath); if (nullptr == mesh) { - ignwarn << "Failed to load mesh from [" << meshSdf->Uri() << "]." << std::endl; + ignwarn << "Failed to load mesh from [" << fullPath << "]." << std::endl; return true; } @@ -499,6 +517,9 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) auto jointAxis = _ecm.Component(_entity); auto jointAxis2 = _ecm.Component(_entity); + // Since we're making copies of the joint axes that were created using + // `Model::Load`, frame semantics should work for resolving their xyz + // axis if (jointAxis) joint.SetAxis(0, jointAxis->Data()); if (jointAxis2) @@ -507,7 +528,11 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm) // Use the parent link's parent model as the model of this joint auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint); - this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys)); + if (jointPtrPhys.Valid()) { + // Some joints may not be supported, so only add them to the map if + // the physics entity is valid + this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys)); + } return true; }); @@ -543,6 +568,12 @@ void Physics::Impl::RemovePhysicsEntities(const EntityComponentManager& _ecm) this->entityCollisionMap.erase(collIt); } } + // First erase the entry associated with this link from the + // linkEntityMap which is the reverse of entityLinkMap + auto linkPhysIt = this->entityLinkMap.find(childLink); + if (linkPhysIt != this->entityLinkMap.end()) { + this->linkEntityMap.erase(linkPhysIt->second); + } this->entityLinkMap.erase(childLink); } @@ -597,12 +628,11 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) auto& jointVelocity = velReset->Data(); if (jointVelocity.size() != jointIt->second->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom " - << "between Joint [" << _name->Data() << "(Entity=" << _entity - << ")] and its JointVelocityReset " - << "component. The joint has " << jointVelocity.size() - << " while the component has " << jointIt->second->GetDegreesOfFreedom() - << ".\n"; + ignwarn << "There is a mismatch in the degrees of freedom between " + << "Joint [" << _name->Data() << "(Entity=" << _entity + << ")] and its JointForceCmd component. The joint has " + << jointIt->second->GetDegreesOfFreedom() << " while the " + << " component has " << jointVelocity.size() << ".\n"; } std::size_t nDofs = @@ -618,12 +648,11 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) auto& jointPosition = posReset->Data(); if (jointPosition.size() != jointIt->second->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom " - << "between Joint [" << _name->Data() << "(Entity=" << _entity - << ")] and its JointPositionyReset " - << "component. The joint has " << jointPosition.size() - << " while the component has " << jointIt->second->GetDegreesOfFreedom() - << ".\n"; + ignwarn << "There is a mismatch in the degrees of freedom between " + << "Joint [" << _name->Data() << "(Entity=" << _entity + << ")] and its JointForceCmd component. The joint has " + << jointIt->second->GetDegreesOfFreedom() << " while the " + << " component has " << jointPosition.size() << ".\n"; } std::size_t nDofs = std::min(jointPosition.size(), jointIt->second->GetDegreesOfFreedom()); @@ -640,8 +669,8 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) ignwarn << "There is a mismatch in the degrees of freedom between " << "Joint [" << _name->Data() << "(Entity=" << _entity << ")] and its JointForceCmd component. The joint has " - << force->Data().size() << " while the component has " - << jointIt->second->GetDegreesOfFreedom() << ".\n"; + << jointIt->second->GetDegreesOfFreedom() << " while the " + << " component has " << force->Data().size() << ".\n"; } std::size_t nDofs = std::min(force->Data().size(), jointIt->second->GetDegreesOfFreedom()); @@ -663,12 +692,11 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) } else { if (velocityCmd.size() != jointIt->second->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom" - << " between Joint [" << _name->Data() << "(Entity=" << _entity - << ")] and its " - << "JointVelocityCmd component. The joint has " - << velocityCmd.size() << " while the component has " - << jointIt->second->GetDegreesOfFreedom() << ".\n"; + ignwarn << "There is a mismatch in the degrees of freedom between" + << " Joint [" << _name->Data() << "(Entity=" << _entity + << ")] and its JointVelocityCmd component. The joint has " + << jointIt->second->GetDegreesOfFreedom() << " while the " + << " component has " << velCmd->Data().size() << ".\n"; } std::size_t nDofs = @@ -707,22 +735,24 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) if (modelIt == this->entityModelMap.end()) return true; - // Get canonical link offset - auto canonicalLink = _ecm.ChildrenByComponents(_entity, components::CanonicalLink()); - if (canonicalLink.empty()) - return true; - - auto canonicalPoseComp = _ecm.Component(canonicalLink[0]); - if (nullptr == canonicalPoseComp) - return true; + // The canonical link as specified by sdformat is different from the + // canonical link of the FreeGroup object // TODO(addisu) Store the free group instead of searching for it at // every iteration auto freeGroup = modelIt->second->FindFreeGroup(); - if (freeGroup) { - freeGroup->SetWorldPose( - math::eigen3::convert(_poseCmd->Data() * canonicalPoseComp->Data())); - } + if (!freeGroup) + return true; + + // Get canonical link offset + auto linkEntityIt = this->linkEntityMap.find(freeGroup->CanonicalLink()); + if (linkEntityIt == this->linkEntityMap.end()) + return true; + + auto canonicalPoseComp = _ecm.Component(linkEntityIt->second); + + freeGroup->SetWorldPose( + math::eigen3::convert(_poseCmd->Data() * canonicalPoseComp->Data())); // Process pose commands for static models here, as one-time changes const components::Static* staticComp = _ecm.Component(_entity); @@ -749,16 +779,19 @@ void Physics::Impl::UpdatePhysics(EntityComponentManager& _ecm) if (modelIt == this->entityModelMap.end()) return true; + // The canonical link as specified by sdformat is different from the + // canonical link of the FreeGroup object + // TODO(addisu) Store the free group instead of searching for it at // every iteration - auto freeGroup = modelIt->second->FindFreeGroup(); - // The FreeGroup is created only for floating-base object that do not have any defined - // joint between the world and their base + // The FreeGroup is created only for floating-base object that do + // not have any defined joint between the world and their base + auto freeGroup = modelIt->second->FindFreeGroup(); if (!freeGroup) { - ignwarn - << "Failed to find FreeGroup. Linear and angular velocities commands ignored." - << std::endl; + ignwarn << "Failed to find FreeGroup. Linear and angular " + "velocities commands ignored." + << std::endl; return true; }