Tesseract 0.28.4
Loading...
Searching...
No Matches
collision_signed_distance_field_unit.cpp File Reference

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

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)
checker addCollisionObject ("sdf_link", 0, CollisionShapesConst{ sdf }, poses, true)
checker addCollisionObject ("sphere_link", 0, CollisionShapesConst{ std::make_shared< tesseract::geometry::Sphere >(0.1) }, poses, true)
checker setActiveCollisionObjects ({ "sdf_link", "sphere_link" })
checker setDefaultCollisionMargin (0.0)
sphere_tf translation ().x()=0.45
checker setCollisionObjectsTransform ("sphere_link", sphere_tf)
checker contactTest (result, ContactRequest(ContactTestType::CLOSEST))
result flattenMoveResults (result_vector)
 ASSERT_EQ (result_vector.size(), 1)
 EXPECT_NEAR (result_vector[0].distance, -0.15, 1e-3)
 EXPECT_FALSE (sdf->isDiscretized())
 EXPECT_NEAR (result_vector[0].distance, -0.15, 2e-2)
 EXPECT_EQ (calls, sampled)
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")
 runSDFTest (checker, makeSparseSphereSDF)
 runSDFPositiveDistanceTest (checker, makeSparseSphereSDF)
void runSDFBandClampTest (DiscreteContactManager &checker)
 A narrow band says nothing past its band clamp. With the clamp a margin reaching past the band still measures the true separation, to the surface; without it the saturated values read as a flat distance with no direction.
 runSDFBandClampTest (checker)
 runSDFPairTest (checker, make_sdf, make_sdf)
 runSDFPairTest (checker, [] { return sparseSphereSDF(32);}, fineSphereSDF)
 runScaledSDFTest (checker, false)
 runScaledSDFTest (checker, true)
 for (const auto &geom :{ field, dense })
 EXPECT_THROW (TesseractSdfCollisionShape(nullptr), std::invalid_argument)
 EXPECT_EQ (shape->getShapeType(), SDF_SHAPE_PROXYTYPE)
 EXPECT_STREQ (shape->getName(), "TesseractSdfCollisionShape")
 EXPECT_NO_THROW (shape->setLocalScaling(btVector3(1, 1, 1)))
 EXPECT_THROW (shape->setLocalScaling(btVector3(2, 2, 2)), std::invalid_argument)
 EXPECT_EQ (shape->getLocalScaling(), btVector3(1, 1, 1))
btVector3 inertia (1, 1, 1)
shape calculateLocalInertia (1, inertia)
 EXPECT_EQ (inertia, btVector3(0, 0, 0))
shape processAllTriangles (nullptr, btVector3(-1, -1, -1), btVector3(1, 1, 1))
const TesseractSdfCollisionShape nan_shape (tesseract::geometry::createSignedDistanceField(nan_field, Eigen::Vector3d(-1, -1, -1), Eigen::Vector3d(1, 1, 1), Eigen::Vector3i(4, 4, 4)))
 EXPECT_FALSE (nan_shape.queryDistance(btVector3(0, 0, 0), distance))
 EXPECT_FALSE (nan_shape.queryPoint(btVector3(0, 0, 0), distance, normal))
bool querySdfShapeContact (const btCollisionShape &shape, const btVector3 &local_point, btScalar max_distance, btScalar &distance, btVector3 &normal)
 Distance and normal at a point in an SDF shape's frame, but only if the distance is within max_distance (a contact threshold): the distance is read first and the normal only for points that pass. shape must be a TesseractSdfCollisionShape.
bool querySdfShapePoint (const btCollisionShape &shape, const btVector3 &local_point, btScalar &distance, btVector3 &normal)
 Distance and normal at a point in an SDF shape's frame. shape must be a TesseractSdfCollisionShape.

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::LinkIdTransformMap location = Eigen::Isometry3d::Identity()
ContactResultMap result
auto sdf
BulletDiscreteBVHManager checker
Eigen::Isometry3d sphere_tf = Eigen::Isometry3d::Identity()
ContactResultVector result_vector
const tesseract::geometry::SignedDistanceFunction sphere
const std::size_t sampled = calls
const auto make_sdf = [] { return sparseSphereSDF(32); }
auto field = std::static_pointer_cast<const tesseract::geometry::SignedDistanceField>(makeSparseSphereSDF())
auto dense = std::static_pointer_cast<const tesseract::geometry::SignedDistanceField>(makeSphereSDF())
auto geom = std::static_pointer_cast<const tesseract::geometry::SignedDistanceField>(makeSphereSDF())
auto shape = std::make_unique<TesseractSdfCollisionShape>(geom)
const tesseract::geometry::SignedDistanceFunction nan_field
btScalar distance { 0 }
btVector3 normal

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 and added to the Bullet and FCL backends, which both query it in place through the shared implicit solver. A probe is then placed at known locations and the reported contact distance is validated.

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() [1/2]

runSDFTest ( checker )
Initial value:
{
A BVH implementation of a bullet manager.
Definition bullet_discrete_bvh_manager.h:55

◆ runSDFPositiveDistanceTest() [1/2]

runSDFPositiveDistanceTest ( checker )
Initial value:

◆ runSDFPairTest() [1/2]

runSDFPairTest ( checker )
Initial value:
{
A simple implementation of a bullet manager which does not use BHV.
Definition bullet_discrete_simple_manager.h:55

◆ runSDFCapsuleTest()

runSDFCapsuleTest ( checker )
Initial value:

◆ runSDFBoxTest()

runSDFBoxTest ( checker )
Initial value:

◆ runSDFCurvedPrimitiveTest() [1/2]

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

◆ runSDFCurvedPrimitiveTest() [2/2]

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

◆ runSDFTest() [2/2]

runSDFTest ( checker ,
makeSparseSphereSDF  )
Initial value:

◆ runSDFPositiveDistanceTest() [2/2]

runSDFPositiveDistanceTest ( checker ,
makeSparseSphereSDF  )
Initial value:
{
A FCL implementation of the discrete contact manager.
Definition fcl_discrete_managers.h:55

◆ runSDFBandClampTest()

runSDFBandClampTest ( checker )
Initial value:

◆ runSDFPairTest() [2/2]

runSDFPairTest ( checker ,
[] { return sparseSphereSDF(32);} ,
fineSphereSDF  )
Initial value:

◆ runScaledSDFTest() [1/2]

runScaledSDFTest ( checker ,
false  )
Initial value:

◆ runScaledSDFTest() [2/2]

runScaledSDFTest ( checker ,
true  )
Initial value:

◆ querySdfShapeContact()

bool tesseract::collision::bullet_internal::querySdfShapeContact ( const btCollisionShape & shape,
const btVector3 & local_point,
btScalar max_distance,
btScalar & distance,
btVector3 & normal )

Distance and normal at a point in an SDF shape's frame, but only if the distance is within max_distance (a contact threshold): the distance is read first and the normal only for points that pass. shape must be a TesseractSdfCollisionShape.

Returns
true if the shape has a value there and it is within max_distance

◆ querySdfShapePoint()

bool tesseract::collision::bullet_internal::querySdfShapePoint ( const btCollisionShape & shape,
const btVector3 & local_point,
btScalar & distance,
btVector3 & normal )

Distance and normal at a point in an SDF shape's frame. shape must be a TesseractSdfCollisionShape.

Returns
false where the shape has no value

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

◆ sphere

Initial value:
= [&calls](const Eigen::Vector3d& p) {
++calls;
return p.norm() - 0.5;
}

◆ nan_field

Initial value:
= [](const Eigen::Vector3d&) {
return std::numeric_limits<double>::quiet_NaN();
}