diff --git a/Changelog.md b/Changelog.md index 3caed7539..5e698183b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,53 @@ ## Gazebo Physics 7.x +### Gazebo Physics 7.3.0 (2024-06-25) + +1. Backport: Add Cone as a collision shape + * [Pull request #639](https://github.com/gazebosim/gz-physics/pull/639) + +1. [featherstone] Publish JointFeedback forces. + * [Pull request #628](https://github.com/gazebosim/gz-physics/pull/628) + +1. Parse voxel resolution when decomposing meshes + * [Pull request #655](https://github.com/gazebosim/gz-physics/pull/655) + +1. bullet-featherstone: Fix attaching fixed joint between models with inertial pose offset + * [Pull request #653](https://github.com/gazebosim/gz-physics/pull/653) + +1. Ray intersection simulation feature + * [Pull request #641](https://github.com/gazebosim/gz-physics/pull/641) + +1. bullet-featherstone: Fix bounding box for collisions with pose offset + * [Pull request #647](https://github.com/gazebosim/gz-physics/pull/647) + +1. bullet-featherstone: Update fixed constraint behavior + * [Pull request #632](https://github.com/gazebosim/gz-physics/pull/632) + +1. Update InspectFeatures.hh + * [Pull request #645](https://github.com/gazebosim/gz-physics/pull/645) + +1. bullet-featherstone: Fix convex hull shape's AABB + * [Pull request #637](https://github.com/gazebosim/gz-physics/pull/637) + +1. Add package.xml + * [Pull request #608](https://github.com/gazebosim/gz-physics/pull/608) + +1. bullet-featurestore: Enable auto deactivation + * [Pull request #630](https://github.com/gazebosim/gz-physics/pull/630) + +1. bullet-featherstone: Support convex decomposition for meshes + * [Pull request #606](https://github.com/gazebosim/gz-physics/pull/606) + +1. backport bullet-featherstone solver iters + * [Pull request #619](https://github.com/gazebosim/gz-physics/pull/619) + +1. bullet-featherstone: fix SetWorldPose with off-diagonal moment of inertia + * [Pull request #623](https://github.com/gazebosim/gz-physics/pull/623) + +1. Revert "Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS (#613)" + * [Pull request #615](https://github.com/gazebosim/gz-physics/pull/615) + ### Gazebo Physics 7.2.0 (2024-04-10) 1. Use relative install paths for plugin shared libraries diff --git a/bullet-featherstone/README.md b/bullet-featherstone/README.md index 932842b68..8b3325de5 100644 --- a/bullet-featherstone/README.md +++ b/bullet-featherstone/README.md @@ -2,7 +2,7 @@ This component enables the use of this [Bullet Physics](https://github.com/bulletphysics/bullet3) `btMultiBody` Featherstone implementation. The Featherstone uses minimal coordinates to represent articulated bodies and efficiently simulate them. - + # Features Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gazebosim/gz-physics/issues/423) @@ -11,7 +11,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz * Constructing SDF Models * Constructing SDF Worlds -* Joint Types: +* Joint Types: * Fixed * Prismatic * Revolute @@ -25,7 +25,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz These are the specific physics API features implemented. -* Entity Management Features +* Entity Management Features * ConstructEmptyWorldFeature * GetEngineInfo * GetJointFromModel @@ -47,6 +47,9 @@ These are the specific physics API features implemented. * SetBasicJointState * GetBasicJointProperties * SetJointVelocityCommandFeature + * SetJointPositionLimitsFeature, + * SetJointVelocityLimitsFeature, + * SetJointEffortLimitsFeature, * SetJointTransformFromParentFeature * AttachFixedJointFeature * DetachJointFeature diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 8df941a4c..fcf82e70d 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -76,6 +76,8 @@ struct WorldInfo std::unordered_map modelNameToEntityId; int nextModelIndex = 0; + double stepSize = 0.001; + explicit WorldInfo(std::string name); }; @@ -189,7 +191,15 @@ struct JointInfo Identity model; // This field gets set by AddJoint std::size_t indexInGzModel = 0; - btScalar effort = 0; + + // joint limits + // \todo(iche033) Extend to support joints with more than 1 dof + double minEffort = 0.0; + double maxEffort = 0.0; + double minVelocity = 0.0; + double maxVelocity = 0.0; + double axisLower = 0.0; + double axisUpper = 0.0; std::shared_ptr motor = nullptr; std::shared_ptr jointLimits = nullptr; diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index e2c460a32..59d5c65a8 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -18,6 +18,8 @@ #include "JointFeatures.hh" #include +#include +#include #include #include @@ -28,6 +30,29 @@ namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +void recreateJointLimitConstraint(JointInfo *_jointInfo, ModelInfo *_modelInfo, + WorldInfo *_worldInfo) +{ + const auto *identifier = std::get_if(&_jointInfo->identifier); + if (!identifier) + return; + + if (_jointInfo->jointLimits) + { + _worldInfo->world->removeMultiBodyConstraint(_jointInfo->jointLimits.get()); + _jointInfo->jointLimits.reset(); + } + + _jointInfo->jointLimits = + std::make_shared( + _modelInfo->body.get(), identifier->indexInBtModel, + static_cast(_jointInfo->axisLower), + static_cast(_jointInfo->axisUpper)); + + _worldInfo->world->addMultiBodyConstraint(_jointInfo->jointLimits.get()); +} + ///////////////////////////////////////////////// void makeColliderStatic(LinkInfo *_linkInfo) { @@ -158,18 +183,59 @@ double JointFeatures::GetJointAcceleration( ///////////////////////////////////////////////// double JointFeatures::GetJointForce( - const Identity &_id, const std::size_t _dof) const + const Identity &_id, const std::size_t /*_dof*/) const { + double results = gz::math::NAN_D; const auto *joint = this->ReferenceInterface(_id); const auto *identifier = std::get_if(&joint->identifier); + if (identifier) { const auto *model = this->ReferenceInterface(joint->model); - return model->body->getJointTorqueMultiDof( - identifier->indexInBtModel)[_dof]; + auto feedback = model->body->getLink( + identifier->indexInBtModel).m_jointFeedback; + const auto &link = model->body->getLink(identifier->indexInBtModel); + results = 0.0; + if (link.m_jointType == btMultibodyLink::eRevolute) + { + // According to the documentation in btMultibodyLink.h, + // m_axesTop[0] is the joint axis for revolute joints. + Eigen::Vector3d axis = convert(link.getAxisTop(0)); + math::Vector3d axis_converted(axis[0], axis[1], axis[2]); + btVector3 angular = feedback->m_reactionForces.getAngular(); + math::Vector3d angularTorque( + angular.getX(), + angular.getY(), + angular.getZ()); + results += axis_converted.Dot(angularTorque); + #if BT_BULLET_VERSION < 326 + // not always true + return results / 2.0; + #else + return results; + #endif + } + else if (link.m_jointType == btMultibodyLink::ePrismatic) + { + auto axis = convert(link.getAxisBottom(0)); + math::Vector3d axis_converted(axis[0], axis[1], axis[2]); + btVector3 linear = feedback->m_reactionForces.getLinear(); + math::Vector3d linearForce( + linear.getX(), + linear.getY(), + linear.getZ()); + results += axis_converted.Dot(linearForce); + #if BT_BULLET_VERSION < 326 + // Not always true see for reference: + // https://github.com/bulletphysics/bullet3/discussions/3713 + // https://github.com/gazebosim/gz-physics/issues/565 + return results / 2.0; + #else + return results; + #endif + } } - - return gz::math::NAN_D; + return results; } ///////////////////////////////////////////////// @@ -236,8 +302,13 @@ void JointFeatures::SetJointForce( return; const auto *model = this->ReferenceInterface(joint->model); + + // clamp the values + double force = std::clamp(_value, + joint->minEffort, joint->maxEffort); + model->body->getJointTorqueMultiDof( - identifier->indexInBtModel)[_dof] = static_cast(_value); + identifier->indexInBtModel)[_dof] = static_cast(force); model->body->wakeUp(); } @@ -368,20 +439,153 @@ void JointFeatures::SetJointVelocityCommand( auto modelInfo = this->ReferenceInterface(jointInfo->model); if (!jointInfo->motor) { + auto *world = this->ReferenceInterface(modelInfo->world); + // \todo(iche033) The motor constraint is created with a max impulse + // computed by maxEffort * stepsize. However, our API supports + // stepping sim with varying dt. We should recompute max impulse + // if stepSize changes. jointInfo->motor = std::make_shared( modelInfo->body.get(), std::get(jointInfo->identifier).indexInBtModel, 0, static_cast(0), - static_cast(jointInfo->effort)); - auto *world = this->ReferenceInterface(modelInfo->world); + static_cast(jointInfo->maxEffort * world->stepSize)); world->world->addMultiBodyConstraint(jointInfo->motor.get()); } - jointInfo->motor->setVelocityTarget(static_cast(_value)); + // clamp the values + double velocity = std::clamp(_value, + jointInfo->minVelocity, jointInfo->maxVelocity); + + jointInfo->motor->setVelocityTarget(static_cast(velocity)); modelInfo->body->wakeUp(); } +///////////////////////////////////////////////// +void JointFeatures::SetJointMinPosition( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid minimum joint position value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + jointInfo->axisLower = _value; + + auto *modelInfo = this->ReferenceInterface(jointInfo->model); + auto *worldInfo = this->ReferenceInterface(modelInfo->world); + recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxPosition( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid maximum joint position value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + + jointInfo->axisUpper = _value; + + auto *modelInfo = this->ReferenceInterface(jointInfo->model); + auto *worldInfo = this->ReferenceInterface(modelInfo->world); + recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinVelocity( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid minimum joint velocity value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + + jointInfo->minVelocity = _value; +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxVelocity( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid maximum joint velocity value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + + jointInfo->maxVelocity = _value; +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinEffort( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid minimum joint effort value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + + return; + } + + jointInfo->minEffort = _value; +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxEffort( + const Identity &_id, std::size_t _dof, double _value) +{ + auto *jointInfo = this->ReferenceInterface(_id); + if (std::isnan(_value)) + { + gzerr << "Invalid maximum joint effort value [" << _value + << "] commanded on joint [" << jointInfo->name << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + + const auto *identifier = std::get_if(&jointInfo->identifier); + if (!identifier) + return; + + jointInfo->maxEffort = _value; + + auto *modelInfo = this->ReferenceInterface(jointInfo->model); + auto *world = this->ReferenceInterface(modelInfo->world); + + if (jointInfo->motor) + { + world->world->removeMultiBodyConstraint(jointInfo->motor.get()); + jointInfo->motor.reset(); + } + + jointInfo->motor = std::make_shared( + modelInfo->body.get(), + std::get(jointInfo->identifier).indexInBtModel, + 0, + static_cast(0), + static_cast(jointInfo->maxEffort * world->stepSize)); + world->world->addMultiBodyConstraint(jointInfo->motor.get()); +} + ///////////////////////////////////////////////// Identity JointFeatures::AttachFixedJoint( const Identity &_childID, diff --git a/bullet-featherstone/src/JointFeatures.hh b/bullet-featherstone/src/JointFeatures.hh index bc6c274f8..ba103e40f 100644 --- a/bullet-featherstone/src/JointFeatures.hh +++ b/bullet-featherstone/src/JointFeatures.hh @@ -37,6 +37,9 @@ struct JointFeatureList : FeatureList< GetBasicJointProperties, SetJointVelocityCommandFeature, + SetJointPositionLimitsFeature, + SetJointVelocityLimitsFeature, + SetJointEffortLimitsFeature, SetJointTransformFromParentFeature, AttachFixedJointFeature, @@ -127,6 +130,30 @@ class JointFeatures : const Identity &_id, const std::size_t _dof, const double _value) override; + public: void SetJointMinPosition( + const Identity &_id, std::size_t _dof, + double _value) override; + + public: void SetJointMaxPosition( + const Identity &_id, std::size_t _dof, + double _value) override; + + public: void SetJointMinVelocity( + const Identity &_id, std::size_t _dof, + double _value) override; + + public: void SetJointMaxVelocity( + const Identity &_id, std::size_t _dof, + double _value) override; + + public: void SetJointMinEffort( + const Identity &_id, std::size_t _dof, + double _value) override; + + public: void SetJointMaxEffort( + const Identity &_id, std::size_t _dof, + double _value) override; + // ----- AttachFixedJointFeature ----- public: Identity AttachFixedJoint( const Identity &_childID, diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index e6ee725f7..ebb7c686c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -89,6 +90,12 @@ Identity SDFFeatures::ConstructSdfWorld( worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity())); + const ::sdf::Physics *physics = _sdfWorld.PhysicsByIndex(0); + if (physics) + worldInfo->stepSize = physics->MaxStepSize(); + else + worldInfo->stepSize = 0.001; + for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) { const ::sdf::Model *model = _sdfWorld.ModelByIndex(i); @@ -770,6 +777,10 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (::sdf::JointType::PRISMATIC == joint->Type() || ::sdf::JointType::REVOLUTE == joint->Type()) { + // Note: These m_joint* properties below are currently not supported by + // bullet-featherstone and so setting them does not have any effect. + // The lower and uppper limit is supported via the + // btMultiBodyJointLimitConstraint. model->body->getLink(i).m_jointLowerLimit = static_cast(joint->Axis()->Lower()); model->body->getLink(i).m_jointUpperLimit = @@ -782,7 +793,13 @@ Identity SDFFeatures::ConstructSdfModelImpl( static_cast(joint->Axis()->MaxVelocity()); model->body->getLink(i).m_jointMaxForce = static_cast(joint->Axis()->Effort()); - jointInfo->effort = static_cast(joint->Axis()->Effort()); + + jointInfo->minEffort = -joint->Axis()->Effort(); + jointInfo->maxEffort = joint->Axis()->Effort(); + jointInfo->minVelocity = -joint->Axis()->MaxVelocity(); + jointInfo->maxVelocity = joint->Axis()->MaxVelocity(); + jointInfo->axisLower = joint->Axis()->Lower(); + jointInfo->axisUpper = joint->Axis()->Upper(); jointInfo->jointLimits = std::make_shared( @@ -873,12 +890,21 @@ Identity SDFFeatures::ConstructSdfModelImpl( for (const auto& [linkSdf, linkID] : linkIDs) { - for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) + if (linkSdf->CollisionCount() == 0u) { - // If we fail to add the collision, just keep building the model. It may - // need to be constructed outside of the SDF generation pipeline, e.g. - // with AttachHeightmap. - this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); + // create empty link collider and compound shape if there are no + // collisions + this->CreateLinkCollider(linkID, isStatic); + } + else + { + for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) + { + // If we fail to add the collision, just keep building the model. It may + // need to be constructed outside of the SDF generation pipeline, e.g. + // with AttachHeightmap. + this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); + } } } @@ -938,7 +964,7 @@ Identity SDFFeatures::ConstructSdfModel( bool SDFFeatures::AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, - bool isStatic) + bool _isStatic) { if (!_collision.Geom()) { @@ -1062,21 +1088,25 @@ bool SDFFeatures::AddSdfCollision( ::sdf::MeshOptimization::CONVEX_HULL) { std::size_t maxConvexHulls = 16u; + std::size_t voxelResolution = 200000u; + if (meshSdf->ConvexDecomposition()) + { + // limit max number of convex hulls to generate + maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls(); + voxelResolution = meshSdf->ConvexDecomposition()->VoxelResolution(); + } if (meshSdf->Optimization() == ::sdf::MeshOptimization::CONVEX_HULL) { /// create 1 convex hull for the whole submesh maxConvexHulls = 1u; } - else if (meshSdf->ConvexDecomposition()) - { - // limit max number of convex hulls to generate - maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls(); - } // Check if MeshManager contains the decomposed mesh already. If not // add it to the MeshManager so we do not need to decompose it again. const std::string convexMeshName = - mesh->Name() + "_CONVEX_" + std::to_string(maxConvexHulls); + mesh->Name() + "_" + meshSdf->Submesh() + "_CONVEX_" + + std::to_string(maxConvexHulls) + "_" + + std::to_string(voxelResolution); auto *decomposedMesh = meshManager.MeshByName(convexMeshName); if (!decomposedMesh) { @@ -1088,7 +1118,7 @@ bool SDFFeatures::AddSdfCollision( auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock(); std::vector decomposed = gz::common::MeshManager::ConvexDecomposition( - *mergedSubmesh.get(), maxConvexHulls); + *mergedSubmesh.get(), maxConvexHulls, voxelResolution); gzdbg << "Optimizing mesh (" << meshSdf->OptimizationStr() << "): " << mesh->Name() << std::endl; // Create decomposed mesh and add it to MeshManager @@ -1262,22 +1292,11 @@ bool SDFFeatures::AddSdfCollision( const btTransform btInertialToCollision = convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); - int linkIndexInModel = -1; - if (linkInfo->indexInModel.has_value()) - linkIndexInModel = *linkInfo->indexInModel; - if (!linkInfo->collider) { - linkInfo->shape = std::make_unique(); + this->CreateLinkCollider(_linkID, _isStatic, shape.get(), + btInertialToCollision); - // NOTE: Bullet does not appear to support different surface properties - // for different shapes attached to the same link. - linkInfo->collider = std::make_unique( - model->body.get(), linkIndexInModel); - - linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); - - linkInfo->collider->setCollisionShape(linkInfo->shape.get()); linkInfo->collider->setRestitution(static_cast(restitution)); linkInfo->collider->setRollingFriction( static_cast(rollingFriction)); @@ -1296,63 +1315,6 @@ bool SDFFeatures::AddSdfCollision( const btScalar kd = btScalar(1e14); linkInfo->collider->setContactStiffnessAndDamping(kp, kd); } - - if (linkIndexInModel >= 0) - { - model->body->getLink(linkIndexInModel).m_collider = - linkInfo->collider.get(); - const auto p = model->body->localPosToWorld( - linkIndexInModel, btVector3(0, 0, 0)); - const auto rot = model->body->localFrameToWorld( - linkIndexInModel, btMatrix3x3::getIdentity()); - linkInfo->collider->setWorldTransform(btTransform(rot, p)); - } - else - { - model->body->setBaseCollider(linkInfo->collider.get()); - linkInfo->collider->setWorldTransform( - model->body->getBaseWorldTransform()); - } - - auto *world = this->ReferenceInterface(model->world); - - // Set static filter for collisions in - // 1) a static model - // 2) a fixed base link - // 3) a (non-base) link with zero dofs - bool isFixed = false; - if (model->body->hasFixedBase()) - { - // check if it's a base link - isFixed = std::size_t(_linkID) == - static_cast(model->body->getUserIndex()); - // check if link has zero dofs - if (!isFixed && linkInfo->indexInModel.has_value()) - { - isFixed = model->body->getLink( - linkInfo->indexInModel.value()).m_dofCount == 0; - } - } - if (isStatic || isFixed) - { - world->world->addCollisionObject( - 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 - { - world->world->addCollisionObject( - linkInfo->collider.get(), - btBroadphaseProxy::DefaultFilter, - btBroadphaseProxy::AllFilter); - } } else if (linkInfo->shape) { @@ -1474,6 +1436,82 @@ Identity SDFFeatures::ConstructSdfJoint( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic, + btCollisionShape *_shape, const btTransform &_shapeTF) +{ + auto *linkInfo = this->ReferenceInterface(_linkID); + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *worldInfo = this->ReferenceInterface(modelInfo->world); + int linkIndexInModel = -1; + + if (linkInfo->indexInModel.has_value()) + linkIndexInModel = *linkInfo->indexInModel; + linkInfo->collider = std::make_unique( + modelInfo->body.get(), linkIndexInModel); + + linkInfo->shape = std::make_unique(); + + if (_shape) + linkInfo->shape->addChildShape(_shapeTF, _shape); + + linkInfo->collider->setCollisionShape(linkInfo->shape.get()); + + if (linkIndexInModel >= 0) + { + modelInfo->body->getLink(linkIndexInModel).m_collider = + linkInfo->collider.get(); + const auto p = modelInfo->body->localPosToWorld( + linkIndexInModel, btVector3(0, 0, 0)); + const auto rot = modelInfo->body->localFrameToWorld( + linkIndexInModel, btMatrix3x3::getIdentity()); + linkInfo->collider->setWorldTransform(btTransform(rot, p)); + } + else + { + modelInfo->body->setBaseCollider(linkInfo->collider.get()); + linkInfo->collider->setWorldTransform( + modelInfo->body->getBaseWorldTransform()); + } + + // Set static filter for collisions in + // 1) a static model + // 2) a fixed base link + // 3) a (non-base) link with zero dofs + bool isFixed = false; + if (modelInfo->body->hasFixedBase()) + { + // check if it's a base link + isFixed = std::size_t(_linkID) == + static_cast(modelInfo->body->getUserIndex()); + // check if link has zero dofs + if (!isFixed && linkInfo->indexInModel.has_value()) + { + isFixed = modelInfo->body->getLink( + linkInfo->indexInModel.value()).m_dofCount == 0; + } + } + if (_isStatic || isFixed) + { + worldInfo->world->addCollisionObject( + 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 + { + worldInfo->world->addCollisionObject( + linkInfo->collider.get(), + btBroadphaseProxy::DefaultFilter, + btBroadphaseProxy::AllFilter); + } +} } // namespace bullet_featherstone } // namespace physics diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index bea5faa70..2e8d3c9df 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -78,6 +78,14 @@ class SDFFeatures : /// \return The entity identity if constructed otherwise an invalid identity private: Identity ConstructSdfModelImpl(std::size_t _parentID, const ::sdf::Model &_sdfModel); + + /// \brief Create and initialze the link collider in link info + /// \param[in] _linkID ID of link to create the collider for + /// \param[in] _isStatic True if the link is static + /// \param[in] _shape Collision shape to attach to link + private: void CreateLinkCollider(const Identity &_linkID, + bool _isStatic, btCollisionShape *_shape = nullptr, + const btTransform &_shapeTF = btTransform::getIdentity()); }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a32d93d2c..c3509721d 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -37,14 +37,19 @@ void SimulationFeatures::WorldForwardStep( const auto worldInfo = this->ReferenceInterface(_worldID); auto *dtDur = _u.Query(); + double stepSize = 0.001; if (dtDur) { std::chrono::duration dt = *dtDur; stepSize = dt.count(); } - worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, - static_cast(this->stepSize)); + // \todo(iche033) Stepping sim with varying dt may not work properly. + // One example is the motor constraint that's created in + // JointFeatures::SetJointVelocityCommand which assumes a fixed step + // size. + worldInfo->world->stepSimulation(static_cast(stepSize), 1, + static_cast(stepSize)); this->WriteRequiredData(_h); this->Write(_h.Get()); diff --git a/bullet-featherstone/src/SimulationFeatures.hh b/bullet-featherstone/src/SimulationFeatures.hh index 299a499da..847d8c49c 100644 --- a/bullet-featherstone/src/SimulationFeatures.hh +++ b/bullet-featherstone/src/SimulationFeatures.hh @@ -58,8 +58,6 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; - private: double stepSize = 0.001; - /// \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/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 72c99849b..cae3c384f 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -81,6 +81,11 @@ foreach(test ${tests}) "BT_BULLET_VERSION_LE_306" ) endif() + if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.07) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_LE_307" + ) + endif() if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index ab643e4bf..435fdabcb 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -254,6 +254,7 @@ TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceContr joint->SetForce(0, 100); world->Step(output, state, input); } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3); for (std::size_t i = 0; i < 100; ++i) @@ -300,9 +301,6 @@ struct JointFeaturePositionLimitsForceControlList : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, gz::physics::GetModelFromWorld, - // This feature is not requited but it will force to use dart6.10 which is required - // to run these tests - gz::physics::GetShapeFrictionPyramidSlipCompliance, gz::physics::SetBasicJointState, gz::physics::SetJointEffortLimitsFeature, gz::physics::SetJointPositionLimitsFeature, @@ -592,13 +590,21 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi // TODO(anyone): position limits do not work very well with velocity control // bug https://github.com/dartsim/dart/issues/1583 // resolved in DART 6.11.0 -TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositionLimitsWithVelocityControl) +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetPositionLimitsWithVelocityControl) { for (const std::string &name : this->pluginNames) { - if(this->PhysicsEngineName(name) != "dartsim") + if (this->PhysicsEngineName(name) == "dartsim") + { + GTEST_SKIP(); + } + else if (this->PhysicsEngineName(name) == "bullet-featherstone") { +#ifdef BT_BULLET_VERSION_LE_307 + // joint position limits does not work well with velocity control in + // bullet versions <= 3.07 GTEST_SKIP(); +#endif } std::cout << "Testing plugin: " << name << std::endl; @@ -638,7 +644,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositio if (i % 500 == 499) { EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-5); } } } @@ -648,11 +654,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi { for (const std::string &name : this->pluginNames) { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -719,11 +720,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith { for (const std::string &name : this->pluginNames) { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -779,11 +775,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi { for (const std::string &name : this->pluginNames) { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -1292,17 +1283,16 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) 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 + // \todo(iche033) Investigate behavior differences in dartsim. + // Locally, model3 falls after joint is detached. + // On CI, model3 has zero velocity which could mean model3 and model1 are + // stuck together since they overlap with each other if (this->PhysicsEngineName(name) != "dartsim") -#endif - EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + { + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } } // Test attaching fixed joint with reverse the parent and child diff --git a/test/common_test/worlds/test.world b/test/common_test/worlds/test.world index b75496857..4ee56c3b2 100644 --- a/test/common_test/worlds/test.world +++ b/test/common_test/worlds/test.world @@ -333,7 +333,7 @@ 0.1 0.1 0.1 - +