collision_core_unit.cpp File Reference

#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <vector>
#include <string>
#include <tesseract_collision/core/common.h>
#include <tesseract_common/utils.h>

Include dependency graph for collision_core_unit.cpp:

Functions
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("link_1", "link_2"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("link_1", "link_3"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("link_2", "link_3"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("base_link", "link_1"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("base_link", "link_2"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("base_link", "link_3"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("part_link", "link_1"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("part_link", "link_2"))
check_pairs	push_back (tesseract_common::makeOrderedLinkPair("part_link", "link_3"))
	EXPECT_TRUE (tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >(pairs, check_pairs, false))
check_pairs	clear ()
	EXPECT_TRUE (tesseract_collision::isContactAllowed("base_link", "base_link", acm, false))
	EXPECT_FALSE (tesseract_collision::isContactAllowed("base_link", "link_2", acm, false))
	EXPECT_TRUE (tesseract_collision::isContactAllowed("base_link", "link_1", acm, true))
base_vertices	push_back (Eigen::Vector3d(0, 0, 0))
base_vertices	push_back (Eigen::Vector3d(0, 0, 1))
base_vertices	push_back (Eigen::Vector3d(0, 1, 1))
base_vertices	push_back (Eigen::Vector3d(0, 1, 0))
base_vertices	push_back (Eigen::Vector3d(1, 0, 0))
base_vertices	push_back (Eigen::Vector3d(1, 0, 1))
base_vertices	push_back (Eigen::Vector3d(1, 1, 1))
base_vertices	push_back (Eigen::Vector3d(1, 1, 0))
	for (std::size_t i=0;i< 8;++i)
	EXPECT_TRUE (test_vertices[0].isApprox(Eigen::Vector3d(-4.5, -4.5, -4.5)))
	EXPECT_TRUE (test_vertices[1].isApprox(Eigen::Vector3d(-4.5, -4.5, 5.5)))
	EXPECT_TRUE (test_vertices[2].isApprox(Eigen::Vector3d(-4.5, 5.5, 5.5)))
	EXPECT_TRUE (test_vertices[3].isApprox(Eigen::Vector3d(-4.5, 5.5, -4.5)))
	EXPECT_TRUE (test_vertices[4].isApprox(Eigen::Vector3d(5.5, -4.5, -4.5)))
	EXPECT_TRUE (test_vertices[5].isApprox(Eigen::Vector3d(5.5, -4.5, 5.5)))
	EXPECT_TRUE (test_vertices[6].isApprox(Eigen::Vector3d(5.5, 5.5, 5.5)))
	EXPECT_TRUE (test_vertices[7].isApprox(Eigen::Vector3d(5.5, 5.5, -4.5)))
	EXPECT_TRUE (test_vertices[0].isApprox(Eigen::Vector3d(0, 0, 0)))
	EXPECT_TRUE (test_vertices[1].isApprox(Eigen::Vector3d(0, 0, 10)))
	EXPECT_TRUE (test_vertices[2].isApprox(Eigen::Vector3d(0, 10, 10)))
	EXPECT_TRUE (test_vertices[3].isApprox(Eigen::Vector3d(0, 10, 0)))
	EXPECT_TRUE (test_vertices[4].isApprox(Eigen::Vector3d(10, 0, 0)))
	EXPECT_TRUE (test_vertices[5].isApprox(Eigen::Vector3d(10, 0, 10)))
	EXPECT_TRUE (test_vertices[6].isApprox(Eigen::Vector3d(10, 10, 10)))
	EXPECT_TRUE (test_vertices[7].isApprox(Eigen::Vector3d(10, 10, 0)))
	EXPECT_EQ (results.distance, std::numeric_limits< double >::max())
	EXPECT_TRUE (results.nearest_points[0].isApprox(Eigen::Vector3d::Zero()))
	EXPECT_TRUE (results.nearest_points[1].isApprox(Eigen::Vector3d::Zero()))
	EXPECT_TRUE (results.nearest_points_local[0].isApprox(Eigen::Vector3d::Zero()))
	EXPECT_TRUE (results.nearest_points_local[1].isApprox(Eigen::Vector3d::Zero()))
	EXPECT_TRUE (results.transform[0].isApprox(Eigen::Isometry3d::Identity()))
	EXPECT_TRUE (results.transform[1].isApprox(Eigen::Isometry3d::Identity()))
	EXPECT_TRUE (results.link_names[0].empty())
	EXPECT_TRUE (results.link_names[1].empty())
	EXPECT_EQ (results.shape_id[0], -1)
	EXPECT_EQ (results.shape_id[1], -1)
	EXPECT_EQ (results.type_id[0], 0)
	EXPECT_EQ (results.type_id[1], 0)
	EXPECT_TRUE (results.normal.isApprox(Eigen::Vector3d::Zero()))
	EXPECT_EQ (results.cc_type[0], tesseract_collision::ContinuousCollisionType::CCType_None)
	EXPECT_EQ (results.cc_type[1], tesseract_collision::ContinuousCollisionType::CCType_None)
	EXPECT_TRUE (results.cc_transform[0].isApprox(Eigen::Isometry3d::Identity()))
	EXPECT_TRUE (results.cc_transform[1].isApprox(Eigen::Isometry3d::Identity()))
	EXPECT_EQ (results.single_contact_point, false)
result_map	addContactResult (key1, tesseract_collision::ContactResult{})
	EXPECT_EQ (result_map.count(), 1)
	EXPECT_EQ (result_map.getContainer().size(), 1)
	EXPECT_TRUE (it !=result_map.end())
	EXPECT_EQ (it->second.size(), 1)
	EXPECT_EQ (result_map.count(), 2)
	EXPECT_EQ (it->second.size(), 2)
result_map	addContactResult (key2, tesseract_collision::ContactResult{})
	EXPECT_EQ (result_map.count(), 3)
	EXPECT_EQ (result_map.getContainer().size(), 2)
	EXPECT_EQ (result_map.count(), 0)
	EXPECT_EQ (it->second.size(), 0)
	EXPECT_TRUE (it->second.capacity() > 0)
result_map	release ()
	EXPECT_TRUE (result_map.getContainer().empty())
result_map	addContactResult (key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} })
	EXPECT_EQ (result_map.count(), 4)
	EXPECT_EQ (it->second.size(), 4)
result_map	addContactResult (key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} })
	EXPECT_EQ (result_map.count(), 6)
result_map	setContactResult (key1, tesseract_collision::ContactResult{})
result_map	setContactResult (key2, tesseract_collision::ContactResult{})
result_map	setContactResult (key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} })
result_map	setContactResult (key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} })
result_map	flattenMoveResults (result_vector)
	EXPECT_EQ (result_vector.size(), 3)
result_map	flattenCopyResults (result_vector)
result_map	flattenWrapperResults (result_vector)
result_map	filter (filter)
	EXPECT_NEAR (config.contact_manager_config.margin_data.getDefaultCollisionMargin(), 5, 1e-6)
	EXPECT_EQ (config.type, tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)
	EXPECT_NEAR (config.longest_valid_segment_length, 0.5, 1e-6)
int	main (int argc, char **argv)

Variables
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH	TESSERACT_COMMON_IGNORE_WARNINGS_POP
std::vector< std::string >	static_links { "base_link", "part_link" }
std::vector< tesseract_collision::ObjectPairKey >	check_pairs
std::vector< tesseract_collision::ObjectPairKey >	pairs
auto	acm
tesseract_common::VectorVector3d	test_vertices { base_vertices }
results	distance = 10
results	nearest_points [0] = Eigen::Vector3d(1, 2, 3)
results	nearest_points_local [0] = Eigen::Vector3d(1, 2, 3)
results	transform [0] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3)
results	link_names [0] = "notempty"
results	shape_id [0] = 5
results	subshape_id [0] = 10
results	type_id [0] = 3
results	normal = Eigen::Vector3d(1, 2, 3)
results	cc_time [0] = 7
results	cc_type [0] = tesseract_collision::ContinuousCollisionType::CCType_Between
results	cc_transform [0] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3)
results	single_contact_point = true
auto	key2 = tesseract_common::makeOrderedLinkPair("link2", "link3")
auto	it = result_map.find(key1)
tesseract_collision::ContactResultVector	result_vector
auto	filter
tesseract_collision::CollisionCheckConfig	config (5, request, tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE, 0.5)

Function Documentation

addContactResult() [1/4]

result_map addContactResult	(	key1	,
		tesseract_collision::ContactResult{}
	)

addContactResult() [2/4]

result_map addContactResult	(	key1	,
		{ tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }
	)

Initial value:

{  
    tesseract_collision::ContactResultMap result_map

addContactResult() [3/4]

result_map addContactResult	(	key2	,
		tesseract_collision::ContactResult{}
	)

addContactResult() [4/4]

result_map addContactResult	(	key2	,
		{ tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }
	)

clear()

l clear

(

)

Initial value:

{
      manager->applyContactManagerConfig(config.contact_manager_config)

EXPECT_EQ() [1/22]

EXPECT_EQ	(	config.	type,
		tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE
	)

EXPECT_EQ() [2/22]

EXPECT_EQ	(	it->second.	size(),
		0
	)

EXPECT_EQ() [3/22]

EXPECT_EQ	(	it->second.	size(),
		1
	)

EXPECT_EQ() [4/22]

EXPECT_EQ	(	it->second.	size(),
		2
	)

EXPECT_EQ() [5/22]

EXPECT_EQ	(	it->second.	size(),
		4
	)

EXPECT_EQ() [6/22]

EXPECT_EQ	(	result_map.	count(),
		0
	)

EXPECT_EQ() [7/22]

EXPECT_EQ	(	result_map.	count(),
		1
	)

EXPECT_EQ() [8/22]

EXPECT_EQ	(	result_map.	count(),
		2
	)

EXPECT_EQ() [9/22]

EXPECT_EQ	(	result_map.	count(),
		3
	)

EXPECT_EQ() [10/22]

EXPECT_EQ	(	result_map.	count(),
		4
	)

EXPECT_EQ() [11/22]

EXPECT_EQ	(	result_map.	count(),
		6
	)

EXPECT_EQ() [12/22]

EXPECT_EQ	(	result_map.	getContainer).size(,
		1
	)

EXPECT_EQ() [13/22]

EXPECT_EQ	(	result_map.	getContainer).size(,
		2
	)

EXPECT_EQ() [14/22]

EXPECT_EQ	(	result_vector.	size(),
		3
	)

EXPECT_EQ() [15/22]

EXPECT_EQ	(	results.	cc_type[0],
		tesseract_collision::ContinuousCollisionType::CCType_None
	)

EXPECT_EQ() [16/22]

EXPECT_EQ	(	results.	cc_type[1],
		tesseract_collision::ContinuousCollisionType::CCType_None
	)

EXPECT_EQ() [17/22]

EXPECT_EQ	(	results.	distance,
		std::numeric_limits< double >	::max()
	)

EXPECT_EQ() [18/22]

EXPECT_EQ	(	results.	shape_id[0],
		-	1
	)

EXPECT_EQ() [19/22]

EXPECT_EQ	(	results.	shape_id[1],
		-	1
	)

EXPECT_EQ() [20/22]

EXPECT_EQ	(	results.	single_contact_point,
		false
	)

EXPECT_EQ() [21/22]

EXPECT_EQ	(	results.	type_id[0],
		0
	)

EXPECT_EQ() [22/22]

EXPECT_EQ	(	results.	type_id[1],
		0
	)

EXPECT_FALSE()

EXPECT_FALSE

(

tesseract_collision::isContactAllowed("base_link", "link_2", acm, false)

)

EXPECT_NEAR() [1/2]

EXPECT_NEAR	(	config.contact_manager_config.margin_data.	getDefaultCollisionMargin(),
		5	,
		1e-	6
	)

EXPECT_NEAR() [2/2]

EXPECT_NEAR	(	config.	longest_valid_segment_length,
		0.	5,
		1e-	6
	)

EXPECT_TRUE() [1/33]

EXPECT_TRUE

(

it !

= result_map.end()

)

EXPECT_TRUE() [2/33]

EXPECT_TRUE	(	it->second.	capacity(),
		0
	)

EXPECT_TRUE() [3/33]

EXPECT_TRUE

(

result_map.

getContainer).empty(

)

EXPECT_TRUE() [4/33]

EXPECT_TRUE

(

results.cc_transform.

isApprox(Eigen::Isometry3d::Identity())[0]

)

EXPECT_TRUE() [5/33]

EXPECT_TRUE

(

results.cc_transform.

isApprox(Eigen::Isometry3d::Identity())[1]

)

EXPECT_TRUE() [6/33]

EXPECT_TRUE

(

results.link_names.

empty()[0]

)

EXPECT_TRUE() [7/33]

EXPECT_TRUE

(

results.link_names.

empty()[1]

)

EXPECT_TRUE() [8/33]

EXPECT_TRUE

(

results.nearest_points.

isApprox(Eigen::Vector3d::Zero())[0]

)

EXPECT_TRUE() [9/33]

EXPECT_TRUE

(

results.nearest_points.

isApprox(Eigen::Vector3d::Zero())[1]

)

EXPECT_TRUE() [10/33]

EXPECT_TRUE

(

results.nearest_points_local.

isApprox(Eigen::Vector3d::Zero())[0]

)

EXPECT_TRUE() [11/33]

EXPECT_TRUE

(

results.nearest_points_local.

isApprox(Eigen::Vector3d::Zero())[1]

)

EXPECT_TRUE() [12/33]

EXPECT_TRUE

(

results.normal.

isApproxEigen::Vector3d::Zero()

)

EXPECT_TRUE() [13/33]

EXPECT_TRUE

(

results.transform.

isApprox(Eigen::Isometry3d::Identity())[0]

)

EXPECT_TRUE() [14/33]

EXPECT_TRUE

(

results.transform.

isApprox(Eigen::Isometry3d::Identity())[1]

)

EXPECT_TRUE() [15/33]

EXPECT_TRUE

(

tesseract_collision::isContactAllowed("base_link", "base_link", acm, false)

)

EXPECT_TRUE() [16/33]

EXPECT_TRUE

(

tesseract_collision::isContactAllowed("base_link", "link_1", acm, true)

)

EXPECT_TRUE() [17/33]

EXPECT_TRUE

(

tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >

pairs, check_pairs, false

)

EXPECT_TRUE() [18/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(-4.5, -4.5, -4.5))[0]

)

EXPECT_TRUE() [19/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(0, 0, 0))[0]

)

EXPECT_TRUE() [20/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(-4.5, -4.5, 5.5))[1]

)

EXPECT_TRUE() [21/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(0, 0, 10))[1]

)

EXPECT_TRUE() [22/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(-4.5, 5.5, 5.5))[2]

)

EXPECT_TRUE() [23/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(0, 10, 10))[2]

)

EXPECT_TRUE() [24/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(-4.5, 5.5, -4.5))[3]

)

EXPECT_TRUE() [25/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(0, 10, 0))[3]

)

EXPECT_TRUE() [26/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(10, 0, 0))[4]

)

EXPECT_TRUE() [27/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(5.5, -4.5, -4.5))[4]

)

EXPECT_TRUE() [28/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(10, 0, 10))[5]

)

EXPECT_TRUE() [29/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(5.5, -4.5, 5.5))[5]

)

EXPECT_TRUE() [30/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(10, 10, 10))[6]

)

EXPECT_TRUE() [31/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(5.5, 5.5, 5.5))[6]

)

EXPECT_TRUE() [32/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(10, 10, 0))[7]

)

EXPECT_TRUE() [33/33]

EXPECT_TRUE

(

test_vertices.

isApprox(Eigen::Vector3d(5.5, 5.5, -4.5))[7]

)

filter()

result_map filter

(

filter

)

flattenCopyResults()

result_map flattenCopyResults

(

result_vector

)

flattenMoveResults()

result_map flattenMoveResults

(

result_vector

)

flattenWrapperResults()

result_map flattenWrapperResults

(

result_vector

)

for()

for

(

)

Initial value:

{
  std::vector<uint8_t> data

main()

int main	(	int	argc,
		char **	argv
	)

push_back() [1/17]

base_vertices push_back

(

Eigen::Vector3d(0, 0, 0)

)

push_back() [2/17]

base_vertices push_back

(

Eigen::Vector3d(0, 0, 1)

)

push_back() [3/17]

base_vertices push_back

(

Eigen::Vector3d(0, 1, 0)

)

push_back() [4/17]

base_vertices push_back

(

Eigen::Vector3d(0, 1, 1)

)

push_back() [5/17]

base_vertices push_back

(

Eigen::Vector3d(1, 0, 0)

)

push_back() [6/17]

base_vertices push_back

(

Eigen::Vector3d(1, 0, 1)

)

push_back() [7/17]

base_vertices push_back

(

Eigen::Vector3d(1, 1, 0)

)

push_back() [8/17]

base_vertices push_back

(

Eigen::Vector3d(1, 1, 1)

)

push_back() [9/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("base_link", "link_1")

)

push_back() [10/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("base_link", "link_2")

)

push_back() [11/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("base_link", "link_3")

)

push_back() [12/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("link_1", "link_2")

)

push_back() [13/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("link_1", "link_3")

)

push_back() [14/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("link_2", "link_3")

)

push_back() [15/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("part_link", "link_1")

)

push_back() [16/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("part_link", "link_2")

)

push_back() [17/17]

check_pairs push_back

(

tesseract_common::makeOrderedLinkPair("part_link", "link_3")

)

release()

result_map release

(

)

setContactResult() [1/4]

result_map setContactResult	(	key1	,
		tesseract_collision::ContactResult{}
	)

setContactResult() [2/4]

result_map setContactResult	(	key1	,
		{ tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }
	)

Initial value:

{  
    tesseract_collision::ContactResultMap result_map

setContactResult() [3/4]

result_map setContactResult	(	key2	,
		tesseract_collision::ContactResult{}
	)

setContactResult() [4/4]

result_map setContactResult	(	key2	,
		{ tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }
	)

Variable Documentation

acm

tesseract_common::AllowedCollisionMatrix acm

Initial value:

= [](const std::string& s1, const std::string& s2) {
    return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") ==
            tesseract_common::makeOrderedLinkPair(s1, s2));
  }

cc_time

results cc_time = 7

cc_transform

results cc_transform = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3)

cc_type

results cc_type = tesseract_collision::ContinuousCollisionType::CCType_Between

check_pairs

std::vector<tesseract_collision::ObjectPairKey> check_pairs

config

plugin config	(	5	,
		request	,
		tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE	,
		0.	5
	)

Initial value:

{
  tesseract_collision::ContactRequest request

distance

results distance = 10

filter

auto filter

Initial value:

= [key1](tesseract_collision::ContactResultMap::PairType& pair) {
      if (key1 == pair.first)
        pair.second.clear();
    }

it

auto it = result_map.find(key1)

key2

auto key2 = tesseract_common::makeOrderedLinkPair("link2", "link3")

link_names

results link_names = "notempty"

nearest_points

results nearest_points = Eigen::Vector3d(1, 2, 3)

nearest_points_local

results nearest_points_local = Eigen::Vector3d(1, 2, 3)

normal

results normal = Eigen::Vector3d(1, 2, 3)

pairs

pairs

Initial value:

=
      tesseract_collision::getCollisionObjectPairs(active_links, static_links)

result_vector

std::vector< std::reference_wrapper< const tesseract_collision::ContactResult > > result_vector

shape_id

results shape_id = 5

single_contact_point

results single_contact_point = true

static_links

std::vector<std::string> static_links { "base_link", "part_link" }

subshape_id

results subshape_id = 10

TESSERACT_COMMON_IGNORE_WARNINGS_POP

TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP

Initial value:

{
  std::vector<std::string> active_links{ "link_1", "link_2", "link_3" }

test_vertices

test_vertices { base_vertices }

transform

results transform = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3)

type_id

results type_id = 3