diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 000000000..4bd4a9aa0 --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f4696b2a..7864ba394 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ gz_find_package(DART utils utils-urdf CONFIG - VERSION 6.9 + VERSION 6.10 REQUIRED_BY dartsim PKGCONFIG dart PKGCONFIG_VER_COMPARISON >=) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 652584f90..8df941a4c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -112,6 +112,24 @@ struct ModelInfo } }; +/// \brief Custom GzMultiBodyLinkCollider class +class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { + using btMultiBodyLinkCollider::btMultiBodyLinkCollider; + + /// \brief Overrides base function to enable support for ignoring + /// collision with objects from other bodies if + /// btCollisionObject::setIgnoreCollisionCheck is called. + /// Note: originally btMultiBodyLinkCollider::checkCollideWithOverride + /// just returns true if the input collision object is from a + /// different body and disregards any setIgnoreCollisionCheck calls. + public: bool checkCollideWithOverride(const btCollisionObject *_co) const + override + { + return btMultiBodyLinkCollider::checkCollideWithOverride(_co) && + btCollisionObject::checkCollideWithOverride(_co); + } +}; + /// Link information is embedded inside the model, so all we need to store here /// is a reference to the model and the index of this link inside of it. struct LinkInfo @@ -120,10 +138,12 @@ struct LinkInfo std::optional indexInModel; Identity model; Eigen::Isometry3d inertiaToLinkFrame; - std::unique_ptr collider = nullptr; + std::unique_ptr collider = nullptr; std::unique_ptr shape = nullptr; std::vector collisionEntityIds = {}; std::unordered_map collisionNameToEntityId = {}; + // Link is either static, fixed to world, or has zero dofs + bool isStaticOrFixed = false; }; struct CollisionInfo diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 4fd79f57f..de0be3dca 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -18,11 +18,67 @@ #include "FreeGroupFeatures.hh" #include +#include namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex) +{ + if (_linkIndex == -1) + { + return _body->getBaseWorldTransform(); + } + else + { + btMultiBodyLinkCollider *collider = _body->getLinkCollider(_linkIndex); + return collider->getWorldTransform(); + } +} + +///////////////////////////////////////////////// +void enforceFixedConstraint( + btMultiBodyFixedConstraint *_fixedConstraint, + const std::unordered_map &_allJoints) +{ + // Update fixed constraint's child link pose to maintain a fixed transform + // from the parent link. + btMultiBody *parent = _fixedConstraint->getMultiBodyA(); + btMultiBody *child = _fixedConstraint->getMultiBodyB(); + + btTransform parentToChildTf; + parentToChildTf.setOrigin(_fixedConstraint->getPivotInA()); + parentToChildTf.setBasis(_fixedConstraint->getFrameInA()); + + int parentLinkIndex = _fixedConstraint->getLinkA(); + int childLinkIndex = _fixedConstraint->getLinkB(); + + btTransform parentLinkTf = getWorldTransformForLink(parent, parentLinkIndex); + btTransform childLinkTf = getWorldTransformForLink(child, childLinkIndex); + + btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf; + btTransform childBaseTf = child->getBaseWorldTransform(); + btTransform childBaseToLink = + childBaseTf.inverse() * childLinkTf; + btTransform newChildBaseTf = + expectedChildLinkTf * childBaseToLink.inverse(); + child->setBaseWorldTransform(newChildBaseTf); + for (const auto &joint : _allJoints) + { + if (joint.second->fixedConstraint) + { + // Recursively enforce constraints where the child here is a parent to + // other constraints. + if (joint.second->fixedConstraint->getMultiBodyA() == child) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), _allJoints); + } + } + } +} + ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const @@ -33,6 +89,20 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); + // Also reject if the model is a child of a fixed constraint + // (detachable joint) + for (const auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get()) + { + return this->GenerateInvalidId(); + } + } + } + + return _modelID; } @@ -69,6 +139,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( if (model) { model->body->setBaseOmega(convertVec(_angularVelocity)); + model->body->wakeUp(); } } @@ -82,6 +153,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( if (model) { model->body->setBaseVel(convertVec(_linearVelocity)); + model->body->wakeUp(); } } @@ -95,6 +167,25 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( { model->body->setBaseWorldTransform( convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + + model->body->wakeUp(); + + for (auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + // Only the parent body of a fixed joint can be moved but we need to + // enforce the fixed constraint by updating the child pose so it + // remains at a fixed transform from parent. We do this recursively to + // account for other constraints attached to the child. + btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); + if (parent == model->body.get()) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), + this->joints); + } + } + } } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 980313a7b..e2c460a32 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,85 @@ namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +void makeColliderStatic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + // if link is already static or fixed, we do not need to change its + // collision flags + if (_linkInfo->isStaticOrFixed) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + childProxy->m_collisionFilterMask = + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif +} + +///////////////////////////////////////////////// +void makeColliderDynamic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + // If broadphase and collision object flags do not agree, the + // link was originally non-static but made static by AttachJoint + if (!_linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + childProxy->m_collisionFilterGroup = + btBroadphaseProxy::DefaultFilter; + childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); +#endif + } +} + +///////////////////////////////////////////////// +void updateColliderFlagsRecursive(std::size_t _linkID, + const std::unordered_map> &_joints, + const std::unordered_map> &_links, + std::function _updateColliderCb) +{ + btMultiBodyFixedConstraint *fixedConstraint = nullptr; + std::size_t childLinkID = 0u; + for (const auto &joint : _joints) + { + if (!joint.second->fixedConstraint) + continue; + if (!joint.second->parentLinkID.has_value() || + joint.second->parentLinkID.value() != _linkID) + continue; + + fixedConstraint = joint.second->fixedConstraint.get(); + childLinkID = std::size_t(joint.second->childLinkID); + } + + if (!fixedConstraint) + return; + + auto childInfo = _links.at(childLinkID); + _updateColliderCb(childInfo.get()); + + updateColliderFlagsRecursive(childLinkID, _joints, _links, _updateColliderCb); +} + ///////////////////////////////////////////////// double JointFeatures::GetJointPosition( const Identity &_id, const std::size_t _dof) const @@ -112,6 +192,7 @@ void JointFeatures::SetJointPosition( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -126,6 +207,7 @@ void JointFeatures::SetJointVelocity( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -156,6 +238,7 @@ void JointFeatures::SetJointForce( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointTorqueMultiDof( identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -282,9 +365,9 @@ void JointFeatures::SetJointVelocityCommand( return; } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (!jointInfo->motor) { - auto modelInfo = this->ReferenceInterface(jointInfo->model); jointInfo->motor = std::make_shared( modelInfo->body.get(), std::get(jointInfo->identifier).indexInBtModel, @@ -296,6 +379,7 @@ void JointFeatures::SetJointVelocityCommand( } jointInfo->motor->setVelocityTarget(static_cast(_value)); + modelInfo->body->wakeUp(); } ///////////////////////////////////////////////// @@ -337,6 +421,31 @@ Identity JointFeatures::AttachFixedJoint( if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); + jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); + + // Make child link static if parent is static + // This is done by updating collision flags + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + // disable collision between parent and child collider + // \todo(iche033) if self collide is true, extend this to + // disable collisions between all the links in the parent's model with + // all the links in the child's model. + parentCollider->setIgnoreCollisionCheck(childCollider, true); + childCollider->setIgnoreCollisionCheck(parentCollider, true); + + // If parent link is static or fixed, recusively update child colliders + // collision flags to be static. + if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed) + { + makeColliderStatic(linkInfo); + updateColliderFlagsRecursive(std::size_t(_childID), + this->joints, this->links, makeColliderStatic); + } + } + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -349,6 +458,37 @@ void JointFeatures::DetachJoint(const Identity &_jointId) auto jointInfo = this->ReferenceInterface(_jointId); if (jointInfo->fixedConstraint) { + // Make links dynamic again as they were originally not static + // This is done by reverting any collision flag changes + // made in AttachJoint + auto *linkInfo = this->ReferenceInterface(jointInfo->childLinkID); + if (jointInfo->parentLinkID.has_value()) + { + auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value()); + + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + parentCollider->setIgnoreCollisionCheck(childCollider, false); + childCollider->setIgnoreCollisionCheck(parentCollider, false); + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (childProxy) + { + // Recursively make child colliders dynamic if they were originally + // not static + if (!linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + makeColliderDynamic(linkInfo); + updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID), + this->joints, this->links, makeColliderDynamic); + } + } + } + } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (modelInfo) { @@ -356,6 +496,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get()); jointInfo->fixedConstraint.reset(); jointInfo->fixedConstraint = nullptr; + modelInfo->body->wakeUp(); } } } @@ -368,11 +509,19 @@ void JointFeatures::SetJointTransformFromParent( if (jointInfo->fixedConstraint) { - auto tf = convertTf(_pose); - jointInfo->fixedConstraint->setPivotInA( - tf.getOrigin()); - jointInfo->fixedConstraint->setFrameInA( - tf.getBasis()); + Eigen::Isometry3d parentInertiaToLinkFrame = Eigen::Isometry3d::Identity(); + if (jointInfo->parentLinkID.has_value()) + { + auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value()); + parentInertiaToLinkFrame = parentLinkInfo->inertiaToLinkFrame; + } + auto *linkInfo = this->ReferenceInterface(jointInfo->childLinkID); + auto tf = convertTf(parentInertiaToLinkFrame * _pose * + linkInfo->inertiaToLinkFrame.inverse()); + jointInfo->fixedConstraint->setPivotInA( + tf.getOrigin()); + jointInfo->fixedConstraint->setFrameInA( + tf.getBasis()); } } diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 85341ce6a..c65cc5b48 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -22,30 +22,40 @@ namespace gz { namespace physics { namespace bullet_featherstone { + +FrameData3d getNonBaseLinkFrameData(const ModelInfo *_modelInfo, + const LinkInfo *_linkInfo) +{ + const auto index = _linkInfo->indexInModel.value(); + FrameData3d data; + data.pose = GetWorldTransformOfLink(*_modelInfo, *_linkInfo); + const auto &link = _modelInfo->body->getLink(index); + data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); + data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); + return data; +} + ///////////////////////////////////////////////// FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( const FrameID &_id) const { - const auto linkIt = this->links.find(_id.ID()); + bool isModel = false; + bool isCollision = false; const ModelInfo *model = nullptr; + Eigen::Isometry3d collisionPoseOffset = Eigen::Isometry3d(); + + const auto linkIt = this->links.find(_id.ID()); if (linkIt != this->links.end()) { const auto &linkInfo = linkIt->second; - const auto indexOpt = linkInfo->indexInModel; model = this->ReferenceInterface(linkInfo->model); - if (indexOpt.has_value()) + if (linkInfo->indexInModel.has_value()) { - const auto index = *indexOpt; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo); - const auto &link = model->body->getLink(index); - data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo.get()); } - // If indexOpt is nullopt then the link is the base link which will be + // If indexInModel is nullopt then the link is the base link which will be // calculated below. } else @@ -59,60 +69,55 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( if (linkIt2 != this->links.end()) { const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; model = this->ReferenceInterface(linkInfo2->model); - - if (indexOpt2.has_value()) + if (linkInfo2->indexInModel.has_value()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo2.get()); } } } - - auto collisionIt = this->collisions.find(_id.ID()); - if (collisionIt != this->collisions.end()) + else { - const auto &collisionInfo = collisionIt->second; - - const auto linkIt2 = this->links.find(collisionInfo->link); - if (linkIt2 != this->links.end()) + auto collisionIt = this->collisions.find(_id.ID()); + if (collisionIt != this->collisions.end()) { - const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; - model = this->ReferenceInterface(linkInfo2->model); + isCollision = true; + const auto &collisionInfo = collisionIt->second; - if (indexOpt2.has_value()) + const auto linkIt2 = this->links.find(collisionInfo->link); + if (linkIt2 != this->links.end()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + const auto &linkInfo2 = linkIt2->second; + model = this->ReferenceInterface(linkInfo2->model); + collisionPoseOffset = collisionInfo->linkToCollision; + if (linkInfo2->indexInModel.has_value()) + { + auto data = getNonBaseLinkFrameData(model, linkInfo2.get()); + data.pose = data.pose * collisionPoseOffset; + return data; + } + } + } + else + { + auto modelIt = this->models.find(_id.ID()); + if (modelIt != this->models.end()) + { + model = modelIt->second.get(); + isModel = true; } } } - - if (!model || model->body == nullptr) - model = this->FrameInterface(_id); } FrameData data; - if(model && model->body) + if (model && model->body) { - data.pose = convert(model->body->getBaseWorldTransform()) - * model->baseInertiaToLinkFrame; + data.pose = convert(model->body->getBaseWorldTransform()); + if (!isModel) + data.pose = data.pose * model->baseInertiaToLinkFrame; + if (isCollision) + data.pose = data.pose * collisionPoseOffset; data.linearVelocity = convert(model->body->getBaseVel()); data.angularVelocity = convert(model->body->getBaseOmega()); } diff --git a/bullet-featherstone/src/KinematicsFeatures.hh b/bullet-featherstone/src/KinematicsFeatures.hh index 259db0dd2..54ff7554c 100644 --- a/bullet-featherstone/src/KinematicsFeatures.hh +++ b/bullet-featherstone/src/KinematicsFeatures.hh @@ -30,7 +30,8 @@ namespace bullet_featherstone { struct KinematicsFeatureList : gz::physics::FeatureList< LinkFrameSemantics, ModelFrameSemantics, - FreeGroupFrameSemantics + FreeGroupFrameSemantics, + ShapeFrameSemantics > { }; class KinematicsFeatures : diff --git a/bullet-featherstone/src/LinkFeatures.cc b/bullet-featherstone/src/LinkFeatures.cc index b8dc44356..29e227d7d 100644 --- a/bullet-featherstone/src/LinkFeatures.cc +++ b/bullet-featherstone/src/LinkFeatures.cc @@ -50,6 +50,7 @@ void LinkFeatures::AddLinkExternalForceInWorld( model->body->addBaseForce(F); model->body->addBaseTorque(relPosWorld.cross(F)); } + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -73,6 +74,7 @@ void LinkFeatures::AddLinkExternalTorqueInWorld( btVector3 torqueWorld = model->body->getBaseWorldTransform().getBasis() * T; model->body->addBaseTorque(torqueWorld); } + model->body->wakeUp(); } } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3ad3b3f5b..e6ee725f7 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1133,6 +1133,8 @@ bool SDFFeatures::AddSdfCollision( &(vertices[0].getX()), vertices.size())); auto *convexShape = this->meshesConvex.back().get(); convexShape->setMargin(collisionMargin); + convexShape->recalcLocalAabb(); + convexShape->optimizeConvexHull(); btTransform trans; trans.setIdentity(); @@ -1270,7 +1272,7 @@ bool SDFFeatures::AddSdfCollision( // NOTE: Bullet does not appear to support different surface properties // for different shapes attached to the same link. - linkInfo->collider = std::make_unique( + linkInfo->collider = std::make_unique( model->body.get(), linkIndexInModel); linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); @@ -1337,6 +1339,12 @@ bool SDFFeatures::AddSdfCollision( linkInfo->collider.get(), btBroadphaseProxy::StaticFilter, btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + linkInfo->isStaticOrFixed = true; + + // Set collider collision flags +#if BT_BULLET_VERSION >= 307 + linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif } else { diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 038ad8357..e702a9eac 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -37,8 +37,6 @@ AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox( t.setIdentity(); btVector3 minBox(0, 0, 0); btVector3 maxBox(0, 0, 0); - btVector3 minBox2(0, 0, 0); - btVector3 maxBox2(0, 0, 0); collider->collider->getAabb(t, minBox, maxBox); return math::eigen3::convert(math::AxisAlignedBox( math::Vector3d(minBox[0], minBox[1], minBox[2]), diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a34a6741c..a32d93d2c 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -46,25 +46,6 @@ void SimulationFeatures::WorldForwardStep( worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, static_cast(this->stepSize)); - for (auto & m : this->models) - { - if (m.second->body) - { - m.second->body->checkMotionAndSleepIfRequired( - static_cast(this->stepSize)); - btMultiBodyLinkCollider* col = m.second->body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState(ACTIVE_TAG); - - for (int b = 0; b < m.second->body->getNumLinks(); b++) - { - col = m.second->body->getLink(b).m_collider; - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState(ACTIVE_TAG); - } - } - } - this->WriteRequiredData(_h); this->Write(_h.Get()); } diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 47caa9b0b..9475ef984 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -165,6 +166,45 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const this->prevLinkPoses = std::move(newPoses); } +SimulationFeatures::RayIntersection +SimulationFeatures::GetRayIntersectionFromLastStep( + const Identity &_worldID, + const LinearVector3d &_from, + const LinearVector3d &_to) const +{ + auto *const world = this->ReferenceInterface(_worldID); + auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); + auto collisionGroup = world->getConstraintSolver()->getCollisionGroup().get(); + + // Perform raycast + dart::collision::RaycastOption option; + dart::collision::RaycastResult result; + collisionDetector->raycast(collisionGroup, _from, _to, option, &result); + + // Currently, raycast supports only the Bullet collision detector. + // For other collision detectors, the result will always be NaN. + SimulationFeatures::RayIntersection intersection; + if (result.hasHit()) + { + // Store intersection data if there is a ray hit + const auto &firstHit = result.mRayHits[0]; + intersection.point = firstHit.mPoint; + intersection.normal = firstHit.mNormal; + intersection.fraction = firstHit.mFraction; + } + else + { + // Set invalid measurements to NaN according to REP-117 + intersection.point = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + intersection.normal = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + intersection.fraction = std::numeric_limits::quiet_NaN(); + } + + return intersection; +} + std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index c13873837..a3365feb8 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -56,7 +57,8 @@ struct SimulationFeatureList : FeatureList< #ifdef DART_HAS_CONTACT_SURFACE SetContactPropertiesCallbackFeature, #endif - GetContactsFromLastStepFeature + GetContactsFromLastStepFeature, + GetRayIntersectionFromLastStepFeature > { }; #ifdef DART_HAS_CONTACT_SURFACE @@ -97,6 +99,9 @@ class SimulationFeatures : public: using GetContactsFromLastStepFeature::Implementation ::ContactInternal; + public: using GetRayIntersectionFromLastStepFeature::Implementation< + FeaturePolicy3d>::RayIntersection; + public: SimulationFeatures() = default; public: ~SimulationFeatures() override = default; @@ -113,6 +118,11 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; + public: RayIntersection GetRayIntersectionFromLastStep( + const Identity &_worldID, + const LinearVector3d &_from, + const LinearVector3d &_end) const override; + /// \brief link poses from the most recent pose change/update. /// The key is the link's ID, and the value is the link's pose private: mutable std::unordered_map prevLinkPoses; diff --git a/include/gz/physics/GetRayIntersection.hh b/include/gz/physics/GetRayIntersection.hh new file mode 100644 index 000000000..1c9e31223 --- /dev/null +++ b/include/gz/physics/GetRayIntersection.hh @@ -0,0 +1,86 @@ +/* + * 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_PHYSICS_GETRAYINTERSECTION_HH_ +#define GZ_PHYSICS_GETRAYINTERSECTION_HH_ + +#include +#include +#include +#include + +namespace gz +{ +namespace physics +{ +/// \brief GetRayIntersectionFromLastStepFeature is a feature for retrieving +/// the ray intersection generated in the previous simulation step. +class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature + : public virtual FeatureWithRequirements +{ + public: template + struct RayIntersectionT + { + public: using Scalar = typename PolicyT::Scalar; + public: using VectorType = + typename FromPolicy::template Use; + + /// \brief The hit point in the world coordinates + VectorType point; + + /// \brief The fraction of the ray length at the intersection/hit point. + Scalar fraction; + + /// \brief The normal at the hit point in the world coordinates + VectorType normal; + }; + + public: template + class World : public virtual Feature::World + { + public: using VectorType = + typename FromPolicy::template Use; + public: using RayIntersection = RayIntersectionT; + public: using RayIntersectionData = + SpecifyData>; + + /// \brief Get ray intersection generated in the previous simulation step + /// \param[in] _from The start point of the ray in world coordinates + /// \param[in] _to The end point of the ray in world coordinates + public: RayIntersectionData GetRayIntersectionFromLastStep( + const VectorType &_from, const VectorType &_to) const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using RayIntersection = RayIntersectionT; + public: using VectorType = + typename FromPolicy::template Use; + + public: virtual RayIntersection GetRayIntersectionFromLastStep( + const Identity &_worldID, + const VectorType &_from, + const VectorType &_to) const = 0; + }; +}; +} +} + +#include "gz/physics/detail/GetRayIntersection.hh" + +#endif /* end of include guard: GZ_PHYSICS_GETRAYINTERSECTION_HH_ */ diff --git a/include/gz/physics/detail/GetRayIntersection.hh b/include/gz/physics/detail/GetRayIntersection.hh new file mode 100644 index 000000000..8448ea608 --- /dev/null +++ b/include/gz/physics/detail/GetRayIntersection.hh @@ -0,0 +1,49 @@ +/* + * 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_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_ +#define GZ_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_ + +#include +#include + +namespace gz +{ +namespace physics +{ +///////////////////////////////////////////////// +template +auto GetRayIntersectionFromLastStepFeature::World< + PolicyT, FeaturesT>::GetRayIntersectionFromLastStep( + const VectorType &_from, + const VectorType &_to) const -> RayIntersectionData +{ + auto result = + this->template Interface() + ->GetRayIntersectionFromLastStep(this->identity, _from, _to); + + RayIntersection intersection{result.point, result.fraction, result.normal}; + + RayIntersectionData output; + output.template Get() = std::move(intersection); + return output; +} + +} // namespace physics +} // namespace gz + +#endif diff --git a/include/gz/physics/detail/InspectFeatures.hh b/include/gz/physics/detail/InspectFeatures.hh index 4a02210f2..ca58e6d4c 100644 --- a/include/gz/physics/detail/InspectFeatures.hh +++ b/include/gz/physics/detail/InspectFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_DETAIL_INSPECTFEATURES_HH #define GZ_PHYSICS_DETAIL_INSPECTFEATURES_HH +#include #include #include #include diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..e190f27d5 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + gz-physics8 + 8.0.0 + Gazebo Physics : Physics classes and functions for robot applications + Steve Peters + Apache License 2.0 + https://github.com/gazebosim/gz-physics + + cmake + + gz-cmake4 + + benchmark + bullet + DART + eigen + gz-common6 + gz-math8 + gz-plugin3 + gz-utils3 + sdformat15 + + + cmake + + diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 7752edfd1..72c99849b 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -76,6 +76,11 @@ foreach(test ${tests}) "BT_BULLET_VERSION_LE_325" ) endif() + if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.06) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_LE_306" + ) + endif() if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE) diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 7ac4284d5..326ac201d 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -36,6 +36,8 @@ const auto kFallingWorld = CommonTestWorld("falling.world"); const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world"); const auto kGroundSdf = CommonTestWorld("ground.sdf"); const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); +const auto kJointAcrossModelsFixedSdf = + CommonTestWorld("joint_across_models_fixed.sdf"); const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); const auto kMimicFastSlowPendulumsWorld = CommonTestWorld("mimic_fast_slow_pendulums_world.sdf"); @@ -45,6 +47,7 @@ const auto kMimicUniversalWorld = CommonTestWorld("mimic_universal_world.sdf"); const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); const auto kPendulumJointWrenchSdf = CommonTestWorld("pendulum_joint_wrench.sdf"); +const auto kPoseOffsetSdf = CommonTestWorld("pose_offset.sdf"); const auto kShapesWorld = CommonTestWorld("shapes.world"); const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index b19f0e980..ab643e4bf 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -14,6 +14,10 @@ * limitations under the License. * */ + +#include +#include +#include #include #include @@ -22,6 +26,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -1146,22 +1151,396 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } // After a while, body2 should reach the ground and come to a stop - std::size_t stepCount = 0u; - const std::size_t maxNumSteps = 2000u; - while (stepCount++ < maxNumSteps) + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + } +} + +///////////////////////////////////////////////// +// Attach a fixed joint between links from non-static models to a link +// from a model fixed to the world. Verify the models are not moving when +// the fixed joint is attached. +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint - model1 body is the parent + auto poseParent = frameDataModel1Body.pose; + auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint13 = model3Body->AttachFixedJoint(model1Body); + fixedJoint13->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So it tries to resolve overlapping bodies held together by + // a fixed joint. Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 1e-2; +#endif + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So overlapping bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif + { + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); + } + } + + // now detach joint and expect model2 and model3 to start moving + fixedJoint12->Detach(); + fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Test attaching fixed joint with reverse the parent and child + // relationship - model2 body is now the parent and model1 is child and + // fixed to world + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel1Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12b = model1Body->AttachFixedJoint(model2Body); + fixedJoint12b->SetTransformFromParent(poseParentChild); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect both models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + } + + // detach joint and expect model2 to start falling again + fixedJoint12b->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect the model1 to still be fixed to the world and model2 + // to start falling + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + // Negative z velocity + EXPECT_GT(0.0, body2LinearVelocity.Z()); + } + } +} + +///////////////////////////////////////////////// +// Create a chain of models by attaching them with fixed joints: +// M1 (static) -> M2 (dynamic) -> M3 (dynamic) +// Verify that M2 and M3 become static once attached to M1 +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachChain) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint between M2 (dynamic) and M3 (dynamic) + auto poseParent = frameDataModel2Body.pose; + auto poseChild = frameDataModel3Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint23 = model3Body->AttachFixedJoint(model2Body); + fixedJoint23->SetTransformFromParent(poseParentChild); + + // attach the fixed joint between M1 (static) and M2 (dynamic) + // this should recusively make M2 and M3 static + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - // Expected Z height of model2 is 0.75 when both boxes are stacked on top - // of each other since each is 0.5 high. - if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 1e-2 && - fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3) + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 0.1; +#endif + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), tol); + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif { - break; + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } } - EXPECT_GT(stepCount, 1u); - EXPECT_LT(stepCount, maxNumSteps); + + // Now detach joint between M2 (dynamic) and M3 (dynamic) + // Expect M2 to be static as it is still attached to M1 (static) + // Expect M3 to start moving + fixedJoint23->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Now detach joint between M1 (static) and M2 (dynamic) + // Expect M2 to start falling + // Expect M3 to continue moving + fixedJoint12->Detach(); + // fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } } } @@ -1269,8 +1648,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) EXPECT_EQ(initialModel3Pose, gz::math::eigen3::convert(frameDataModel3Body.pose)); - // Step through initial transients const std::size_t numSteps = 100; + /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -1283,18 +1662,15 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect all the bodies to be at rest. // (since they're held in place by the joints) - { - world->Step(output, state, input); - EXPECT_NEAR(initialModel1Pose.Z(), - frameDataModel1Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel2Pose.Z(), - frameDataModel2Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel3Pose.Z(), - frameDataModel3Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 3e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel3Body.linearVelocity.z(), 3e-3); - } + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1302,13 +1678,6 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) fixedJoint_m2m1->Detach(); fixedJoint_m2m3->Detach(); - frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); - frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); - const double preStepBody1LinearVelocityZ = - frameDataModel1Body.linearVelocity.z(); - const double preStepBody3LinearVelocityZ = - frameDataModel3Body.linearVelocity.z(); - /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { @@ -1322,11 +1691,16 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect the middle box to be still as it is already at rest. // Expect the two side boxes to fall away. - EXPECT_NEAR(preStepBody1LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel1Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(preStepBody3LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel3Body.linearVelocity.z(), 1e-3); + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + + EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 1e-3); } } } @@ -1638,6 +2012,124 @@ TEST_F(WorldModelTest, JointSetCommand) } } +using FixedJointFreeGroupFeatureList = gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::AttachFixedJointFeature, + gz::physics::DetachJointFeature, + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld +>; + +using FixedJointFreeGroupFeatureTestTypes = + JointFeaturesTest; + +TEST_F(FixedJointFreeGroupFeatureTestTypes, FixedJointFreeGroupMove) +{ + // Attach joint between links from 2 models with fixed joint. Move + // parent model using free group and verify child body moves along with the + // parent link. + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d::From( + plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + + const gz::math::Pose3d model1Pose(0, 0, 0.25, 0, 0, 0.0); + const gz::math::Pose3d model2Pose(0, 1, 0.25, 0, 0, 0.0); + auto freeGroupM1 = model1->FindFreeGroup(); + auto freeGroupM2 = model2->FindFreeGroup(); + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(model1Pose)); + freeGroupM2->SetWorldPose(gz::math::eigen3::convert(model2Pose)); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(model1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(model2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect the model1 and model2 to stay at rest + // (since they are on the ground) + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + } + + // Attach fixed joint + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + fixedJoint->SetTransformFromParent(poseParentChild); + + gz::math::Pose3d poseToMoveModel(1, 0, 0, 0, 0, 0.0); + gz::math::Pose3d newModel1Pose = poseToMoveModel * model1Pose; + gz::math::Pose3d newModel2Pose = poseToMoveModel * model2Pose; + + // Move parent model using free group + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Parent should be at the new pose and the child should follow + EXPECT_EQ(newModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(newModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index effa773dc..3b5459bee 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -29,6 +30,7 @@ #include #include #include +#include #include #include @@ -72,11 +74,13 @@ struct KinematicFeaturesList : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::ForwardStep, gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetShapeFromLink, gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::GetJointFromModel, + gz::physics::JointFrameSemantics, gz::physics::LinkFrameSemantics, - gz::physics::JointFrameSemantics + gz::physics::ShapeFrameSemantics > { }; using KinematicFeaturesTestTypes = @@ -146,12 +150,18 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) EXPECT_EQ( F_WCexpected.pose.rotation(), childLinkFrameData.pose.rotation()); - // TODO(ahcorde): Rewiew this in bullet-featherstone - if(this->PhysicsEngineName(name) == "bullet_featherstone") + // TODO(ahcorde): Review this in bullet-featherstone + if (this->PhysicsEngineName(name) != "bullet-featherstone") { - EXPECT_EQ( - F_WCexpected.pose.translation(), - childLinkFrameData.pose.translation()); + EXPECT_NEAR( + F_WCexpected.pose.translation().x(), + childLinkFrameData.pose.translation().x(), 1e-6); + EXPECT_NEAR( + F_WCexpected.pose.translation().y(), + childLinkFrameData.pose.translation().y(), 1e-6); + EXPECT_NEAR( + F_WCexpected.pose.translation().z(), + childLinkFrameData.pose.translation().z(), 1e-6); } EXPECT_TRUE( gz::physics::test::Equal( @@ -166,6 +176,70 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) } } +TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load( + common_test::worlds::kPoseOffsetSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("model"); + ASSERT_NE(nullptr, model); + auto baseLink = model->GetLink("base"); + ASSERT_NE(nullptr, baseLink); + auto nonBaseLink = model->GetLink("link"); + ASSERT_NE(nullptr, nonBaseLink); + auto baseCol = baseLink->GetShape("base_collision"); + ASSERT_NE(nullptr, baseCol); + auto linkCol = nonBaseLink->GetShape("link_collision"); + ASSERT_NE(nullptr, linkCol); + + gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); + auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); + auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); + gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); + gz::math::Pose3d expectedLinkWorldPose = + actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, baseLinkPose); + + auto baseColFrameData = baseCol->FrameDataRelativeToWorld(); + auto baseColPose = gz::math::eigen3::convert(baseColFrameData.pose); + gz::math::Pose3d actualColLocalPose(0, 0, 0.01, 0, 0, 0); + gz::math::Pose3d expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), baseColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + baseColPose.Rot().Euler()); + + auto nonBaseLinkFrameData = nonBaseLink->FrameDataRelativeToWorld(); + auto nonBaseLinkPose = gz::math::eigen3::convert(nonBaseLinkFrameData.pose); + actualLinkLocalPose = gz::math::Pose3d (0, 0, 2.1, -1.5708, 0, 0); + expectedLinkWorldPose = actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, nonBaseLinkPose); + + auto linkColFrameData = linkCol->FrameDataRelativeToWorld(); + auto linkColPose = gz::math::eigen3::convert(linkColFrameData.pose); + actualColLocalPose = gz::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0); + expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + linkColPose.Rot().Euler()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index adcb54817..5f5ae3112 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -293,14 +293,26 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) } } -TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) +using LinkBoundingBoxFeaturesList = gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetEntities, + gz::physics::GetLinkBoundingBox, + gz::physics::GetModelBoundingBox +>; + +using LinkBoundingBoxFeaturesTestTypes = + LinkFeaturesTest; + +TEST_F(LinkBoundingBoxFeaturesTestTypes, AxisAlignedBoundingBox) { for (const std::string &name : this->pluginNames) { std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - auto engine = gz::physics::RequestEngine3d::From(plugin); + auto engine = + gz::physics::RequestEngine3d::From(plugin); ASSERT_NE(nullptr, engine); sdf::Root root; @@ -313,9 +325,6 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("double_pendulum_with_base"); auto baseLink = model->GetLink("base"); auto bbox = baseLink->GetAxisAlignedBoundingBox(); @@ -364,9 +373,6 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("sphere"); auto bbox = model->GetAxisAlignedBoundingBox(); AssertVectorApprox vectorPredicate(1e-4); diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index ec0c05072..bc30de759 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -44,6 +44,7 @@ #include #include #include +#include #include "gz/physics/SphereShape.hh" #include @@ -1295,6 +1296,103 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions) } } +///////////////////////////////////////////////// +// The features that an engine must have to be loaded by this loader. +struct FeaturesRayIntersections : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetRayIntersectionFromLastStepFeature, + gz::physics::CollisionDetector, + gz::physics::ForwardStep +> {}; + +template +class SimulationFeaturesRayIntersectionTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesRayIntersectionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest, + SimulationFeaturesRayIntersectionTestTypes); + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) +{ + std::vector supportedCollisionDetectors = {"bullet"}; + + for (const std::string &name : this->pluginNames) + { + CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe") + + for (const std::string &collisionDetector : supportedCollisionDetectors) { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + // ray hits the sphere + auto result = + world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)); + + auto rayIntersection = + result.template + Get::RayIntersection>(); + + double epsilon = 1e-3; + EXPECT_TRUE( + rayIntersection.point.isApprox(Eigen::Vector3d(-1, 0, 2), epsilon)); + EXPECT_TRUE( + rayIntersection.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon)); + EXPECT_DOUBLE_EQ(rayIntersection.fraction, 0.25); + + // ray does not hit the sphere + result = world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10)); + rayIntersection = + result.template + Get::RayIntersection>(); + + ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); + ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); + ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + } + } +} + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections) +{ + std::vector unsupportedCollisionDetectors = {"ode", "dart", "fcl", "banana"}; + + for (const std::string &name : this->pluginNames) + { + CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe") + + for (const std::string &collisionDetector : unsupportedCollisionDetectors) { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + // ray would hit the sphere, but the collision detector does + // not support ray intersection + auto result = world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)); + + auto rayIntersection = + result.template + Get::RayIntersection>(); + + ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); + ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); + ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + } + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/detachable_joint.world b/test/common_test/worlds/detachable_joint.world index b7e4c5310..e6922e931 100644 --- a/test/common_test/worlds/detachable_joint.world +++ b/test/common_test/worlds/detachable_joint.world @@ -101,6 +101,7 @@ 1.0 + 0.0 0 0.3 0 0 0 0.0068 0 diff --git a/test/common_test/worlds/joint_across_models_fixed.sdf b/test/common_test/worlds/joint_across_models_fixed.sdf new file mode 100644 index 000000000..a6e355550 --- /dev/null +++ b/test/common_test/worlds/joint_across_models_fixed.sdf @@ -0,0 +1,145 @@ + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + world + body + + + + 0 0 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0.3 2 3.0 0 0.0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf new file mode 100644 index 000000000..97b2145cd --- /dev/null +++ b/test/common_test/worlds/pose_offset.sdf @@ -0,0 +1,46 @@ + + + + + 1 0 0 0 0 0 + + 0 1 0 0 0 0 + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + base + link + + 1.0 0 0 + + + + +