Skip to content
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.

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

Merge upstream modifications into vendored Physics system #140

Merged
Merged
Changes from all commits
Commits
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
165 changes: 99 additions & 66 deletions plugins/Physics/Physics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include <ignition/common/MeshManager.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Util.hh>
#include <ignition/gazebo/components/AngularAcceleration.hh>
#include <ignition/gazebo/components/AngularVelocity.hh>
#include <ignition/gazebo/components/BatterySoC.hh>
Expand Down Expand Up @@ -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<ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
Expand Down Expand Up @@ -200,8 +206,12 @@ class Physics::Impl
/// ign-physics.
std::unordered_map<Entity, LinkPtrType> 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<LinkPtrType, Entity> linkEntityMap;

/// \brief A map between collision entity ids in the ECM to Shape Entities
/// in ign-physics.
std::unordered_map<Entity, ShapePtrType> entityCollisionMap;

/// \brief A map between shape entities in ign-physics to collision entities
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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;
});
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}
diegoferigo marked this conversation as resolved.
Show resolved Hide resolved

Expand Down Expand Up @@ -499,6 +517,9 @@ void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm)
auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
auto jointAxis2 = _ecm.Component<components::JointAxis2>(_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)
Expand All @@ -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;
});

Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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 =
Expand All @@ -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());
Expand All @@ -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());
Expand All @@ -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 =
Expand Down Expand Up @@ -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<components::Pose>(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<components::Pose>(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<components::Static>(_entity);
Expand All @@ -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;
}

Expand Down