From a1cb98905ddce83379b3f8367b3c1fd3c7ffb8fb Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 17 Jun 2024 09:16:09 -0700 Subject: [PATCH 1/7] Parse voxel resolution when decomposing meshes (#655) Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 15e7125a5..f424c3bc9 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1053,21 +1053,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) { @@ -1079,7 +1083,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 From 3323de54901e9d90d3bbaa1c49c88a88da36dadc Mon Sep 17 00:00:00 2001 From: Graziato Davide <85335579+Fixit-Davide@users.noreply.github.com> Date: Tue, 18 Jun 2024 21:31:24 +0200 Subject: [PATCH 2/7] [featherstone] Publish JointFeedback forces. (#628) Signed-off-by: Davide Graziato Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 51 +++++++++++++++++++++--- 1 file changed, 46 insertions(+), 5 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index e2c460a32..4ead27ed3 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -158,18 +158,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::Vector3 axis_converted(axis[0], axis[1], axis[2]); + btVector3 angular = feedback->m_reactionForces.getAngular(); + math::Vector3 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::Vector3 axis_converted(axis[0], axis[1], axis[2]); + btVector3 linear = feedback->m_reactionForces.getLinear(); + math::Vector3 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; } ///////////////////////////////////////////////// From 12d589b9240a40b571d90a1255458711ab31a88d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 25 Jun 2024 12:48:31 -0400 Subject: [PATCH 3/7] Backport: Add Cone as a collision shape (#639) Signed-off-by: Benjamin Perseghetti --- README.md | 2 +- bullet-featherstone/README.md | 2 + bullet-featherstone/src/SDFFeatures.cc | 9 ++ bullet-featherstone/src/ShapeFeatures.cc | 80 +++++++++++ bullet-featherstone/src/ShapeFeatures.hh | 21 +++ bullet/src/SDFFeatures.cc | 11 +- dartsim/src/CustomConeMeshShape.cc | 62 +++++++++ dartsim/src/CustomConeMeshShape.hh | 44 +++++++ dartsim/src/SDFFeatures.cc | 20 +++ dartsim/src/SDFFeatures_TEST.cc | 5 +- dartsim/src/ShapeFeatures.cc | 70 ++++++++++ dartsim/src/ShapeFeatures.hh | 22 ++++ include/gz/physics/ConeShape.hh | 154 ++++++++++++++++++++++ include/gz/physics/detail/ConeShape.hh | 84 ++++++++++++ test/common_test/joint_features.cc | 15 +-- test/common_test/simulation_features.cc | 72 +++++++++- test/common_test/worlds/shapes.world | 34 +++++ tpe/lib/src/Collision.cc | 6 + tpe/lib/src/Shape.cc | 39 ++++++ tpe/lib/src/Shape.hh | 39 ++++++ tpe/plugin/src/SDFFeatures.cc | 9 ++ tpe/plugin/src/ShapeFeatures.cc | 103 +++++++++++++-- tpe/plugin/src/ShapeFeatures.hh | 23 +++- tpe/plugin/src/SimulationFeatures_TEST.cc | 38 +++++- 24 files changed, 929 insertions(+), 35 deletions(-) create mode 100644 dartsim/src/CustomConeMeshShape.cc create mode 100644 dartsim/src/CustomConeMeshShape.hh create mode 100644 include/gz/physics/ConeShape.hh create mode 100644 include/gz/physics/detail/ConeShape.hh diff --git a/README.md b/README.md index b0eda090e..7c1607eee 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ Gazebo Physics provides the following functionality: at runtime. * Features for common aspects of rigid body dynamic simulation - Construct model from [SDFormat](http://sdformat.org/) file. - - Collision shapes (such as box, sphere, cylinder, capsule, ellipsoid, mesh, heightmap). + - Collision shapes (such as box, sphere, cylinder, cone, capsule, ellipsoid, mesh, heightmap). - Joint types (such as revolute, prismatic, fixed, ball, screw, universal). - Step simulation, get/set state, apply inputs. * Reference implementation of physics plugin using diff --git a/bullet-featherstone/README.md b/bullet-featherstone/README.md index 5444cd089..932842b68 100644 --- a/bullet-featherstone/README.md +++ b/bullet-featherstone/README.md @@ -65,6 +65,8 @@ These are the specific physics API features implemented. * AttachBoxShapeFeature * GetCapsuleShapeProperties * AttachCapsuleShapeFeature + * GetConeShapeProperties + * AttachConeShapeFeature * GetCylinderShapeProperties * AttachCylinderShapeFeature * GetEllipsoidShapeProperties diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index f424c3bc9..8e6c4a742 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -963,6 +964,14 @@ bool SDFFeatures::AddSdfCollision( const auto radius = sphere->Radius(); shape = std::make_unique(static_cast(radius)); } + else if (const auto *cone = geom->ConeShape()) + { + const auto radius = static_cast(cone->Radius()); + const auto height = static_cast(cone->Length()); + shape = + std::make_unique(radius, height); + shape->setMargin(0.0); + } else if (const auto *cylinder = geom->CylinderShape()) { const auto radius = static_cast(cylinder->Radius()); diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 34f78007c..e702a9eac 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -177,6 +177,86 @@ Identity ShapeFeatures::AttachCapsuleShape( return identity; } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cone = static_cast( + it->second->collider.get()); + if (cone) + { + return cone->getRadius(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cone = static_cast( + it->second->collider.get()); + if (cone) + { + return cone->getHeight(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + const auto radius = static_cast(_radius); + const auto height = static_cast(_height); + auto shape = + std::make_unique(radius, height); + shape->setMargin(0.0); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + _pose}); + + return identity; +} + ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const { diff --git a/bullet-featherstone/src/ShapeFeatures.hh b/bullet-featherstone/src/ShapeFeatures.hh index 998c97e66..4bcd86fac 100644 --- a/bullet-featherstone/src/ShapeFeatures.hh +++ b/bullet-featherstone/src/ShapeFeatures.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList< GetCapsuleShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -90,6 +94,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index efdb37b67..2d69cc61f 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -22,9 +22,10 @@ #include #include #include +#include +#include #include #include -#include #include #include @@ -228,6 +229,14 @@ Identity SDFFeatures::ConstructSdfCollision( const auto radius = static_cast(sphere->Radius()); shape = std::make_shared(radius); } + else if (geom->ConeShape()) + { + const auto cone = geom->ConeShape(); + const auto radius = static_cast(cone->Radius()); + const auto height = static_cast(cone->Length()); + shape = + std::make_shared(radius, height); + } else if (geom->CylinderShape()) { const auto cylinder = geom->CylinderShape(); diff --git a/dartsim/src/CustomConeMeshShape.cc b/dartsim/src/CustomConeMeshShape.cc new file mode 100644 index 000000000..a3f8bf7ed --- /dev/null +++ b/dartsim/src/CustomConeMeshShape.cc @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2018 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. + * +*/ + +#include "CustomConeMeshShape.hh" + +#include +#include + +#include +#include +#include + +namespace gz { +namespace physics { +namespace dartsim { + +///////////////////////////////////////////////// +const gz::common::Mesh* MakeCustomConeMesh( + const gz::math::Coned &_cone, + int _meshRings, + int _meshSegments) +{ + common::MeshManager *meshMgr = common::MeshManager::Instance(); + std::string coneMeshName = std::string("cone_mesh") + + "_" + std::to_string(_cone.Radius()) + + "_" + std::to_string(_cone.Length()); + meshMgr->CreateCone( + coneMeshName, + _cone.Radius(), + _cone.Length(), + _meshRings, _meshSegments); + return meshMgr->MeshByName(coneMeshName); +} + +///////////////////////////////////////////////// +CustomConeMeshShape::CustomConeMeshShape( + const gz::math::Coned &_cone, + int _meshRings, + int _meshSegments) + : CustomMeshShape(*MakeCustomConeMesh(_cone, _meshRings, _meshSegments), + Eigen::Vector3d(1, 1, 1)), + cone(_cone) +{ +} + +} +} +} diff --git a/dartsim/src/CustomConeMeshShape.hh b/dartsim/src/CustomConeMeshShape.hh new file mode 100644 index 000000000..64e0e9cec --- /dev/null +++ b/dartsim/src/CustomConeMeshShape.hh @@ -0,0 +1,44 @@ +/* + * 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_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_ +#define GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_ + +#include +#include "CustomMeshShape.hh" + +namespace gz { +namespace physics { +namespace dartsim { + +/// \brief This class creates a custom derivative of the CustomMeshShape to +/// allow casting to a Cone shape. +class CustomConeMeshShape : public CustomMeshShape +{ + public: CustomConeMeshShape( + const gz::math::Coned &_cone, + int _meshRings = 1, + int _meshSegments = 36); + + public: gz::math::Coned cone; +}; + +} +} +} + +#endif // GZ_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_ diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 08a1183f9..3993dceb6 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -20,12 +20,14 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +68,7 @@ #include #include "AddedMassFeatures.hh" +#include "CustomConeMeshShape.hh" #include "CustomMeshShape.hh" namespace gz { @@ -340,11 +344,27 @@ static ShapeAndTransform ConstructGeometry( { return ConstructCapsule(*_geometry.CapsuleShape()); } + else if (_geometry.ConeShape()) + { + // TODO(anyone): Replace this code when Cone is supported by DART + gzwarn << "DART: Cone is not a supported collision geomerty" + << " primitive, using generated mesh of a cone instead" + << std::endl; + auto mesh = + std::make_shared(_geometry.ConeShape()->Shape()); + auto mesh2 = std::dynamic_pointer_cast(mesh); + return {mesh2}; + } else if (_geometry.CylinderShape()) + { return ConstructCylinder(*_geometry.CylinderShape()); + } else if (_geometry.EllipsoidShape()) { // TODO(anyone): Replace this code when Ellipsoid is supported by DART + gzwarn << "DART: Ellipsoid is not a supported collision geomerty" + << " primitive, using generated mesh of an ellipsoid instead" + << std::endl; common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = std::string("ellipsoid_mesh") + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X()) diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index d3db87464..613bb8144 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -972,10 +972,11 @@ TEST_P(SDFFeatures_TEST, Shapes) auto dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); - ASSERT_EQ(5u, dartWorld->getNumSkeletons()); + ASSERT_EQ(6u, dartWorld->getNumSkeletons()); int count{0}; - for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"}) + for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid", + "cone"}) { const auto skeleton = dartWorld->getSkeleton(count++); ASSERT_NE(nullptr, skeleton); diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 8b42a0dbb..5b19c17a0 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -17,7 +17,9 @@ #include "ShapeFeatures.hh" +#include #include +#include #include #include @@ -32,6 +34,7 @@ #include #include +#include "CustomConeMeshShape.hh" #include "CustomHeightmapShape.hh" #include "CustomMeshShape.hh" @@ -161,6 +164,70 @@ Identity ShapeFeatures::AttachCapsuleShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + + const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); + + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_coneID); + + auto *coneShape = + static_cast( + shapeInfo->node->getShape().get()); + + return coneShape->cone.Radius(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_coneID); + + auto *coneShape = + static_cast( + shapeInfo->node->getShape().get()); + + return coneShape->cone.Length(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + gzwarn << "DART: Cone is not a supported collision geomerty" + << " primitive, using generated mesh of a cone instead" + << std::endl; + auto mesh = + std::make_shared(gz::math::Coned(_height, _radius)); + + DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); + dart::dynamics::ShapeNode *sn = + bn->createShapeNodeWith( + mesh, bn->getName() + ":" + _name); + + sn->setRelativeTransform(_pose); + const std::size_t shapeID = this->AddShape({sn, _name}); + return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); +} + ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const { @@ -255,6 +322,9 @@ Identity ShapeFeatures::AttachEllipsoidShape( const Vector3d &_radii, const Pose3d &_pose) { + gzwarn << "DART: Ellipsoid is not a supported collision geomerty" + << " primitive, using generated mesh of an ellipsoid instead" + << std::endl; common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = _name + "_ellipsoid_mesh" + "_" + std::to_string(_radii[0]) diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index e83992419..b112856c2 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -55,6 +56,10 @@ struct ShapeFeatureList : FeatureList< // SetCapsulerShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, +// SetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, // SetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -121,6 +126,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; diff --git a/include/gz/physics/ConeShape.hh b/include/gz/physics/ConeShape.hh new file mode 100644 index 000000000..143d3490a --- /dev/null +++ b/include/gz/physics/ConeShape.hh @@ -0,0 +1,154 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_CONESHAPE_HH_ +#define GZ_PHYSICS_CONESHAPE_HH_ + +#include + +#include +#include + +namespace gz +{ + namespace physics + { + GZ_PHYSICS_DECLARE_SHAPE_TYPE(ConeShape) + + class GZ_PHYSICS_VISIBLE GetConeShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class ConeShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Get the radius of this ConeShape + /// \return the radius of this ConeShape + public: Scalar GetRadius() const; + + /// \brief Get the height (length along the local z-axis) of this + /// ConeShape. + /// \return the height of this ConeShape + public: Scalar GetHeight() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual Scalar GetConeShapeRadius( + const Identity &_coneID) const = 0; + + public: virtual Scalar GetConeShapeHeight( + const Identity &_coneID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the ConeShape properties such as + /// the cone radius and height. + class GZ_PHYSICS_VISIBLE SetConeShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class ConeShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the radius of this ConeShape + /// \param[in] _radius + /// The desired radius of this ConeShape + public: void SetRadius(Scalar _radius); + + /// \brief Set the height of this ConeShape + /// \param[in] _height + /// The desired height of this ConeShape + public: void SetHeight(Scalar _height); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual void SetConeShapeRadius( + const Identity &_coneID, Scalar _radius) = 0; + + public: virtual void SetConeShapeHeight( + const Identity &_coneID, Scalar _height) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new cone shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// cone center point in actual implementation. + class GZ_PHYSICS_VISIBLE AttachConeShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = ConeShapePtr; + + /// \brief Rigidly attach a ConeShape to this link. + /// \param[in] _name + /// \param[in] _radius + /// The radius of the cone. + /// \param[in] _height + /// The height of the cone. + /// \param[in] _pose + /// The desired pose of the ConeShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed ConeShape + public: ShapePtrType AttachConeShape( + const std::string &_name = "cone", + Scalar _radius = 1.0, + Scalar _height = 1.0, + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + Scalar _radius, + Scalar _height, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/detail/ConeShape.hh b/include/gz/physics/detail/ConeShape.hh new file mode 100644 index 000000000..0630b281a --- /dev/null +++ b/include/gz/physics/detail/ConeShape.hh @@ -0,0 +1,84 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_CONESHAPE_HH_ +#define GZ_PHYSICS_DETAIL_CONESHAPE_HH_ + +#include + +#include + +namespace gz +{ + namespace physics + { + ///////////////////////////////////////////////// + template + auto GetConeShapeProperties::ConeShape + ::GetRadius() const -> Scalar + { + return this->template Interface() + ->GetConeShapeRadius(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetConeShapeProperties::ConeShape + ::GetHeight() const -> Scalar + { + return this->template Interface() + ->GetConeShapeHeight(this->identity); + } + + ///////////////////////////////////////////////// + template + void SetConeShapeProperties::ConeShape + ::SetRadius(Scalar _radius) + { + this->template Interface() + ->SetConeShapeRadius(this->identity, _radius); + } + + ///////////////////////////////////////////////// + template + void SetConeShapeProperties::ConeShape + ::SetHeight(Scalar _height) + { + this->template Interface() + ->SetConeShapeHeight(this->identity, _height); + } + + ///////////////////////////////////////////////// + template + auto AttachConeShapeFeature::Link + ::AttachConeShape( + const std::string &_name, + Scalar _radius, + Scalar _height, + const PoseType &_pose) -> ShapePtrType + { + return ShapePtrType(this->pimpl, + this->template Interface() + ->AttachConeShape( + this->identity, _name, _radius, _height, _pose)); + } + } +} + +#endif diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index ab643e4bf..f67f219a0 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1292,17 +1292,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/simulation_features.cc b/test/common_test/simulation_features.cc index 83afc7bdd..bc30de759 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -36,14 +36,15 @@ #include #include "gz/physics/BoxShape.hh" -#include -#include #include "gz/physics/ContactProperties.hh" -#include "gz/physics/CylinderShape.hh" #include "gz/physics/CapsuleShape.hh" +#include "gz/physics/ConeShape.hh" +#include "gz/physics/CylinderShape.hh" #include "gz/physics/EllipsoidShape.hh" #include #include +#include +#include #include "gz/physics/SphereShape.hh" #include @@ -80,11 +81,13 @@ struct Features : gz::physics::FeatureList< gz::physics::AttachBoxShapeFeature, gz::physics::AttachSphereShapeFeature, + gz::physics::AttachConeShapeFeature, gz::physics::AttachCylinderShapeFeature, gz::physics::AttachEllipsoidShapeFeature, gz::physics::AttachCapsuleShapeFeature, gz::physics::GetSphereShapeProperties, gz::physics::GetBoxShapeProperties, + gz::physics::GetConeShapeProperties, gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties @@ -265,7 +268,7 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, EXPECT_TRUE(checkedOutput); contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); + EXPECT_EQ(5u, contacts.size()); world->SetCollisionPairMaxContacts(0u); EXPECT_EQ(0u, world->GetCollisionPairMaxContacts()); @@ -373,11 +376,13 @@ struct FeaturesShapeFeatures : gz::physics::FeatureList< gz::physics::AttachBoxShapeFeature, gz::physics::AttachSphereShapeFeature, + gz::physics::AttachConeShapeFeature, gz::physics::AttachCylinderShapeFeature, gz::physics::AttachEllipsoidShapeFeature, gz::physics::AttachCapsuleShapeFeature, gz::physics::GetSphereShapeProperties, gz::physics::GetBoxShapeProperties, + gz::physics::GetConeShapeProperties, gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties @@ -446,6 +451,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto cylinderLink = cylinder->GetLink(0); auto cylinderCollision = cylinderLink->GetShape(0); auto cylinderShape = cylinderCollision->CastToCylinderShape(); + ASSERT_NE(nullptr, cylinderShape); EXPECT_NEAR(0.5, cylinderShape->GetRadius(), 1e-6); EXPECT_NEAR(1.1, cylinderShape->GetHeight(), 1e-6); @@ -453,6 +459,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, cylinderLink->GetShapeCount()); EXPECT_EQ(cylinder2, cylinderLink->GetShape(1)); + ASSERT_NE(nullptr, cylinderLink->GetShape(1)->CastToCylinderShape()); EXPECT_NEAR(3.0, cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(), 1e-6); @@ -464,6 +471,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto ellipsoidLink = ellipsoid->GetLink(0); auto ellipsoidCollision = ellipsoidLink->GetShape(0); auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidShape); EXPECT_TRUE( gz::math::Vector3d(0.2, 0.3, 0.5).Equal( gz::math::eigen3::convert(ellipsoidShape->GetRadii()), 0.1)); @@ -479,6 +487,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto capsuleLink = capsule->GetLink(0); auto capsuleCollision = capsuleLink->GetShape(0); auto capsuleShape = capsuleCollision->CastToCapsuleShape(); + ASSERT_NE(nullptr, capsuleShape); EXPECT_NEAR(0.2, capsuleShape->GetRadius(), 1e-6); EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6); @@ -487,6 +496,26 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_EQ(2u, capsuleLink->GetShapeCount()); EXPECT_EQ(capsule2, capsuleLink->GetShape(1)); + auto cone = world->GetModel("cone"); + auto coneLink = cone->GetLink(0); + auto coneCollision = coneLink->GetShape(0); + auto coneShape = coneCollision->CastToConeShape(); + ASSERT_NE(nullptr, coneShape); + EXPECT_NEAR(0.5, coneShape->GetRadius(), 1e-6); + EXPECT_NEAR(1.1, coneShape->GetHeight(), 1e-6); + + auto cone2 = coneLink->AttachConeShape( + "cone2", 3.0, 4.0, Eigen::Isometry3d::Identity()); + EXPECT_EQ(2u, coneLink->GetShapeCount()); + EXPECT_EQ(cone2, coneLink->GetShape(1)); + ASSERT_NE(nullptr, coneLink->GetShape(1)->CastToConeShape()); + EXPECT_NEAR(3.0, + coneLink->GetShape(1)->CastToConeShape()->GetRadius(), + 1e-6); + EXPECT_NEAR(4.0, + coneLink->GetShape(1)->CastToConeShape()->GetHeight(), + 1e-6); + // Test the bounding boxes in the local frames auto sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); @@ -498,6 +527,8 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision); auto capsuleAABB = capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision); + auto coneAABB = + coneCollision->GetAxisAlignedBoundingBox(*coneCollision); EXPECT_TRUE(gz::math::Vector3d(-1, -1, -1).Equal( gz::math::eigen3::convert(sphereAABB).Min(), 0.1)); @@ -519,6 +550,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) gz::math::eigen3::convert(capsuleAABB).Min(), 0.1)); EXPECT_TRUE(gz::math::Vector3d(0.2, 0.2, 0.5).Equal( gz::math::eigen3::convert(capsuleAABB).Max(), 0.1)); + EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -0.55), + gz::math::eigen3::convert(coneAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 0.55), + gz::math::eigen3::convert(coneAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); @@ -526,6 +561,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); + auto coneModelAABB = cone->GetAxisAlignedBoundingBox(); EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), gz::math::eigen3::convert(sphereModelAABB).Min()); @@ -547,6 +583,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) gz::math::eigen3::convert(capsuleModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(0.2, -2.8, 1), gz::math::eigen3::convert(capsuleModelAABB).Max()); + EXPECT_EQ(gz::math::Vector3d(-3, -9.5, -1.5), + gz::math::eigen3::convert(coneModelAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(3, -3.5, 2.5), + gz::math::eigen3::convert(coneModelAABB).Max()); } } @@ -781,6 +821,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); EXPECT_NE(nullptr, ellipsoidFreeGroup); + auto cone = world->GetModel("cone"); + auto coneFreeGroup = cone->FindFreeGroup(); + EXPECT_NE(nullptr, coneFreeGroup); + auto box = world->GetModel("box"); // step and get contacts @@ -795,6 +839,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) unsigned int contactBoxCylinder = 0u; unsigned int contactBoxCapsule = 0u; unsigned int contactBoxEllipsoid = 0u; + unsigned int contactBoxCone = 0u; for (auto &contact : contacts) { @@ -828,6 +873,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { contactBoxEllipsoid++; } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + } else { FAIL() << "There should not be contacts between: " @@ -838,6 +888,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) EXPECT_NE(0u, contactBoxCylinder); EXPECT_NE(0u, contactBoxCapsule); EXPECT_NE(0u, contactBoxEllipsoid); + EXPECT_NE(0u, contactBoxCone); // move sphere away sphereFreeGroup->SetWorldPose(gz::math::eigen3::convert( @@ -849,12 +900,13 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with cylinder, capsule, - // ellipsoid + // ellipsoid, cone EXPECT_NE(0u, contacts.size()); contactBoxCylinder = 0u; contactBoxCapsule = 0u; contactBoxEllipsoid = 0u; + contactBoxCone = 0u; for (auto contact : contacts) { const auto &contactPoint = @@ -882,6 +934,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { contactBoxEllipsoid++; } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + } else { FAIL() << "There should only be contacts between box and cylinder"; @@ -890,6 +947,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) EXPECT_NE(0u, contactBoxCylinder); EXPECT_NE(0u, contactBoxCapsule); EXPECT_NE(0u, contactBoxEllipsoid); + EXPECT_NE(0u, contactBoxCone); // move cylinder away cylinderFreeGroup->SetWorldPose(gz::math::eigen3::convert( @@ -903,6 +961,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) ellipsoidFreeGroup->SetWorldPose(gz::math::eigen3::convert( gz::math::Pose3d(0, -100, -100, 0, 0, 0))); + // move cone away + coneFreeGroup->SetWorldPose(gz::math::eigen3::convert( + gz::math::Pose3d(100, -100, 0.5, 0, 0, 0))); + // step and get contacts checkedOutput = StepWorld(world, false).first; EXPECT_FALSE(checkedOutput); diff --git a/test/common_test/worlds/shapes.world b/test/common_test/worlds/shapes.world index cb3bce5d9..0b66579d6 100644 --- a/test/common_test/worlds/shapes.world +++ b/test/common_test/worlds/shapes.world @@ -165,5 +165,39 @@ + + + 0 -6.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + 0 0 -0.275 0 0 0 + + + + + 0.5 + 1.1 + + + + + + + 0.5 + 1.1 + + + + + diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index 114851f1a..1a340e0a4 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -81,6 +81,12 @@ void Collision::SetShape(const Shape &_shape) static_cast(&_shape); this->dataPtr->shape.reset(new CapsuleShape(*typedShape)); } + else if (_shape.GetType() == ShapeType::CONE) + { + const ConeShape *typedShape = + static_cast(&_shape); + this->dataPtr->shape.reset(new ConeShape(*typedShape)); + } else if (_shape.GetType() == ShapeType::CYLINDER) { const CylinderShape *typedShape = diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc index 44fbc0e1f..4f1d12126 100644 --- a/tpe/lib/src/Shape.cc +++ b/tpe/lib/src/Shape.cc @@ -115,6 +115,45 @@ void CapsuleShape::UpdateBoundingBox() this->bbox = math::AxisAlignedBox(-halfSize, halfSize); } +////////////////////////////////////////////////// +ConeShape::ConeShape() : Shape() +{ + this->type = ShapeType::CONE; +} + +////////////////////////////////////////////////// +double ConeShape::GetRadius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +void ConeShape::SetRadius(double _radius) +{ + this->radius = _radius; + this->dirty = true; +} + +////////////////////////////////////////////////// +double ConeShape::GetLength() const +{ + return this->length; +} + +////////////////////////////////////////////////// +void ConeShape::SetLength(double _length) +{ + this->length = _length; + this->dirty = true; +} + +////////////////////////////////////////////////// +void ConeShape::UpdateBoundingBox() +{ + math::Vector3d halfSize(this->radius, this->radius, this->length*0.5); + this->bbox = math::AxisAlignedBox(-halfSize, halfSize); +} + ////////////////////////////////////////////////// CylinderShape::CylinderShape() : Shape() { diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 1cdb15415..55b5374f4 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -59,6 +59,9 @@ enum class GZ_PHYSICS_TPELIB_VISIBLE ShapeType /// \brief A ellipsoid shape. ELLIPSOID = 7, + + /// \brief A cone shape. + CONE = 10, }; @@ -153,6 +156,42 @@ class GZ_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape private: double length = 0.0; }; +/// \brief Cone geometry +class GZ_PHYSICS_TPELIB_VISIBLE ConeShape : public Shape +{ + /// \brief Constructor + public: ConeShape(); + + /// \brief Destructor + public: virtual ~ConeShape() = default; + + /// \brief Get cone radius + /// \return cone radius + public: double GetRadius() const; + + /// \brief Set cone radius + /// \param[in] _radius Cone radius + public: void SetRadius(double _radius); + + /// \brief Get cone length + /// \return Cone length + public: double GetLength() const; + + /// \brief Set cone length + /// \param[in] _length Cone length + public: void SetLength(double _length); + + // Documentation inherited + protected: virtual void UpdateBoundingBox() override; + + /// \brief Cone radius + private: double radius = 0.0; + + /// \brief Cone length + private: double length = 0.0; +}; + + /// \brief Cylinder geometry class GZ_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape { diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 409e222c7..4286fdb49 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -292,6 +293,14 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetLength(capsuleSdf->Length()); collision->SetShape(shape); } + else if (geom->Type() == ::sdf::GeometryType::CONE) + { + const auto coneSdf = geom->ConeShape(); + tpelib::ConeShape shape; + shape.SetRadius(coneSdf->Radius()); + shape.SetLength(coneSdf->Length()); + collision->SetShape(shape); + } else if (geom->Type() == ::sdf::GeometryType::CYLINDER) { const auto cylinderSdf = geom->CylinderShape(); diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index c01b0b9c4..c4f6b7f84 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -82,19 +82,6 @@ Identity ShapeFeatures::AttachBoxShape( return this->GenerateInvalidId(); } -///////////////////////////////////////////////// -Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const -{ - auto it = this->collisions.find(_shapeID); - if (it != this->collisions.end() && it->second != nullptr) - { - auto *shape = it->second->collision->GetShape(); - if (shape != nullptr && dynamic_cast(shape)) - return this->GenerateIdentity(_shapeID, it->second); - } - return this->GenerateInvalidId(); -} - ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const { @@ -172,6 +159,96 @@ Identity ShapeFeatures::AttachCapsuleShape( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const +{ + auto it = this->collisions.find(_shapeID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr && dynamic_cast(shape)) + return this->GenerateIdentity(_shapeID, it->second); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + // assume _coneID ~= _collisionID + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *cone = static_cast(shape); + return cone->GetRadius(); + } + } + // return invalid radius if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + // assume _coneID ~= _collisionID + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *cone = static_cast(shape); + return cone->GetLength(); + } + } + // return invalid height if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + auto it = this->links.find(_linkID); + if (it != this->links.end() && it->second != nullptr) + { + auto &collision = static_cast( + it->second->link->AddCollision()); + collision.SetName(_name); + collision.SetPose(math::eigen3::convert(_pose)); + + tpelib::ConeShape coneshape; + coneshape.SetRadius(_radius); + coneshape.SetLength(_height); + collision.SetShape(coneshape); + + return this->AddCollision(_linkID, collision); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const +{ + auto it = this->collisions.find(_shapeID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr && dynamic_cast(shape)) + return this->GenerateIdentity(_shapeID, it->second); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// double ShapeFeatures::GetCylinderShapeRadius( const Identity &_cylinderID) const diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh index e09564e38..6b90dbc5c 100644 --- a/tpe/plugin/src/ShapeFeatures.hh +++ b/tpe/plugin/src/ShapeFeatures.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList< GetCapsuleShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -89,6 +93,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; @@ -106,7 +127,7 @@ class ShapeFeatures : double _height, const Pose3d &_pose) override; - // ----- Capsule Features ----- + // ----- Ellipsoid Features ----- public: Identity CastToEllipsoidShape( const Identity &_shapeID) const override; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index ddecee5de..cf44d5485 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -603,6 +603,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); EXPECT_NE(nullptr, ellipsoidFreeGroup); + auto cone = world->GetModel("cone"); + auto coneFreeGroup = cone->FindFreeGroup(); + EXPECT_NE(nullptr, coneFreeGroup); + auto box = world->GetModel("box"); // step and get contacts @@ -611,12 +615,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with sphere, cylinder, - // capsule and ellipsoid - EXPECT_EQ(4u, contacts.size()); + // capsule, ellipsoid, and cone + EXPECT_EQ(5u, contacts.size()); unsigned int contactBoxSphere = 0u; unsigned int contactBoxCylinder = 0u; unsigned int contactBoxCapsule = 0u; unsigned int contactBoxEllipsoid = 0u; + unsigned int contactBoxCone = 0u; for (auto &contact : contacts) { @@ -661,6 +666,16 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5); + EXPECT_TRUE(physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)) + << "expected: " << expectedContactPos << "\n" + << " actual: " << contactPoint.point; + } else { FAIL() << "There should not be contacts between: " @@ -671,6 +686,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxCylinder); EXPECT_EQ(1u, contactBoxCapsule); EXPECT_EQ(1u, contactBoxEllipsoid); + EXPECT_EQ(1u, contactBoxCone); // move sphere away sphereFreeGroup->SetWorldPose(math::eigen3::convert( @@ -682,12 +698,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with cylinder, capsule, - // ellipsoid - EXPECT_EQ(3u, contacts.size()); + // ellipsoid, and cone + EXPECT_EQ(4u, contacts.size()); contactBoxCylinder = 0u; contactBoxCapsule = 0u; contactBoxEllipsoid = 0u; + contactBoxCone = 0u; for (auto contact : contacts) { const auto &contactPoint = contact.Get<::TestContactPoint>(); @@ -723,6 +740,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5); + EXPECT_TRUE(physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } else { FAIL() << "There should only be contacts between box and cylinder"; @@ -731,6 +756,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxCylinder); EXPECT_EQ(1u, contactBoxCapsule); EXPECT_EQ(1u, contactBoxEllipsoid); + EXPECT_EQ(1u, contactBoxCone); // move cylinder away cylinderFreeGroup->SetWorldPose(math::eigen3::convert( @@ -744,6 +770,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) ellipsoidFreeGroup->SetWorldPose(math::eigen3::convert( math::Pose3d(0, -100, -100, 0, 0, 0))); + // move cone away + coneFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, -200, 0, 0, 0))); + // step and get contacts checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); From f54d4ded615626d3cff34a5dcfa0cedfe8bd903f Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 25 Jun 2024 19:21:26 -0400 Subject: [PATCH 4/7] Prepare for 7.3.0 (#659) Signed-off-by: Benjamin Perseghetti Co-authored-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 47 +++++++++++++++++++++++++++++++++++++++++++++++ package.xml | 2 +- 3 files changed, 49 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fd48ffb51..9b13a30e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-physics7 VERSION 7.2.0) +project(gz-physics7 VERSION 7.3.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index b2cbe43bd..d2166a972 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,52 @@ ## 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/package.xml b/package.xml index 964ebee39..d5fe5e2e7 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ gz-physics7 - 7.2.0 + 7.3.0 Gazebo Physics : Physics classes and functions for robot applications Steve Peters Apache License 2.0 From 707aa699c196caa6eb384c74ea75ed3a382c5129 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 08:14:46 -0700 Subject: [PATCH 5/7] bullet-featherstone: Enforce joint velocity and effort limits for velocity control commands (#658) Signed-off-by: Ian Chen --- bullet-featherstone/README.md | 9 +- bullet-featherstone/src/Base.hh | 12 +- bullet-featherstone/src/JointFeatures.cc | 171 +++++++++++++++++- bullet-featherstone/src/JointFeatures.hh | 27 +++ bullet-featherstone/src/SDFFeatures.cc | 19 +- bullet-featherstone/src/SimulationFeatures.cc | 9 +- bullet-featherstone/src/SimulationFeatures.hh | 2 - test/common_test/CMakeLists.txt | 5 + test/common_test/joint_features.cc | 33 ++-- test/common_test/worlds/test.world | 18 +- 10 files changed, 270 insertions(+), 35 deletions(-) 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 4ead27ed3..55b606cee 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) { @@ -277,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(); } @@ -409,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 8e6c4a742..ffc95c846 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( 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 eac823f60..0268857d7 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 f67f219a0..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); diff --git a/test/common_test/worlds/test.world b/test/common_test/worlds/test.world index b75496857..715aa2472 100644 --- a/test/common_test/worlds/test.world +++ b/test/common_test/worlds/test.world @@ -313,6 +313,15 @@ 100 + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + 0 0 0 -1.57 0 0 @@ -328,12 +337,19 @@ 1 + + + + 0.1 0.1 0.1 + + + 0.1 0.1 0.1 - + From fa53310e58395704c9ca8de41a4a5b3b31c93031 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Jul 2024 10:23:35 -0700 Subject: [PATCH 6/7] Fix compile warning (#663) Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 55b606cee..59d5c65a8 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -201,9 +201,9 @@ double JointFeatures::GetJointForce( // 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::Vector3 axis_converted(axis[0], axis[1], axis[2]); + math::Vector3d axis_converted(axis[0], axis[1], axis[2]); btVector3 angular = feedback->m_reactionForces.getAngular(); - math::Vector3 angularTorque( + math::Vector3d angularTorque( angular.getX(), angular.getY(), angular.getZ()); @@ -218,9 +218,9 @@ double JointFeatures::GetJointForce( else if (link.m_jointType == btMultibodyLink::ePrismatic) { auto axis = convert(link.getAxisBottom(0)); - math::Vector3 axis_converted(axis[0], axis[1], axis[2]); + math::Vector3d axis_converted(axis[0], axis[1], axis[2]); btVector3 linear = feedback->m_reactionForces.getLinear(); - math::Vector3 linearForce( + math::Vector3d linearForce( linear.getX(), linear.getY(), linear.getZ()); From cca7e923702b1711f4f16db8c869eec5e710acf7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 9 Jul 2024 14:05:05 -0700 Subject: [PATCH 7/7] bullet-featherstone: Support empty links (#665) Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 169 ++++++++++++++----------- bullet-featherstone/src/SDFFeatures.hh | 8 ++ test/common_test/worlds/test.world | 16 --- 3 files changed, 101 insertions(+), 92 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index ffc95c846..ebb7c686c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -890,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); + } } } @@ -955,7 +964,7 @@ Identity SDFFeatures::ConstructSdfModel( bool SDFFeatures::AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, - bool isStatic) + bool _isStatic) { if (!_collision.Geom()) { @@ -1283,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(); - - // 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); + this->CreateLinkCollider(_linkID, _isStatic, shape.get(), + btInertialToCollision); - linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); - - linkInfo->collider->setCollisionShape(linkInfo->shape.get()); linkInfo->collider->setRestitution(static_cast(restitution)); linkInfo->collider->setRollingFriction( static_cast(rollingFriction)); @@ -1317,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) { @@ -1495,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/test/common_test/worlds/test.world b/test/common_test/worlds/test.world index 715aa2472..4ee56c3b2 100644 --- a/test/common_test/worlds/test.world +++ b/test/common_test/worlds/test.world @@ -313,15 +313,6 @@ 100 - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - 0 0 0 -1.57 0 0 @@ -337,13 +328,6 @@ 1 - - - - 0.1 0.1 0.1 - - -