diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 2a89d8614..cdae3d479 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -24,7 +24,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -37,12 +39,16 @@ #include #include +#include +#include #include #include #include #include +#include #include +#include #include #include #include @@ -55,6 +61,8 @@ #include #include +#include "CustomMeshShape.hh" + namespace ignition { namespace physics { namespace dartsim { @@ -273,6 +281,14 @@ static ShapeAndTransform ConstructSphere( return {std::make_shared(_sphere.Radius())}; } +///////////////////////////////////////////////// +static ShapeAndTransform ConstructCapsule( + const ::sdf::Capsule &_capsule) +{ + return {std::make_shared( + _capsule.Radius(), _capsule.Length())}; +} + ///////////////////////////////////////////////// static ShapeAndTransform ConstructPlane( const ::sdf::Plane &_plane) @@ -315,8 +331,31 @@ static ShapeAndTransform ConstructGeometry( { if (_geometry.BoxShape()) return ConstructBox(*_geometry.BoxShape()); + else if (_geometry.CapsuleShape()) + { + return ConstructCapsule(*_geometry.CapsuleShape()); + } else if (_geometry.CylinderShape()) return ConstructCylinder(*_geometry.CylinderShape()); + else if (_geometry.EllipsoidShape()) + { + // TODO(anyone): Replace this code when Ellipsoid is supported by DART + common::MeshManager *meshMgr = common::MeshManager::Instance(); + std::string ellipsoidMeshName = std::string("ellipsoid_mesh") + + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X()) + + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Y()) + + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Z()); + meshMgr->CreateEllipsoid( + ellipsoidMeshName, + _geometry.EllipsoidShape()->Radii(), + 6, 12); + const ignition::common::Mesh * _mesh = + meshMgr->MeshByName(ellipsoidMeshName); + + auto mesh = std::make_shared(*_mesh, Vector3d(1, 1, 1)); + auto mesh2 = std::dynamic_pointer_cast(mesh); + return {mesh2}; + } else if (_geometry.SphereShape()) return ConstructSphere(*_geometry.SphereShape()); else if (_geometry.PlaneShape()) @@ -739,6 +778,8 @@ Identity SDFFeatures::ConstructSdfCollision( if (!shape) { // The geometry element was empty, or the shape type is not supported + ignerr << "The geometry element of collision [" << _collision.Name() << "] " + << "couldn't be created\n"; return this->GenerateInvalidId(); } @@ -853,6 +894,8 @@ Identity SDFFeatures::ConstructSdfVisual( if (!shape) { // The geometry element was empty, or the shape type is not supported + ignerr << "The geometry element of visual [" << _visual.Name() << "] " + << "couldn't be created\n"; return this->GenerateInvalidId(); } diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 09a2c77b0..ce5f5438d 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -862,6 +862,27 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) expPose, link1->getWorldTransform(), 1e-5)); } +///////////////////////////////////////////////// +TEST_P(SDFFeatures_TEST, Shapes) +{ + auto world = this->LoadWorld(TEST_WORLD_DIR"/shapes.sdf"); + ASSERT_NE(nullptr, world); + + auto dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + ASSERT_EQ(5u, dartWorld->getNumSkeletons()); + + int count{0}; + for (auto name : {"box", "cylinder", "sphere", "capsule", "ellipsoid"}) + { + const auto skeleton = dartWorld->getSkeleton(count++); + ASSERT_NE(nullptr, skeleton); + EXPECT_EQ(name, skeleton->getName()); + ASSERT_EQ(1u, skeleton->getNumBodyNodes()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 11d042325..c51264149 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -20,12 +20,17 @@ #include #include +#include #include +#include #include #include #include #include +#include +#include + #include "CustomMeshShape.hh" namespace ignition { @@ -93,6 +98,67 @@ Identity ShapeFeatures::AttachBoxShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCapsuleShape(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::GetCapsuleShapeRadius( + const Identity &_capsuleID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_capsuleID); + + dart::dynamics::CapsuleShape *capsule = + static_cast( + shapeInfo->node->getShape().get()); + + return capsule->getRadius(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCapsuleShapeLength( + const Identity &_capsuleID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_capsuleID); + dart::dynamics::CapsuleShape *capsule = + static_cast( + shapeInfo->node->getShape().get()); + + return capsule->getHeight(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _length, + const Pose3d &_pose) +{ + auto capsule = std::make_shared( + _radius, _length); + + auto bn = this->ReferenceInterface(_linkID)->link; + dart::dynamics::ShapeNode *sn = + bn->createShapeNodeWith( + capsule, 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 { @@ -154,6 +220,62 @@ Identity ShapeFeatures::AttachCylinderShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToEllipsoidShape(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(); +} + +///////////////////////////////////////////////// +Vector3d ShapeFeatures::GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_ellipsoidID); + + dart::dynamics::EllipsoidShape *ellipsoid = + static_cast( + shapeInfo->node->getShape().get()); + + return ellipsoid->getRadii(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + const Vector3d _radii, + const Pose3d &_pose) +{ + common::MeshManager *meshMgr = common::MeshManager::Instance(); + std::string ellipsoidMeshName = _name + "_ellipsoid_mesh" + + "_" + std::to_string(_radii[0]) + + "_" + std::to_string(_radii[1]) + + "_" + std::to_string(_radii[2]); + meshMgr->CreateEllipsoid(ellipsoidMeshName, + ignition::math::Vector3d(_radii[0], _radii[1], _radii[2]), + 16, 16); + const ignition::common::Mesh * _mesh = meshMgr->MeshByName(ellipsoidMeshName); + + auto mesh = std::make_shared(*_mesh, Vector3d(1, 1, 1)); + + 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::CastToSphereShape( const Identity &_shapeID) const diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 9ec443714..c32b6fed5 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -22,7 +22,9 @@ #include #include +#include #include +#include #include #include #include @@ -48,10 +50,18 @@ struct ShapeFeatureList : FeatureList< // SetBoxShapeProperties, AttachBoxShapeFeature, + GetCapsuleShapeProperties, + // SetCapsulerShapeProperties, + AttachCapsuleShapeFeature, + GetCylinderShapeProperties, // SetCylinderShapeProperties, AttachCylinderShapeFeature, + GetEllipsoidShapeProperties, + // SetEllipsoidShapeProperties, + AttachEllipsoidShapeFeature, + GetSphereShapeProperties, // SetSphereShapeProperties, AttachSphereShapeFeature, @@ -89,6 +99,22 @@ class ShapeFeatures : const LinearVector3d &_size, const Pose3d &_pose) override; + // ----- Capsule Features ----- + public: Identity CastToCapsuleShape( + const Identity &_shapeID) const override; + + public: double GetCapsuleShapeRadius( + const Identity &_capsuleID) const override; + + public: double GetCapsuleShapeLength( + const Identity &_capsuleID) const override; + + public: Identity AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _length, + const Pose3d &_pose) override; // ----- Cylinder Features ----- public: Identity CastToCylinderShape( @@ -107,6 +133,18 @@ class ShapeFeatures : double _height, const Pose3d &_pose) override; + // ----- Ellipsoid Features ----- + public: Identity CastToEllipsoidShape( + const Identity &_shapeID) const override; + + public: Vector3d GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const override; + + public: Identity AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + const Vector3d _radii, + const Pose3d &_pose) override; // ----- Sphere Features ----- public: Identity CastToSphereShape( diff --git a/dartsim/worlds/shapes.sdf b/dartsim/worlds/shapes.sdf new file mode 100644 index 000000000..733c82ac2 --- /dev/null +++ b/dartsim/worlds/shapes.sdf @@ -0,0 +1,191 @@ + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 0.1458 + 0 + 0 + 0.1458 + 0 + 0.125 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0 -3.0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + diff --git a/include/ignition/physics/CapsuleShape.hh b/include/ignition/physics/CapsuleShape.hh new file mode 100644 index 000000000..511da9efc --- /dev/null +++ b/include/ignition/physics/CapsuleShape.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2021 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 IGNITION_PHYSICS_CAPSULESHAPE_HH_ +#define IGNITION_PHYSICS_CAPSULESHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(CapsuleShape) + + class IGNITION_PHYSICS_VISIBLE GetCapsuleShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CapsuleShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Get the radius of this CapsuleShape + /// \return the radius of this CapsuleShape + public: Scalar GetRadius() const; + + /// \brief Get the length along the local z-axis of this + /// CapsuleShape's cylinder. + /// \return the length of this CapsuleShape + public: Scalar GetLength() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual Scalar GetCapsuleShapeRadius( + const Identity &_capsuleID) const = 0; + + public: virtual Scalar GetCapsuleShapeLength( + const Identity &_capsuleID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the CapsuleShape properties such as + /// the capsule radius and length. + class IGNITION_PHYSICS_VISIBLE SetCapsuleShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CapsuleShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the radius of this CapsuleShape + /// \param[in] _radius + /// The desired radius of this CapsuleShape + public: void SetRadius(Scalar _radius); + + /// \brief Set the length of this CapsuleShape + /// \param[in] _length + /// The desired length of this CapsuleShape + public: void SetLength(Scalar length); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual void SetCapsuleShapeRadius( + const Identity &_capsuleID, Scalar _radius) = 0; + + public: virtual void SetCapsuleShapeLength( + const Identity &_capsuleID, Scalar _length) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new capsule shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// capsule center point in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachCapsuleShapeFeature + : 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 = CapsuleShapePtr; + + /// \brief Rigidly attach a CapsuleShape to this link. + /// \param[in] _radius + /// The radius of the capsule. + /// \param[in] _length + /// The length of the capsule. + /// \param[in] _pose + /// The desired pose of the CapsuleShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed CapsuleShape + public: ShapePtrType AttachCapsuleShape( + const std::string &_name = "capsule", + Scalar _radius = 0.5, + Scalar _length = 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 AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + Scalar _radius, + Scalar _length, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/ignition/physics/EllipsoidShape.hh b/include/ignition/physics/EllipsoidShape.hh new file mode 100644 index 000000000..7c01b0b1b --- /dev/null +++ b/include/ignition/physics/EllipsoidShape.hh @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2021 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 IGNITION_PHYSICS_ELLIPSOIDSHAPE_HH_ +#define IGNITION_PHYSICS_ELLIPSOIDSHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(EllipsoidShape) + + class IGNITION_PHYSICS_VISIBLE GetEllipsoidShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class EllipsoidShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Get the radius of this EllipsoidShape + /// \return the radius of this EllipsoidShape + public: Dimensions GetRadii() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Dimensions GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the EllipsoidShape properties such as + /// the ellipsoid radii. + class IGNITION_PHYSICS_VISIBLE SetEllipsoidShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class EllipsoidShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Set the radius of this EllipsoidShape + /// \param[in] _radii + /// The desired radius of this EllipsoidShape + public: void SetRadii(Dimensions _radii); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual void SetEllipsoidShapeRadii( + const Identity &_ellipsoidID, Dimensions _radii) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new ellipsoid shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// ellipsoid center point in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachEllipsoidShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = EllipsoidShapePtr; + + /// \brief Rigidly attach a EllipsoidShape to this link. + /// \param[in] _radii + /// The radius of the ellipsoid. + /// \param[in] _pose + /// The desired pose of the EllipsoidShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed EllipsoidShape + public: ShapePtrType AttachEllipsoidShape( + const std::string &_name = "ellipsoid", + Dimensions _radii = Dimensions::Constant(1.0), + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + Dimensions _radii, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/ignition/physics/detail/CapsuleShape.hh b/include/ignition/physics/detail/CapsuleShape.hh new file mode 100644 index 000000000..3481fecf6 --- /dev/null +++ b/include/ignition/physics/detail/CapsuleShape.hh @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2021 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 IGNITION_PHYSICS_DETAIL_CAPSULESHAPE_HH_ +#define IGNITION_PHYSICS_DETAIL_CAPSULESHAPE_HH_ + +#include + +#include + +namespace ignition +{ + namespace physics + { + ///////////////////////////////////////////////// + template + auto GetCapsuleShapeProperties::CapsuleShape + ::GetRadius() const -> Scalar + { + return this->template Interface() + ->GetCapsuleShapeRadius(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetCapsuleShapeProperties::CapsuleShape + ::GetLength() const -> Scalar + { + return this->template Interface() + ->GetCapsuleShapeLength(this->identity); + } + + ///////////////////////////////////////////////// + template + void SetCapsuleShapeProperties::CapsuleShape + ::SetRadius(Scalar _radius) + { + this->template Interface() + ->SetCapsuleShapeRadius(this->identity, _radius); + } + + ///////////////////////////////////////////////// + template + void SetCapsuleShapeProperties::CapsuleShape + ::SetLength(Scalar _length) + { + this->template Interface() + ->SetCapsuleShapeLength(this->identity, _length); + } + + ///////////////////////////////////////////////// + template + auto AttachCapsuleShapeFeature::Link + ::AttachCapsuleShape( + const std::string &_name, + Scalar _radius, + Scalar _length, + const PoseType &_pose) -> ShapePtrType + { + return ShapePtrType(this->pimpl, + this->template Interface() + ->AttachCapsuleShape( + this->identity, _name, _radius, _length, _pose)); + } + } +} + +#endif diff --git a/include/ignition/physics/detail/EllipsoidShape.hh b/include/ignition/physics/detail/EllipsoidShape.hh new file mode 100644 index 000000000..4bc16a40c --- /dev/null +++ b/include/ignition/physics/detail/EllipsoidShape.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 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 IGNITION_PHYSICS_DETAIL_ELLIPSOIDSHAPE_HH_ +#define IGNITION_PHYSICS_DETAIL_ELLIPSOIDSHAPE_HH_ + +#include + +#include + +namespace ignition +{ + namespace physics + { + ///////////////////////////////////////////////// + template + auto GetEllipsoidShapeProperties::EllipsoidShape + ::GetRadii() const -> Dimensions + { + return this->template Interface() + ->GetEllipsoidShapeRadii(this->identity); + } + ///////////////////////////////////////////////// + template + void SetEllipsoidShapeProperties::EllipsoidShape + ::SetRadii(Dimensions _radii) + { + this->template Interface() + ->SetEllipsoidShapeRadii(this->identity, _radii); + } + + ///////////////////////////////////////////////// + template + auto AttachEllipsoidShapeFeature::Link + ::AttachEllipsoidShape( + const std::string &_name, + Dimensions _radii, + const PoseType &_pose) -> ShapePtrType + { + return ShapePtrType(this->pimpl, + this->template Interface() + ->AttachEllipsoidShape( + this->identity, _name, _radii, _pose)); + } + } +} + +#endif diff --git a/test/performance/symbol_names.cc b/test/performance/symbol_names.cc index fdf9718d5..e8f60f824 100644 --- a/test/performance/symbol_names.cc +++ b/test/performance/symbol_names.cc @@ -21,12 +21,28 @@ #include #include +#include #include +#include #include #include #include #include + +///////////////////////////////////////////////// +struct CapsuleFeaturesClass : ignition::physics::FeatureList< + ignition::physics::GetCapsuleShapeProperties, + ignition::physics::SetCapsuleShapeProperties, + ignition::physics::AttachCapsuleShapeFeature +> { }; + +using CapsuleFeaturesAlias = ignition::physics::FeatureList< + ignition::physics::GetCapsuleShapeProperties, + ignition::physics::SetCapsuleShapeProperties, + ignition::physics::AttachCapsuleShapeFeature +>; + ///////////////////////////////////////////////// struct CylinderFeaturesClass : ignition::physics::FeatureList< ignition::physics::GetCylinderShapeProperties, @@ -40,6 +56,19 @@ using CylinderFeaturesAlias = ignition::physics::FeatureList< ignition::physics::AttachCylinderShapeFeature >; +///////////////////////////////////////////////// +struct EllipsoidFeaturesClass : ignition::physics::FeatureList< + ignition::physics::GetEllipsoidShapeProperties, + ignition::physics::SetEllipsoidShapeProperties, + ignition::physics::AttachEllipsoidShapeFeature +> { }; + +using EllipsoidFeaturesAlias = ignition::physics::FeatureList< + ignition::physics::GetEllipsoidShapeProperties, + ignition::physics::SetEllipsoidShapeProperties, + ignition::physics::AttachEllipsoidShapeFeature +>; + ///////////////////////////////////////////////// struct JointFeaturesClass : ignition::physics::FeatureList< ignition::physics::GetRevoluteJointProperties, @@ -66,13 +95,17 @@ using BoxFeaturesAlias = ignition::physics::FeatureList< ///////////////////////////////////////////////// struct FeatureSetClass : ignition::physics::FeatureList< + CapsuleFeaturesClass, CylinderFeaturesClass, + EllipsoidFeaturesClass, JointFeaturesClass, BoxFeaturesClass > { }; using FeatureSetAlias = ignition::physics::FeatureList< + CapsuleFeaturesAlias, CylinderFeaturesAlias, + EllipsoidFeaturesAlias, JointFeaturesAlias, BoxFeaturesAlias >; diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index bb250beb6..583c467dd 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -291,6 +291,11 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetRadius(sphereSdf->Radius()); collision->SetShape(shape); } + else + { + ignwarn << "Geometry type not supported for collision [" << name << "]." + << std::endl; + } // \todo(anyone) add mesh. currently mesh has to be loaded externally // and passed in as argument as there is no logic for searching resources // in ign-physics