collision_signed_distance_field_unit.cpp File Reference

Discrete collision tests for the volumetric SignedDistanceField geometry against a sphere. More...

#include <tesseract/common/macros.h>
#include <gtest/gtest.h>
#include <cmath>
#include <cstdint>
#include <memory>
#include <stdexcept>
#include <vector>
#include <tesseract/collision/bullet/bullet_discrete_simple_manager.h>
#include <tesseract/collision/bullet/bullet_discrete_bvh_manager.h>
#include <tesseract/collision/bullet/bullet_cast_bvh_manager.h>
#include <tesseract/collision/bullet/bullet_utils.h>
#include <tesseract/collision/bullet/bullet_collision_shape_cache.h>
#include <tesseract/collision/fcl/fcl_discrete_managers.h>
#include <tesseract/collision/common.h>
#include <tesseract/geometry/geometries.h>
#include <tesseract/geometry/impl/signed_distance_field_utils.h>

Functions
	runSDFTest (checker)
	runSDFPositiveDistanceTest (checker)
	runSDFPairTest (checker)
checker	addCollisionObject ("sdf_link", 0, sdf_shapes, poses, true)
checker	addCollisionObject ("plane_link", 0, plane_shapes, poses, true)
checker	setActiveCollisionObjects ({ "sdf_link", "plane_link" })
checker	setCollisionObjectsTransform (location)
	EXPECT_THROW (checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)), std::runtime_error)
const Eigen::AlignedBox3d	domain (Eigen::Vector3d::Constant(-1.0), Eigen::Vector3d::Constant(1.0))
	EXPECT_THROW (checker.addCollisionObject("sdf_link", 0, sdf_shapes, poses, true), std::invalid_argument)
void	addCapsuleObjects (DiscreteContactManager &checker)
void	runSDFCapsuleTest (DiscreteContactManager &checker)
void	runSDFBoxTest (DiscreteContactManager &checker)
	runSDFCapsuleTest (checker)
	runSDFBoxTest (checker)
void	runSDFCurvedPrimitiveTest (DiscreteContactManager &checker, const CollisionShapeConstPtr &primitive, const std::string &primitive_link)
	runSDFCurvedPrimitiveTest (checker, std::make_shared< tesseract::geometry::Cylinder >(0.1, 0.2), "cylinder_link")
	runSDFCurvedPrimitiveTest (checker, std::make_shared< tesseract::geometry::Cone >(0.1, 0.2), "cone_link")
	EXPECT_ANY_THROW (checker.addCollisionObject("sdf_link", 0, sdf_shapes, sdf_poses, true))
	EXPECT_EQ (createShapePrimitive(geom), nullptr)
	ASSERT_NE (shape, nullptr)
	ASSERT_NE (shape->top_level, nullptr)
int	main (int argc, char **argv)

Variables
CollisionShapesConst	sdf_shapes { makeSphereSDF() }
CollisionShapesConst	plane_shapes { std::make_shared<tesseract::geometry::Plane>(0.0, 0.0, 1.0, 0.0) }
tesseract::common::VectorIsometry3d	poses { Eigen::Isometry3d::Identity() }
tesseract::common::TransformMap	location = Eigen::Isometry3d::Identity()
ContactResultMap	result
auto	sdf
tesseract::common::VectorIsometry3d	sdf_poses { Eigen::Isometry3d::Identity() }
auto	geom
auto	shape = createShapePrimitive(geom)

Detailed Description

Discrete collision tests for the volumetric SignedDistanceField geometry against a sphere.

A signed distance field for a sphere of radius R centered at the origin is discretized at runtime into the Discregrid / btMiniSDF binary blob format and fed to the Bullet backend. A probe sphere is then placed at known locations and the reported contact distance is validated. This exercises both the SDF data-ingestion path (initializeSDF) and the convex-vs-SDF narrowphase.

License

Software License Agreement (Apache License)

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

Function Documentation

runSDFTest()

runSDFTest

(

checker

)

Initial value:

{
  BulletDiscreteBVHManager checker

runSDFPositiveDistanceTest()

runSDFPositiveDistanceTest

(

checker

)

Initial value:

{
  BulletDiscreteBVHManager checker

runSDFPairTest()

runSDFPairTest

(

checker

)

Initial value:

{
  BulletDiscreteSimpleManager checker

runSDFCapsuleTest()

runSDFCapsuleTest

(

checker

)

Initial value:

{
  BulletDiscreteBVHManager checker

runSDFBoxTest()

runSDFBoxTest

(

checker

)

Initial value:

{
  BulletDiscreteBVHManager checker

runSDFCurvedPrimitiveTest() [1/2]

runSDFCurvedPrimitiveTest	(	checker	,
		std::make_shared< tesseract::geometry::Cylinder >	0.1, 0.2,
		"cylinder_link"
	)

Initial value:

{
  BulletDiscreteBVHManager checker

runSDFCurvedPrimitiveTest() [2/2]

runSDFCurvedPrimitiveTest	(	checker	,
		std::make_shared< tesseract::geometry::Cone >	0.1, 0.2,
		"cone_link"
	)

Initial value:

{
  BulletDiscreteBVHManager checker

Variable Documentation

sdf

auto sdf

Initial value:

= std::make_shared<tesseract::geometry::SignedDistanceField>(
      domain, Eigen::Vector3i(2, 2, 2), std::vector<double>(8, 0.0), Eigen::Vector3d(1.0, 2.0, 1.0))

geom

tesseract::geometry::SignedDistanceField::Ptr geom

Initial value:

= tesseract::geometry::createDiscreteSignedDistanceField(sphere,
                                                                       Eigen::Vector3d(-1, -1, -1),
                                                                       Eigen::Vector3d(1, 1, 1),
                                                                       Eigen::Vector3i(8, 8, 8),
                                                                       Eigen::Vector3d(1, 1, 1))