Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Cone as a collision shape #639

Merged
merged 9 commits into from
Jun 13, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions bullet-featherstone/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ These are the specific physics API features implemented.
* AttachBoxShapeFeature
* GetCapsuleShapeProperties
* AttachCapsuleShapeFeature
* GetConeShapeProperties
* AttachConeShapeFeature
* GetCylinderShapeProperties
* AttachCylinderShapeFeature
* GetEllipsoidShapeProperties
Expand Down
9 changes: 9 additions & 0 deletions bullet-featherstone/src/SDFFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include <sdf/Box.hh>
#include <sdf/Capsule.hh>
#include <sdf/Cone.hh>
#include <sdf/Cylinder.hh>
#include <sdf/Ellipsoid.hh>
#include <sdf/Geometry.hh>
Expand Down Expand Up @@ -963,6 +964,14 @@ bool SDFFeatures::AddSdfCollision(
const auto radius = sphere->Radius();
shape = std::make_unique<btSphereShape>(static_cast<btScalar>(radius));
}
else if (const auto *cone = geom->ConeShape())
{
const auto radius = static_cast<btScalar>(cone->Radius());
const auto height = static_cast<btScalar>(cone->Length());
shape =
std::make_unique<btConeShapeZ>(radius, height);
shape->setMargin(0.0);
}
else if (const auto *cylinder = geom->CylinderShape())
{
const auto radius = static_cast<btScalar>(cylinder->Radius());
Expand Down
80 changes: 80 additions & 0 deletions bullet-featherstone/src/ShapeFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,86 @@ Identity ShapeFeatures::AttachCapsuleShape(
return identity;
}

/////////////////////////////////////////////////
Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
{
const auto *shapeInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
if (shapeInfo != nullptr)
{
const auto &shape = shapeInfo->collider;
if (dynamic_cast<btConeShape*>(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<btConeShape*>(
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<btConeShape*>(
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<btScalar>(_radius);
const auto height = static_cast<btScalar>(_height);
auto shape =
std::make_unique<btConeShapeZ>(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
{
Expand Down
21 changes: 21 additions & 0 deletions bullet-featherstone/src/ShapeFeatures.hh
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gz/physics/Shape.hh>
#include <gz/physics/BoxShape.hh>
#include <gz/physics/CapsuleShape.hh>
#include <gz/physics/ConeShape.hh>
#include <gz/physics/CylinderShape.hh>
#include <gz/physics/EllipsoidShape.hh>
#include <gz/physics/SphereShape.hh>
Expand All @@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList<
GetCapsuleShapeProperties,
AttachCapsuleShapeFeature,

GetConeShapeProperties,
AttachConeShapeFeature,

GetCylinderShapeProperties,
AttachCylinderShapeFeature,

Expand Down Expand Up @@ -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;
Expand Down
11 changes: 10 additions & 1 deletion bullet/src/SDFFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,10 @@
#include <sdf/Geometry.hh>
#include <sdf/Box.hh>
#include <sdf/Capsule.hh>
#include <sdf/Cone.hh>
#include <sdf/Cylinder.hh>
#include <sdf/Ellipsoid.hh>
#include <sdf/Sphere.hh>
#include <sdf/Cylinder.hh>
#include <sdf/Plane.hh>
#include <sdf/JointAxis.hh>

Expand Down Expand Up @@ -228,6 +229,14 @@ Identity SDFFeatures::ConstructSdfCollision(
const auto radius = static_cast<btScalar>(sphere->Radius());
shape = std::make_shared<btSphereShape>(radius);
}
else if (geom->ConeShape())
{
const auto cone = geom->ConeShape();
const auto radius = static_cast<btScalar>(cone->Radius());
const auto height = static_cast<btScalar>(cone->Length());
shape =
std::make_shared<btConeShapeZ>(radius, height);
}
else if (geom->CylinderShape())
{
const auto cylinder = geom->CylinderShape();
Expand Down
62 changes: 62 additions & 0 deletions dartsim/src/CustomConeMeshShape.cc
Original file line number Diff line number Diff line change
@@ -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 <memory>
#include <string>

#include <gz/common/Console.hh>
#include <gz/common/MeshManager.hh>
#include <gz/math/Cone.hh>

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)
{
}

}
}
}
44 changes: 44 additions & 0 deletions dartsim/src/CustomConeMeshShape.hh
Original file line number Diff line number Diff line change
@@ -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 <gz/math/Cone.hh>
#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_
20 changes: 20 additions & 0 deletions dartsim/src/SDFFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,14 @@
#include <cmath>
#include <limits>
#include <memory>
#include <string>
#include <utility>

#include <dart/constraint/ConstraintSolver.hpp>
#include <dart/dynamics/BallJoint.hpp>
#include <dart/dynamics/BoxShape.hpp>
#include <dart/dynamics/CapsuleShape.hpp>
#include <dart/dynamics/ConeShape.hpp>
#include <dart/dynamics/CylinderShape.hpp>
#include <dart/dynamics/EllipsoidShape.hpp>
#include <dart/dynamics/FreeJoint.hpp>
Expand All @@ -49,6 +51,7 @@
#include <sdf/Box.hh>
#include <sdf/Collision.hh>
#include <sdf/Capsule.hh>
#include <sdf/Cone.hh>
#include <sdf/Cylinder.hh>
#include <sdf/Ellipsoid.hh>
#include <sdf/Geometry.hh>
Expand All @@ -65,6 +68,7 @@
#include <sdf/World.hh>

#include "AddedMassFeatures.hh"
#include "CustomConeMeshShape.hh"
#include "CustomMeshShape.hh"

namespace gz {
Expand Down Expand Up @@ -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<CustomConeMeshShape>(_geometry.ConeShape()->Shape());
auto mesh2 = std::dynamic_pointer_cast<dart::dynamics::MeshShape>(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())
Expand Down
5 changes: 3 additions & 2 deletions dartsim/src/SDFFeatures_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading