A class that manages both solid and mesh geometry for various parts of the motion planning system. More...

#include <geometry.h>

Classes
class	ShapeType
	Supported shape types. More...

Public Member Functions
	Geometry (ShapeType::Type type, const Eigen::Vector3d &dimensions, const std::string &resource="", const EigenSTL::vector_Vector3d &vertices=EigenSTL::vector_Vector3d{})
	Constructor. Builds and loads the specified geometry. More...

	Geometry (const shapes::Shape &shape)
	Constructor. Builds and loads the specified geometry from a MoveIt shape. More...

	Geometry (const shape_msgs::SolidPrimitive &msg)
	Constructor. Builds and loads the specified geometry from a MoveIt shape message. More...

	Geometry (const shape_msgs::Mesh &msg)
	Constructor. Builds and loads the specified geometry from a MoveIt shape message. More...

	Geometry (const Geometry &)=delete

Geometry &	operator= (const Geometry &)=delete

bool	contains (const Eigen::Vector3d &point) const
	Checks if the geometry contains a point. More...

std::pair< bool, Eigen::Vector3d >	sample (const unsigned int attempts=50) const
	Tries to sample a point in the geometry. More...

bool	isMesh () const
	Checks if the geometry is a mesh geometry. More...

const shape_msgs::SolidPrimitive	getSolidMsg () const
	Gets the message form of solid primitive geometry (all but ShapeType::MESH). More...

const shape_msgs::Mesh	getMeshMsg () const
	Gets the message form of mesh geometry. More...

const shapes::ShapePtr &	getShape () const
	Gets the underlying shape. More...

const bodies::BodyPtr &	getBody () const
	Gets the underlying body. More...

ShapeType::Type	getType () const
	Gets the type of the geometry. More...

const std::string &	getResource () const
	Gets the mesh resource of the geometry. More...

const EigenSTL::vector_Vector3d &	getVertices () const
	Gets the Vertices of the primitive. More...

const Eigen::Vector3d &	getDimensions () const
	Gets the dimensions of the geometry. More...

Eigen::AlignedBox3d	getAABB (const RobotPose &pose=RobotPose::Identity()) const
	Compute the AABB (axis-aligned bounding box) of the geometry at a given pose. More...

Static Public Member Functions
static GeometryPtr	makeSphere (double radius)
	Create a sphere. More...

static GeometryPtr	makeBox (double x, double y, double z)
	Create a box. More...

static GeometryPtr	makeBox (const Eigen::Vector3d &dimensions)
	Create a box. More...

static GeometryPtr	makeCylinder (double radius, double length)
	Create a cylinder. More...

static GeometryPtr	makeCone (double radius, double length)
	Create a cone. More...

static GeometryPtr	makeSolidPrimitive (const shape_msgs::SolidPrimitive &msg)
	Create a solid primitive. More...

static GeometryPtr	makeMesh (const std::string &resource, const Eigen::Vector3d &scale={1, 1, 1})
	Create a mesh from resource file. More...

static GeometryPtr	makeMesh (const EigenSTL::vector_Vector3d &vertices)
	Create a mesh from triangles represented as vertices . More...

Private Member Functions
shapes::Shape *	loadShape () const
	Loads a shape from the set type_ and dimensions_, and resource_ if a mesh. More...

bodies::Body *	loadBody () const
	Loads a body from the loaded shape_. More...

Private Attributes
ShapeType::Type	type_ {ShapeType::Type::BOX}
	Geometry Type. More...

Eigen::Vector3d	dimensions_ {Eigen::Vector3d::Ones()}
	Dimensions to scale geometry. More...

EigenSTL::vector_Vector3d	vertices_ {{}}
	Vertices of the primitive. More...

std::string	resource_ {""}
	Resource locator for MESH types. More...

shapes::ShapePtr	shape_ {nullptr}
	Loaded shape. More...

bodies::BodyPtr	body_ {nullptr}
	Body operation. More...

Detailed Description

A class that manages both solid and mesh geometry for various parts of the motion planning system.

Definition at line 35 of file geometry.h.

Constructor & Destructor Documentation

◆ Geometry() [1/5]

Geometry::Geometry	(	ShapeType::Type	type,
		const Eigen::Vector3d &	dimensions,
		const std::string &	resource = `""`,
		const EigenSTL::vector_Vector3d &	vertices = `EigenSTL::vector_Vector3d{}`
	)

Constructor. Builds and loads the specified geometry.

Parameters

[in]	type	Type of the geometry to create.
[in]	dimensions	Dimensions of the geometry to load.
[in]	resource	If type is ShapeType::MESH, then resource or vertices must be specified
[in]	vertices	List of vertices that form the mesh. as the mesh file to load.

Definition at line 101 of file geometry.cpp.

   : type_(type)
   , dimensions_(dimensions)
   , vertices_(vertices)
   , resource_((resource.empty()) ? "" : IO::resolvePath(resource))
   , shape_(loadShape())
   , body_(loadBody())
 {
 }

◆ Geometry() [2/5]

Geometry::Geometry ( const shapes::Shape & shape )

Constructor. Builds and loads the specified geometry from a MoveIt shape.

Parameters

[in] shape Shape to construct geometry from.

Definition at line 112 of file geometry.cpp.

 {
     switch (shape.type)
     {
         case shapes::ShapeType::BOX:
         {
             type_ = ShapeType::BOX;
             const auto &box = static_cast<const shapes::Box &>(shape);
             dimensions_ = Eigen::Vector3d{box.size[0], box.size[1], box.size[2]};
             shape_.reset(loadShape());
             break;
         }
         case shapes::ShapeType::SPHERE:
         {
             type_ = ShapeType::SPHERE;
             const auto &sphere = static_cast<const shapes::Sphere &>(shape);
             dimensions_ = Eigen::Vector3d{sphere.radius, 0, 0};
             shape_.reset(loadShape());
             break;
         }
         case shapes::ShapeType::CYLINDER:
         {
             type_ = ShapeType::CYLINDER;
             const auto &cylinder = static_cast<const shapes::Cylinder &>(shape);
             dimensions_ = Eigen::Vector3d{cylinder.radius, cylinder.length, 0};
             shape_.reset(loadShape());
             break;
         }
         case shapes::ShapeType::CONE:
         {
             type_ = ShapeType::CONE;
             const auto &cone = static_cast<const shapes::Cone &>(shape);
             dimensions_ = Eigen::Vector3d{cone.radius, cone.length, 0};
             shape_.reset(loadShape());
             break;
         }
         case shapes::ShapeType::MESH:
         {
             type_ = ShapeType::MESH;
             const auto &mesh = static_cast<const shapes::Mesh &>(shape);
             shape_.reset(mesh.clone());
             break;
         }
         default:
             throw Exception(1, "Invalid type for geometry.");
     }
  
     body_.reset(loadBody());
 }

◆ Geometry() [3/5]

Geometry::Geometry ( const shape_msgs::SolidPrimitive & msg )

Constructor. Builds and loads the specified geometry from a MoveIt shape message.

Parameters

[in] msg Shape message to construct geometry from.

Definition at line 162 of file geometry.cpp.

                                                       : Geometry(*shapes::constructShapeFromMsg(msg))
 {
 }

◆ Geometry() [4/5]

Geometry::Geometry ( const shape_msgs::Mesh & msg )

Constructor. Builds and loads the specified geometry from a MoveIt shape message.

Parameters

[in] msg Shape message to construct mesh geometry from.

Definition at line 166 of file geometry.cpp.

                                             : Geometry(*shapes::constructShapeFromMsg(msg))
 {
 }

◆ Geometry() [5/5]

robowflex::Geometry::Geometry ( const Geometry & )

delete

Member Function Documentation

◆ contains()

bool Geometry::contains ( const Eigen::Vector3d & point ) const

Checks if the geometry contains a point.

Parameters

[in] point The point to check, in the geometry's frame.

Returns: True if the geometry contains the point, false otherwise.

Definition at line 235 of file geometry.cpp.

 {
     return body_->containsPoint(point[0], point[1], point[2]);
 }

◆ getAABB()

Eigen::AlignedBox3d Geometry::getAABB ( const RobotPose & pose = RobotPose::Identity() ) const

Compute the AABB (axis-aligned bounding box) of the geometry at a given pose.

Parameters

[in] pose Pose to compute AABB of geometry at.

Returns: The AABB.

Definition at line 304 of file geometry.cpp.

 {
     moveit::core::AABB aabb;
  
     const auto &extents = shapes::computeShapeExtents(shape_.get());
     aabb.extendWithTransformedBox(pose, extents);
  
     return aabb;
 }

◆ getBody()

const bodies::BodyPtr & Geometry::getBody ( ) const

Gets the underlying body.

Returns: The body.

Definition at line 279 of file geometry.cpp.

 {
     return body_;
 }

◆ getDimensions()

const Eigen::Vector3d & Geometry::getDimensions ( ) const

Gets the dimensions of the geometry.

Returns: The dimensions of geometry.

Definition at line 299 of file geometry.cpp.

 {
     return dimensions_;
 }

◆ getMeshMsg()

const shape_msgs::Mesh Geometry::getMeshMsg ( ) const

Gets the message form of mesh geometry.

Returns: The message.

Definition at line 265 of file geometry.cpp.

 {
     shapes::ShapeMsg msg;
     if (isMesh())
         shapes::constructMsgFromShape(shape_.get(), msg);
  
     return boost::get<shape_msgs::Mesh>(msg);
 }

◆ getResource()

const std::string & Geometry::getResource ( ) const

Gets the mesh resource of the geometry.

Returns: The mesh resource of geometry.

Definition at line 289 of file geometry.cpp.

 {
     return resource_;
 }

◆ getShape()

const shapes::ShapePtr & Geometry::getShape ( ) const

Gets the underlying shape.

Returns: The shape.

Definition at line 274 of file geometry.cpp.

 {
     return shape_;
 }

◆ getSolidMsg()

const shape_msgs::SolidPrimitive Geometry::getSolidMsg ( ) const

Gets the message form of solid primitive geometry (all but ShapeType::MESH).

Returns: The message.

Definition at line 256 of file geometry.cpp.

 {
     shapes::ShapeMsg msg;
     if (!isMesh())
         shapes::constructMsgFromShape(shape_.get(), msg);
  
     return boost::get<shape_msgs::SolidPrimitive>(msg);
 }

◆ getType()

Geometry::ShapeType::Type Geometry::getType ( ) const

Gets the type of the geometry.

Returns: The type of geometry.

Definition at line 284 of file geometry.cpp.

 {
     return type_;
 }

◆ getVertices()

const EigenSTL::vector_Vector3d & Geometry::getVertices ( ) const

Gets the Vertices of the primitive.

Returns: The mesh resource of geometry.

Definition at line 294 of file geometry.cpp.

 {
     return vertices_;
 }

◆ isMesh()

bool Geometry::isMesh ( ) const

Checks if the geometry is a mesh geometry.

Returns: True if the type_ is a mesh (ShapeType::MESH).

Definition at line 251 of file geometry.cpp.

 {
     return type_ == ShapeType::MESH;
 }

◆ loadBody()

bodies::Body * Geometry::loadBody ( ) const

private

Loads a body from the loaded shape_.

Returns: A pointer to a newly allocated body.

Definition at line 207 of file geometry.cpp.

 {
     switch (type_)
     {
         case ShapeType::BOX:
             return new bodies::Box(shape_.get());
             break;
  
         case ShapeType::SPHERE:
             return new bodies::Sphere(shape_.get());
             break;
  
         case ShapeType::CYLINDER:
             return new bodies::Cylinder(shape_.get());
             break;
  
         case ShapeType::MESH:
             return new bodies::ConvexMesh(shape_.get());
             break;
  
         case ShapeType::CONE:
         default:
             break;
     }
  
     return nullptr;
 }

◆ loadShape()

shapes::Shape * Geometry::loadShape ( ) const

private

Loads a shape from the set type_ and dimensions_, and resource_ if a mesh.

Returns: A pointer to a newly allocated shape.

Definition at line 170 of file geometry.cpp.

 {
     switch (type_)
     {
         case ShapeType::BOX:
             return new shapes::Box(dimensions_[0], dimensions_[1], dimensions_[2]);
             break;
  
         case ShapeType::SPHERE:
             return new shapes::Sphere(dimensions_[0]);
             break;
  
         case ShapeType::CYLINDER:
             return new shapes::Cylinder(dimensions_[0], dimensions_[1]);
             break;
  
         case ShapeType::CONE:
             return new shapes::Cone(dimensions_[0], dimensions_[1]);
             break;
  
         case ShapeType::MESH:
             if (!resource_.empty() && vertices_.empty())
                 return shapes::createMeshFromResource("file://" + resource_, dimensions_);
             else if (resource_.empty() && !vertices_.empty())
                 return shapes::createMeshFromVertices(vertices_);
             else
                 throw Exception(1, resource_.empty() ? "No vertices/resource specified for the mesh" :
                                                        "Both vertices/resource specified for the mesh");
             break;
  
         default:
             break;
     }
  
     return nullptr;
 }

◆ makeBox() [1/2]

GeometryPtr Geometry::makeBox ( const Eigen::Vector3d & dimensions )

static

Create a box.

Parameters

[in] dimensions The XYZ dimensions of the box.

Returns: The created box.

Definition at line 53 of file geometry.cpp.

 {
     return makeBox(dimensions[0], dimensions[1], dimensions[2]);
 }

◆ makeBox() [2/2]

GeometryPtr Geometry::makeBox	(	double	x,
		double	y,
		double	z
	)

static

Create a box.

Parameters

[in]	x	The box's x dimension.
[in]	y	The boy's y dimension.
[in]	z	The boz's z dimension.

Returns: The created box.

Definition at line 48 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::BOX, Eigen::Vector3d(x, y, z));
 }

◆ makeCone()

GeometryPtr Geometry::makeCone	(	double	radius,
		double	length
	)

static

Create a cone.

Parameters

[in]	radius	The radius of the base of the cone.
[in]	length	The height of the cone.

Returns: The created cone.

Definition at line 63 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::CONE, Eigen::Vector3d(radius, length, 0));
 }

◆ makeCylinder()

GeometryPtr Geometry::makeCylinder	(	double	radius,
		double	length
	)

static

Create a cylinder.

Parameters

[in]	radius	The radius of the cylinder.
[in]	length	The length of the cylinder.

Returns: The created cylinder.

Definition at line 58 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::CYLINDER, Eigen::Vector3d(radius, length, 0));
 }

◆ makeMesh() [1/2]

GeometryPtr Geometry::makeMesh ( const EigenSTL::vector_Vector3d & vertices )

static

Create a mesh from triangles represented as vertices .

Parameters

[in] vertices The vertices that will create the mesh.

Returns: The created mesh.

Definition at line 96 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::MESH, Eigen::Vector3d::Ones(), "", vertices);
 }

◆ makeMesh() [2/2]

GeometryPtr Geometry::makeMesh	(	const std::string &	resource,
		const Eigen::Vector3d &	scale = `{1, 1, 1}`
	)

static

Create a mesh from resource file.

Parameters

[in]	resource	The resource to load for the mesh.
[in]	scale	The scale of the mesh.

Returns: The created mesh.

Definition at line 91 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::MESH, scale, resource);
 }

◆ makeSolidPrimitive()

GeometryPtr Geometry::makeSolidPrimitive ( const shape_msgs::SolidPrimitive & msg )

static

Create a solid primitive.

Parameters

[in] msg The solid primitive message to load.

Returns: The created shape.

Definition at line 68 of file geometry.cpp.

 {
     using sm = shape_msgs::SolidPrimitive;
  
     switch (msg.type)
     {
         case sm::BOX:
             return makeBox(msg.dimensions[sm::BOX_X],  //
                            msg.dimensions[sm::BOX_Y],  //
                            msg.dimensions[sm::BOX_Z]);
         case sm::SPHERE:
             return makeSphere(msg.dimensions[sm::SPHERE_RADIUS]);
         case sm::CYLINDER:
             return makeCylinder(msg.dimensions[sm::CYLINDER_RADIUS],  //
                                 msg.dimensions[sm::CYLINDER_HEIGHT]);
         case sm::CONE:
             return makeCone(msg.dimensions[sm::CONE_RADIUS],  //
                             msg.dimensions[sm::CONE_HEIGHT]);
         default:
             return nullptr;
     }
 }

◆ makeSphere()

GeometryPtr Geometry::makeSphere ( double radius )

static

Create a sphere.

Parameters

[in] radius The radius of the sphere.

Returns: The created sphere.

Definition at line 43 of file geometry.cpp.

 {
     return std::make_shared<Geometry>(ShapeType::SPHERE, Eigen::Vector3d(radius, 0, 0));
 }

◆ operator=()

Geometry& robowflex::Geometry::operator= ( const Geometry & )

delete

◆ sample()

std::pair< bool, Eigen::Vector3d > Geometry::sample ( const unsigned int attempts = 50 ) const

Tries to sample a point in the geometry.

Parameters

[in] attempts Number of attempts to sample.

Returns: The sampled point and true, or the 0 vector and false on failure.

Definition at line 240 of file geometry.cpp.

 {
     bool success;
     Eigen::Vector3d point;
     random_numbers::RandomNumberGenerator rng;
  
     if (!(success = body_->samplePointInside(rng, attempts, point)))
         point = Eigen::Vector3d{0, 0, 0};
  
     return std::make_pair(success, point);
 }

Member Data Documentation

◆ body_

bodies::BodyPtr robowflex::Geometry::body_ {nullptr}

private

Body operation.

Definition at line 224 of file geometry.h.

◆ dimensions_

Eigen::Vector3d robowflex::Geometry::dimensions_ {Eigen::Vector3d::Ones()}

private

Dimensions to scale geometry.

Definition at line 220 of file geometry.h.

◆ resource_

std::string robowflex::Geometry::resource_ {""}

private

Resource locator for MESH types.

Definition at line 222 of file geometry.h.

◆ shape_

shapes::ShapePtr robowflex::Geometry::shape_ {nullptr}

private

Loaded shape.

Definition at line 223 of file geometry.h.

◆ type_

ShapeType::Type robowflex::Geometry::type_ {ShapeType::Type::BOX}

private

Geometry Type.

Definition at line 219 of file geometry.h.

◆ vertices_

EigenSTL::vector_Vector3d robowflex::Geometry::vertices_ {{}}

private

Vertices of the primitive.

Definition at line 221 of file geometry.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Geometry() [1/5]

◆ Geometry() [2/5]

◆ Geometry() [3/5]

◆ Geometry() [4/5]

◆ Geometry() [5/5]

Member Function Documentation

◆ contains()

◆ getAABB()

◆ getBody()

◆ getDimensions()

◆ getMeshMsg()

◆ getResource()

◆ getShape()

◆ getSolidMsg()

◆ getType()

◆ getVertices()

◆ isMesh()

◆ loadBody()

◆ loadShape()

◆ makeBox() [1/2]

◆ makeBox() [2/2]

◆ makeCone()

◆ makeCylinder()

◆ makeMesh() [1/2]

◆ makeMesh() [2/2]

◆ makeSolidPrimitive()

◆ makeSphere()

◆ operator=()

◆ sample()

Member Data Documentation

◆ body_

◆ dimensions_

◆ resource_

◆ shape_

◆ type_

◆ vertices_