tesseract_srdf_unit.cpp File Reference

#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <iostream>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <tesseract_common/utils.h>
#include <tesseract_common/resource_locator.h>
#include <tesseract_common/yaml_utils.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_srdf/collision_margins.h>
#include <tesseract_srdf/configs.h>
#include <tesseract_srdf/disabled_collisions.h>
#include <tesseract_srdf/group_states.h>
#include <tesseract_srdf/group_tool_center_points.h>
#include <tesseract_srdf/groups.h>
#include <tesseract_srdf/srdf_model.h>
#include <tesseract_srdf/utils.h>
#include <tesseract_support/tesseract_support_resource_locator.h>

Include dependency graph for tesseract_srdf_unit.cpp:

Classes
class	TempResourceLocator

Enumerations
enum class	ABBConfig { ROBOT_ONLY , ROBOT_ON_RAIL , ROBOT_WITH_POSITIONER }

Functions
tesseract_scene_graph::SceneGraph::Ptr	getABBSceneGraph (ABBConfig config=ABBConfig::ROBOT_ONLY)
tesseract_scene_graph::SceneGraph	buildTestSceneGraph ()
g	setName ("kuka_lbr_iiwa_14_r820")
Link	base_link ("base_link")
Link	link_1 ("link_1")
Link	link_2 ("link_2")
Link	link_3 ("link_3")
Link	link_4 ("link_4")
Link	link_5 ("link_5")
Link	link_6 ("link_6")
Link	link_7 ("link_7")
Link	tool0 ("tool0")
	EXPECT_TRUE (g.addLink(base_link))
	EXPECT_TRUE (g.addLink(link_1))
	EXPECT_TRUE (g.addLink(link_2))
	EXPECT_TRUE (g.addLink(link_3))
	EXPECT_TRUE (g.addLink(link_4))
	EXPECT_TRUE (g.addLink(link_5))
	EXPECT_TRUE (g.addLink(link_6))
	EXPECT_TRUE (g.addLink(link_7))
	EXPECT_TRUE (g.addLink(tool0))
Joint	joint_1 ("joint_a1")
	EXPECT_TRUE (g.addJoint(joint_1))
Joint	joint_2 ("joint_a2")
	EXPECT_TRUE (g.addJoint(joint_2))
Joint	joint_3 ("joint_a3")
joint_3 parent_to_joint_origin_transform	translation ()(0)
	EXPECT_TRUE (g.addJoint(joint_3))
Joint	joint_4 ("joint_a4")
	EXPECT_TRUE (g.addJoint(joint_4))
Joint	joint_5 ("joint_a5")
	EXPECT_TRUE (g.addJoint(joint_5))
Joint	joint_6 ("joint_a6")
	EXPECT_TRUE (g.addJoint(joint_6))
Joint	joint_7 ("joint_a7")
	EXPECT_TRUE (g.addJoint(joint_7))
Joint	joint_tool0 ("joint_tool0")
	EXPECT_TRUE (g.addJoint(joint_tool0))
srdf	initFile (g, srdf_file, locator)
	EXPECT_EQ (srdf.name, "kuka_lbr_iiwa_14_r820")
	EXPECT_EQ (srdf.version[0], 1)
	EXPECT_EQ (srdf.version[1], 0)
	EXPECT_EQ (srdf.version[2], 0)
	processSRDFAllowedCollisions (g, srdf)
	EXPECT_TRUE (acm->isCollisionAllowed("link_1", "link_2"))
	EXPECT_FALSE (acm->isCollisionAllowed("base_link", "link_5"))
g	removeAllowedCollision ("link_1", "link_2")
	EXPECT_FALSE (acm->isCollisionAllowed("link_1", "link_2"))
g	clearAllowedCollisions ()
	EXPECT_EQ (acm->getAllAllowedCollisions().size(), 0)
	EXPECT_TRUE (srdf.name=="test_srdf")
	EXPECT_TRUE (chain_groups.empty())
	EXPECT_FALSE (srdf.kinematics_information.chain_groups.empty())
	EXPECT_TRUE (joint_groups.empty())
	EXPECT_FALSE (srdf.kinematics_information.joint_groups.empty())
	EXPECT_TRUE (link_groups.empty())
	EXPECT_FALSE (srdf.kinematics_information.link_groups.empty())
	EXPECT_TRUE (group_state.empty())
	EXPECT_EQ (srdf.kinematics_information.group_states.size(), 3)
	EXPECT_TRUE (group_tcps.empty())
	EXPECT_FALSE (srdf.kinematics_information.group_tcps.empty())
	EXPECT_TRUE (acm.getAllAllowedCollisions().empty())
acm	addAllowedCollision ("link_1", "link_3", "Adjacent")
acm	addAllowedCollision ("link_1", "link_2", "Adjacent")
acm	addAllowedCollision ("link_2", "link_3", "Adjacent")
acm	addAllowedCollision ("link_3", "link_4", "Adjacent")
acm	addAllowedCollision ("link_4", "link_5", "Adjacent")
acm	addAllowedCollision ("base_link", "link_1", "Adjacent")
	EXPECT_FALSE (acm.getAllAllowedCollisions().empty())
srdf	saveToFile (tesseract_common::getTempPath()+"test.srdf")
srdf_reload	initFile (g, tesseract_common::getTempPath()+"test.srdf", locator)
	EXPECT_TRUE (srdf_reload.name=="test_srdf")
	EXPECT_FALSE (srdf_reload.kinematics_information.chain_groups.empty())
	EXPECT_FALSE (srdf_reload.kinematics_information.joint_groups.empty())
	EXPECT_FALSE (srdf_reload.kinematics_information.link_groups.empty())
	EXPECT_EQ (srdf_reload.kinematics_information.group_states.size(), 3)
	EXPECT_TRUE (srdf_reload.kinematics_information.group_states["manipulator_chain"].find("All Zeros") !=srdf_reload.kinematics_information.group_states["manipulator_chain"].end())
	EXPECT_TRUE (srdf_reload.kinematics_information.group_states["manipulator_joint"].find("All Zeros") !=srdf_reload.kinematics_information.group_states["manipulator_joint"].end())
	EXPECT_TRUE (srdf_reload.kinematics_information.group_states["manipulator_link"].find("All Zeros") !=srdf_reload.kinematics_information.group_states["manipulator_link"].end())
	EXPECT_FALSE (srdf_reload.kinematics_information.group_tcps.empty())
	EXPECT_TRUE (srdf_reload.kinematics_information.group_tcps["manipulator_chain"].find("laser") !=srdf_reload.kinematics_information.group_tcps["manipulator_chain"].end())
	EXPECT_TRUE (srdf_reload.kinematics_information.group_tcps["manipulator_joint"].find("laser") !=srdf_reload.kinematics_information.group_tcps["manipulator_joint"].end())
	EXPECT_TRUE (srdf_reload.kinematics_information.group_tcps["manipulator_link"].find("laser") !=srdf_reload.kinematics_information.group_tcps["manipulator_link"].end())
	EXPECT_FALSE (srdf_reload.acm.getAllAllowedCollisions().empty())
srdf_reload	saveToFile (tesseract_common::getTempPath()+"test_reload.srdf")
	EXPECT_EQ (srdf.name, "abb_irb2400")
	EXPECT_ANY_THROW (srdf.initString(*g, xml_string, locator))
	EXPECT_ANY_THROW (srdf.initFile(*g, "/tmp/file_does_not_exist.srdf", locator))
	EXPECT_TRUE (srdf_save.saveToFile(save_path))
	EXPECT_FALSE (srdf.kinematics_information.kinematics_plugin_info.empty())
	EXPECT_FALSE (srdf.contact_managers_plugin_info.empty())
	EXPECT_FALSE (srdf.calibration_info.empty())
	EXPECT_EQ (kin_info.group_tcps.size(), 1)
	EXPECT_TRUE (tcp_it !=kin_info.group_tcps.end())
	EXPECT_EQ (tcp_it->second.size(), 2)
	EXPECT_TRUE (tcp_it->second.find("laser") !=tcp_it->second.end())
	EXPECT_TRUE (tcp_it->second.find("welder") !=tcp_it->second.end())
	EXPECT_EQ (kin_info.chain_groups.size(), 3)
	EXPECT_TRUE (chain_gantry_it !=kin_info.chain_groups.end())
	EXPECT_TRUE (chain_manipulator_it !=kin_info.chain_groups.end())
	EXPECT_TRUE (chain_positioner_it !=kin_info.chain_groups.end())
	EXPECT_EQ (kin_info.joint_groups.size(), 1)
	EXPECT_TRUE (joint_manipulator_it !=kin_info.joint_groups.end())
	EXPECT_EQ (kin_info.group_states.size(), 1)
	EXPECT_TRUE (group_state_it !=kin_info.group_states.end())
	EXPECT_EQ (group_state_it->second.size(), 1)
	EXPECT_TRUE (group_state_it->second.find("all-zeros") !=group_state_it->second.end())
	EXPECT_TRUE (acm->isCollisionAllowed("base_link", "link_1"))
	EXPECT_TRUE (acm->isCollisionAllowed("base_link", "link_2"))
	EXPECT_TRUE (acm->isCollisionAllowed("base_link", "link_3"))
	EXPECT_TRUE (srdf.collision_margin_data !=nullptr)
	EXPECT_NEAR (srdf.collision_margin_data->getDefaultCollisionMargin(), 0.025, 1e-6)
	EXPECT_EQ (srdf.collision_margin_data->getPairCollisionMargins().size(), 2)
	EXPECT_NEAR (srdf.collision_margin_data->getPairCollisionMargin("link_5", "link_6"), 0.01, 1e-6)
	EXPECT_NEAR (srdf.collision_margin_data->getPairCollisionMargin("link_5", "link_4"), 0.015, 1e-6)
	EXPECT_ANY_THROW (bad_srdf.initFile(*g, save_path, locator))
	EXPECT_TRUE (xml_doc.Parse(xml_string.c_str())==tinyxml2::XML_SUCCESS)
	EXPECT_TRUE (element !=nullptr)
	EXPECT_TRUE (acm.isCollisionAllowed("base_link", "link_1"))
	EXPECT_TRUE (acm.isCollisionAllowed("base_link", "link_2"))
	EXPECT_TRUE (acm.isCollisionAllowed("base_link", "link_3"))
	EXPECT_TRUE (is_failure(xml_string))
	EXPECT_ANY_THROW (srdf_model.initString(*g, xml_string, locator))
	EXPECT_FALSE (is_failure(xml_string))
	EXPECT_NO_THROW (srdf_model.initString(*g, xml_string, locator))
	EXPECT_TRUE (xml_doc.Parse(str.c_str())==tinyxml2::XML_SUCCESS)
	EXPECT_EQ (group_names.size(), 1)
	EXPECT_EQ (chain_groups.size(), 1)
	EXPECT_EQ (chain_groups["manipulator"].size(), 1)
	EXPECT_EQ (chain_groups["manipulator"][0].first, "base_link")
	EXPECT_EQ (chain_groups["manipulator"][0].second, "tool0")
	EXPECT_TRUE (is_failure(str))
	EXPECT_EQ (joint_groups.size(), 1)
	EXPECT_EQ (joint_groups["manipulator"].size(), 7)
	EXPECT_EQ (link_groups.size(), 1)
	EXPECT_EQ (link_groups["manipulator"].size(), 8)
	EXPECT_EQ (group_states.size(), 1)
	EXPECT_TRUE (it !=group_states.end())
	EXPECT_TRUE (it2 !=it->second.end())
	EXPECT_EQ (it2->second.size(), 6)
	EXPECT_DOUBLE_EQ (it2->second["joint_1"], 0)
	EXPECT_DOUBLE_EQ (it2->second["joint_2"], 0)
	EXPECT_DOUBLE_EQ (it2->second["joint_3"], 0)
	EXPECT_DOUBLE_EQ (it2->second["joint_4"], 0)
	EXPECT_DOUBLE_EQ (it2->second["joint_5"], 0)
	EXPECT_DOUBLE_EQ (it2->second["joint_6"], 0)
	EXPECT_EQ (group_tcps.size(), 1)
	EXPECT_EQ (it->second.size(), 2)
	EXPECT_TRUE (it3 !=it->second.end())
	EXPECT_TRUE (margin_data !=nullptr)
	EXPECT_NEAR (margin_data->getDefaultCollisionMargin(), 0.025, 1e-6)
	EXPECT_EQ (margin_data->getPairCollisionMargins().size(), 2)
	EXPECT_NEAR (margin_data->getPairCollisionMargin("link_5", "link_6"), 0.01, 1e-6)
	EXPECT_NEAR (margin_data->getPairCollisionMargin("link_5", "link_4"), 0.015, 1e-6)
	EXPECT_EQ (margin_data->getPairCollisionMargins().size(), 0)
	EXPECT_NEAR (margin_data->getDefaultCollisionMargin(), -0.025, 1e-6)
	EXPECT_NEAR (margin_data->getPairCollisionMargin("link_5", "link_6"), -0.01, 1e-6)
	EXPECT_NEAR (margin_data->getPairCollisionMargin("link_5", "link_4"), -0.015, 1e-6)
	EXPECT_TRUE (margin_data==nullptr)
	EXPECT_FALSE (is_failure(str))
chain_group	push_back (std::make_pair("base_link", "tool0"))
info	addChainGroup ("manipulator", chain_group)
	EXPECT_TRUE (info.hasChainGroup("manipulator"))
	EXPECT_TRUE (info.chain_groups.at("manipulator")==chain_group)
	EXPECT_EQ (info.chain_groups.size(), 1)
	EXPECT_EQ (info.group_names.size(), 1)
	EXPECT_EQ (info1_copy, info)
chain_group	push_back (std::make_pair("tool0", "base_link"))
	EXPECT_NE (info1_copy, info)
info1_copy	insert (info)
info	removeChainGroup ("manipulator")
	EXPECT_FALSE (info.hasChainGroup("manipulator"))
	EXPECT_EQ (info.chain_groups.size(), 0)
	EXPECT_EQ (info.group_names.size(), 0)
info	addJointGroup ("manipulator", joint_group)
	EXPECT_TRUE (info.joint_groups.at("manipulator")==joint_group)
	EXPECT_EQ (info.joint_groups.size(), 1)
info	removeJointGroup ("manipulator")
	EXPECT_EQ (info.joint_groups.size(), 0)
info	addLinkGroup ("manipulator", link_group)
	EXPECT_EQ (info.link_groups.size(), 1)
info	removeLinkGroup ("manipulator")
	EXPECT_EQ (info.link_groups.size(), 0)
info	addGroupJointState ("manipulator", "all-zeros", group_states)
	EXPECT_TRUE (info.hasGroupJointState("manipulator", "all-zeros"))
	EXPECT_TRUE (info.group_states.at("manipulator").at("all-zeros")==group_states)
	EXPECT_EQ (info.group_states.at("manipulator").size(), 1)
	EXPECT_EQ (info.group_states.size(), 1)
info	removeGroupJointState ("manipulator", "all-zeros")
	EXPECT_FALSE (info.hasGroupJointState("manipulator", "all-zeros"))
	EXPECT_EQ (info.group_states.size(), 0)
info	addGroupTCP ("manipulator", "laser", tcp_laser)
info	addGroupTCP ("manipulator", "welder", tcp_welder)
	EXPECT_TRUE (info.hasGroupTCP("manipulator", "laser"))
	EXPECT_TRUE (info.hasGroupTCP("manipulator", "welder"))
	EXPECT_TRUE (info.group_tcps.at("manipulator").at("laser").isApprox(tcp_laser, 1e-6))
	EXPECT_TRUE (info.group_tcps.at("manipulator").at("welder").isApprox(tcp_welder, 1e-6))
	EXPECT_EQ (info.group_tcps.at("manipulator").size(), 2)
	EXPECT_EQ (info.group_tcps.size(), 1)
info	removeGroupTCP ("manipulator", "laser")
info	removeGroupTCP ("manipulator", "welder")
	EXPECT_FALSE (info.hasGroupTCP("manipulator", "laser"))
	EXPECT_FALSE (info.hasGroupTCP("manipulator", "welder"))
	EXPECT_EQ (info.group_tcps.size(), 0)
	EXPECT_TRUE (robot_element !=nullptr)
	EXPECT_TRUE (tesseract_common::fs::exists(path))
	EXPECT_ANY_THROW (tesseract_srdf::parseConfigFilePath(locator, element, version))
	EXPECT_FALSE (info.empty())
	EXPECT_ANY_THROW (tesseract_srdf::parseContactManagersPluginConfig(locator, element, version))
	EXPECT_ANY_THROW (tesseract_srdf::parseKinematicsPluginConfig(locator, element, version))
	EXPECT_ANY_THROW (tesseract_srdf::parseCalibrationConfig(*g, locator, element, version))
int	main (int argc, char **argv)

Variables
std::string	srdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"
TesseractSupportResourceLocator	locator
SceneGraph	g = buildTestSceneGraph()
joint_1	parent_link_name = "base_link"
joint_1	child_link_name = "link_1"
joint_1	type = JointType::FIXED
joint_2	limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10)
SRDFModel	srdf
tesseract_common::AllowedCollisionMatrix::ConstPtr	acm = g.getAllowedCollisionMatrix()
srdf	name = "test_srdf"
auto &	chain_groups = srdf.kinematics_information.chain_groups
auto &	joint_groups = srdf.kinematics_information.joint_groups
auto &	link_groups = srdf.kinematics_information.link_groups
auto &	group_state = srdf.kinematics_information.group_states
GroupsJointState	joint_state = 0
auto &	group_tcps = srdf.kinematics_information.group_tcps
SRDFModel	srdf_reload
std::string	xml_string
std::string	yaml_kin_plugins_string
std::string	yaml_cm_plugins_string
std::string	yaml_calibration_string
SRDFModel	srdf_save
YAML::Node	kinematics_plugin_config = YAML::Load(yaml_kin_plugins_string)
srdf_save kinematics_information	kinematics_plugin_info
YAML::Node	contact_managers_plugin_config = YAML::Load(yaml_cm_plugins_string)
srdf_save	contact_managers_plugin_info
YAML::Node	calibration_config = YAML::Load(yaml_calibration_string)
srdf_save	calibration_info = calibration_config[CalibrationInfo::CONFIG_KEY].as<CalibrationInfo>()
std::string	save_path = tesseract_common::getTempPath() + "unit_test_save_srdf.srdf"
KinematicsInformation &	kin_info = srdf.kinematics_information
auto	tcp_it = kin_info.group_tcps.find("gantry")
auto	chain_gantry_it = kin_info.chain_groups.find("gantry")
auto	chain_manipulator_it = kin_info.chain_groups.find("manipulator")
auto	chain_positioner_it = kin_info.chain_groups.find("positioner")
auto	joint_manipulator_it = kin_info.joint_groups.find("manipulator_joint")
auto	group_state_it = kin_info.group_states.find("manipulator")
YAML::Node	bad_calibration_config = YAML::Load(yaml_calibration_string)
SRDFModel	bad_srdf
tinyxml2::XMLDocument	xml_doc
tinyxml2::XMLElement *	element = xml_doc.FirstChildElement("robot")
auto	is_failure
tesseract_srdf::SRDFModel	srdf_model
std::string	str
	auto [group_names, chain_groups, joint_groups, link_groups]
GroupNames	group_names = { "manipulator" }
GroupJointStates	group_states = parseGroupStates(*g, group_names, element, std::array<int, 3>({ 1, 0, 0 }))
auto	it = group_states.find("manipulator")
auto	it2 = it->second.find("all-zeros")
auto	it3 = it->second.find("welder")
tesseract_common::CollisionMarginData::Ptr	margin_data
KinematicsInformation	info
ChainGroup	chain_group = ChainGroup()
KinematicsInformation	info1_copy = info
JointGroup	joint_group = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }
LinkGroup	link_group = { "link_1", "link_2", "link_3", "link_4", "link_5", "link_6" }
Eigen::Isometry3d	tcp_laser = Eigen::Isometry3d::Identity()
Eigen::Isometry3d	tcp_welder = Eigen::Isometry3d::Identity()
tinyxml2::XMLElement *	robot_element = xml_doc.FirstChildElement("robot")
tesseract_common::fs::path	path = tesseract_srdf::parseConfigFilePath(locator, element, version)

Enumeration Type Documentation

ABBConfig

enum class ABBConfig

strong

Enumerator
ROBOT_ONLY
ROBOT_ON_RAIL
ROBOT_WITH_POSITIONER

Function Documentation

addAllowedCollision() [1/6]

acm addAllowedCollision	(	"base_link"	,
		"link_1"	,
		"Adjacent"
	)

addAllowedCollision() [2/6]

acm addAllowedCollision	(	"link_1"	,
		"link_2"	,
		"Adjacent"
	)

addAllowedCollision() [3/6]

acm addAllowedCollision	(	"link_1"	,
		"link_3"	,
		"Adjacent"
	)

addAllowedCollision() [4/6]

acm addAllowedCollision	(	"link_2"	,
		"link_3"	,
		"Adjacent"
	)

addAllowedCollision() [5/6]

acm addAllowedCollision	(	"link_3"	,
		"link_4"	,
		"Adjacent"
	)

addAllowedCollision() [6/6]

acm addAllowedCollision	(	"link_4"	,
		"link_5"	,
		"Adjacent"
	)

addChainGroup()

info1_copy addChainGroup	(	"manipulator"	,
		chain_group
	)

addGroupJointState()

info1_copy addGroupJointState	(	"manipulator"	,
		"all-zeros"	,
		group_states
	)

addGroupTCP() [1/2]

info1_copy addGroupTCP	(	"manipulator"	,
		"laser"	,
		tcp_laser
	)

addGroupTCP() [2/2]

info1_copy addGroupTCP	(	"manipulator"	,
		"welder"	,
		tcp_welder
	)

addJointGroup()

info1_copy addJointGroup	(	"manipulator"	,
		joint_group
	)

addLinkGroup()

info1_copy addLinkGroup	(	"manipulator"	,
		link_group
	)

base_link()

Link base_link

(

"base_link"

)

buildTestSceneGraph()

tesseract_scene_graph::SceneGraph buildTestSceneGraph

(

)

clearAllowedCollisions()

g clearAllowedCollisions

(

)

EXPECT_ANY_THROW() [1/8]

EXPECT_ANY_THROW

(

bad_srdf.initFile *,,

locator

)

EXPECT_ANY_THROW() [2/8]

EXPECT_ANY_THROW

(

srdf.initFile *, "//.",

locator

)

EXPECT_ANY_THROW() [3/8]

EXPECT_ANY_THROW

(

srdf.initString *,,

locator

)

EXPECT_ANY_THROW() [4/8]

EXPECT_ANY_THROW

(

srdf_model.initString *,,

locator

)

EXPECT_ANY_THROW() [5/8]

EXPECT_ANY_THROW

(

tesseract_srdf::parseCalibrationConfig *,,,

version

)

EXPECT_ANY_THROW() [6/8]

EXPECT_ANY_THROW

(

tesseract_srdf::parseConfigFilePath(locator, element, version)

)

EXPECT_ANY_THROW() [7/8]

EXPECT_ANY_THROW

(

tesseract_srdf::parseContactManagersPluginConfig(locator, element, version)

)

EXPECT_ANY_THROW() [8/8]

EXPECT_ANY_THROW

(

tesseract_srdf::parseKinematicsPluginConfig(locator, element, version)

)

EXPECT_DOUBLE_EQ() [1/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_1"],
		0
	)

EXPECT_DOUBLE_EQ() [2/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_2"],
		0
	)

EXPECT_DOUBLE_EQ() [3/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_3"],
		0
	)

EXPECT_DOUBLE_EQ() [4/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_4"],
		0
	)

EXPECT_DOUBLE_EQ() [5/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_5"],
		0
	)

EXPECT_DOUBLE_EQ() [6/6]

EXPECT_DOUBLE_EQ	(	it2->	second["joint_6"],
		0
	)

EXPECT_EQ() [1/45]

EXPECT_EQ	(	acm->	getAllAllowedCollisions).size(,
		0
	)

EXPECT_EQ() [2/45]

EXPECT_EQ	(	chain_groups.	size(),
		1
	)

EXPECT_EQ() [3/45]

EXPECT_EQ	(	chain_groups.	size()["manipulator"],
		1
	)

EXPECT_EQ() [4/45]

EXPECT_EQ	(	chain_groups.	first["manipulator"][0],
		"base_link"
	)

EXPECT_EQ() [5/45]

EXPECT_EQ	(	chain_groups.	second["manipulator"][0],
		"tool0"
	)

EXPECT_EQ() [6/45]

EXPECT_EQ	(	group_names.	size(),
		1
	)

EXPECT_EQ() [7/45]

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

EXPECT_EQ() [8/45]

EXPECT_EQ	(	group_states.	size(),
		1
	)

EXPECT_EQ() [9/45]

EXPECT_EQ	(	group_tcps.	size(),
		1
	)

EXPECT_EQ() [10/45]

EXPECT_EQ	(	info.chain_groups.	size(),
		0
	)

EXPECT_EQ() [11/45]

EXPECT_EQ	(	info.chain_groups.	size(),
		1
	)

EXPECT_EQ() [12/45]

EXPECT_EQ	(	info.group_names.	size(),
		0
	)

EXPECT_EQ() [13/45]

EXPECT_EQ	(	info.group_names.	size(),
		1
	)

EXPECT_EQ() [14/45]

EXPECT_EQ	(	info.group_states.	at"manipulator").size(,
		1
	)

EXPECT_EQ() [15/45]

EXPECT_EQ	(	info.group_states.	size(),
		0
	)

EXPECT_EQ() [16/45]

EXPECT_EQ	(	info.group_states.	size(),
		1
	)

EXPECT_EQ() [17/45]

EXPECT_EQ	(	info.group_tcps.	at"manipulator").size(,
		2
	)

EXPECT_EQ() [18/45]

EXPECT_EQ	(	info.group_tcps.	size(),
		0
	)

EXPECT_EQ() [19/45]

EXPECT_EQ	(	info.group_tcps.	size(),
		1
	)

EXPECT_EQ() [20/45]

EXPECT_EQ	(	info.joint_groups.	size(),
		0
	)

EXPECT_EQ() [21/45]

EXPECT_EQ	(	info.joint_groups.	size(),
		1
	)

EXPECT_EQ() [22/45]

EXPECT_EQ	(	info.link_groups.	size(),
		0
	)

EXPECT_EQ() [23/45]

EXPECT_EQ	(	info.link_groups.	size(),
		1
	)

EXPECT_EQ() [24/45]

EXPECT_EQ	(	info1_copy	,
		info
	)

EXPECT_EQ() [25/45]

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

EXPECT_EQ() [26/45]

EXPECT_EQ	(	it2->second.	size(),
		6
	)

EXPECT_EQ() [27/45]

EXPECT_EQ	(	joint_groups.	size(),
		1
	)

EXPECT_EQ() [28/45]

EXPECT_EQ	(	joint_groups.	size()["manipulator"],
		7
	)

EXPECT_EQ() [29/45]

EXPECT_EQ	(	kin_info.chain_groups.	size(),
		3
	)

EXPECT_EQ() [30/45]

EXPECT_EQ	(	kin_info.group_states.	size(),
		1
	)

EXPECT_EQ() [31/45]

EXPECT_EQ	(	kin_info.group_tcps.	size(),
		1
	)

EXPECT_EQ() [32/45]

EXPECT_EQ	(	kin_info.joint_groups.	size(),
		1
	)

EXPECT_EQ() [33/45]

EXPECT_EQ	(	link_groups.	size(),
		1
	)

EXPECT_EQ() [34/45]

EXPECT_EQ	(	link_groups.	size()["manipulator"],
		8
	)

EXPECT_EQ() [35/45]

EXPECT_EQ	(	margin_data->	getPairCollisionMargins).size(,
		0
	)

EXPECT_EQ() [36/45]

EXPECT_EQ	(	margin_data->	getPairCollisionMargins).size(,
		2
	)

EXPECT_EQ() [37/45]

EXPECT_EQ	(	srdf.collision_margin_data->	getPairCollisionMargins).size(,
		2
	)

EXPECT_EQ() [38/45]

EXPECT_EQ	(	srdf.kinematics_information.group_states.	size(),
		3
	)

EXPECT_EQ() [39/45]

EXPECT_EQ	(	srdf.	name,
		"abb_irb2400"
	)

EXPECT_EQ() [40/45]

EXPECT_EQ	(	srdf.	name,
		"kuka_lbr_iiwa_14_r820"
	)

EXPECT_EQ() [41/45]

EXPECT_EQ	(	srdf.	version[0],
		1
	)

EXPECT_EQ() [42/45]

EXPECT_EQ	(	srdf.	version[1],
		0
	)

EXPECT_EQ() [43/45]

EXPECT_EQ	(	srdf.	version[2],
		0
	)

EXPECT_EQ() [44/45]

EXPECT_EQ	(	srdf_reload.kinematics_information.group_states.	size(),
		3
	)

EXPECT_EQ() [45/45]

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

EXPECT_FALSE() [1/22]

EXPECT_FALSE

(

acm->

isCollisionAllowed"base_link", "link_5"

)

EXPECT_FALSE() [2/22]

EXPECT_FALSE

(

acm->

isCollisionAllowed"link_1", "link_2"

)

EXPECT_FALSE() [3/22]

EXPECT_FALSE

(

acm.

getAllAllowedCollisions).empty(

)

EXPECT_FALSE() [4/22]

EXPECT_FALSE

(

info.

empty()

)

EXPECT_FALSE() [5/22]

EXPECT_FALSE

(

info.

hasChainGroup"manipulator"

)

EXPECT_FALSE() [6/22]

EXPECT_FALSE

(

info.

hasGroupJointState"manipulator", "all-zeros"

)

EXPECT_FALSE() [7/22]

EXPECT_FALSE

(

info.

hasGroupTCP"manipulator", "laser"

)

EXPECT_FALSE() [8/22]

EXPECT_FALSE

(

info.

hasGroupTCP"manipulator", "welder"

)

EXPECT_FALSE() [9/22]

EXPECT_FALSE

(

is_failure(str)

)

Initial value:

{  
    std::string str = R"(<robot name="abb_irb2400">
                           <collision_margins default_margin="-0.025">
                             <pair_margin link1="link_6" link2="missing_link" margin="0.01"/>
                             <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
                           </collision_margins>
                         </robot>)"

EXPECT_FALSE() [10/22]

EXPECT_FALSE

(

is_failure(xml_string)

)

Initial value:

{  
    std::string xml_string =
        R"(<robot name="abb_irb2400">
             <disable_collisions link1="missing_link" link2="link_1" reason="Adjacent" />
             <disable_collisions link1="base_link" link2="link_2" reason="Never" />
             <disable_collisions link1="base_link" link2="link_3" reason="Never" />
           </robot>)"

EXPECT_FALSE() [11/22]

EXPECT_FALSE

(

srdf.calibration_info.

empty()

)

EXPECT_FALSE() [12/22]

EXPECT_FALSE

(

srdf.contact_managers_plugin_info.

empty()

)

EXPECT_FALSE() [13/22]

EXPECT_FALSE

(

srdf.kinematics_information.chain_groups.

empty()

)

EXPECT_FALSE() [14/22]

EXPECT_FALSE

(

srdf.kinematics_information.group_tcps.

empty()

)

EXPECT_FALSE() [15/22]

EXPECT_FALSE

(

srdf.kinematics_information.joint_groups.

empty()

)

EXPECT_FALSE() [16/22]

EXPECT_FALSE

(

srdf.kinematics_information.kinematics_plugin_info.

empty()

)

EXPECT_FALSE() [17/22]

EXPECT_FALSE

(

srdf.kinematics_information.link_groups.

empty()

)

EXPECT_FALSE() [18/22]

EXPECT_FALSE

(

srdf_reload.acm.

getAllAllowedCollisions).empty(

)

EXPECT_FALSE() [19/22]

EXPECT_FALSE

(

srdf_reload.kinematics_information.chain_groups.

empty()

)

EXPECT_FALSE() [20/22]

EXPECT_FALSE

(

srdf_reload.kinematics_information.group_tcps.

empty()

)

EXPECT_FALSE() [21/22]

EXPECT_FALSE

(

srdf_reload.kinematics_information.joint_groups.

empty()

)

EXPECT_FALSE() [22/22]

EXPECT_FALSE

(

srdf_reload.kinematics_information.link_groups.

empty()

)

EXPECT_NE()

EXPECT_NE	(	info1_copy	,
		info
	)

EXPECT_NEAR() [1/9]

EXPECT_NEAR	(	margin_data->	getDefaultCollisionMargin(),
		-0.	025,
		1e-	6
	)

EXPECT_NEAR() [2/9]

EXPECT_NEAR	(	margin_data->	getDefaultCollisionMargin(),
		0.	025,
		1e-	6
	)

EXPECT_NEAR() [3/9]

EXPECT_NEAR	(	margin_data->	getPairCollisionMargin"link_5", "link_4",
		-0.	015,
		1e-	6
	)

EXPECT_NEAR() [4/9]

EXPECT_NEAR	(	margin_data->	getPairCollisionMargin"link_5", "link_4",
		0.	015,
		1e-	6
	)

EXPECT_NEAR() [5/9]

EXPECT_NEAR	(	margin_data->	getPairCollisionMargin"link_5", "link_6",
		-0.	01,
		1e-	6
	)

EXPECT_NEAR() [6/9]

EXPECT_NEAR	(	margin_data->	getPairCollisionMargin"link_5", "link_6",
		0.	01,
		1e-	6
	)

EXPECT_NEAR() [7/9]

EXPECT_NEAR	(	srdf.collision_margin_data->	getDefaultCollisionMargin(),
		0.	025,
		1e-	6
	)

EXPECT_NEAR() [8/9]

EXPECT_NEAR	(	srdf.collision_margin_data->	getPairCollisionMargin"link_5", "link_4",
		0.	015,
		1e-	6
	)

EXPECT_NEAR() [9/9]

EXPECT_NEAR	(	srdf.collision_margin_data->	getPairCollisionMargin"link_5", "link_6",
		0.	01,
		1e-	6
	)

EXPECT_NO_THROW()

EXPECT_NO_THROW

(

srdf_model.initString *,,

locator

)

EXPECT_TRUE() [1/70]

EXPECT_TRUE

(

acm->

isCollisionAllowed"base_link", "link_1"

)

EXPECT_TRUE() [2/70]

EXPECT_TRUE

(

acm->

isCollisionAllowed"base_link", "link_2"

)

EXPECT_TRUE() [3/70]

EXPECT_TRUE

(

acm->

isCollisionAllowed"base_link", "link_3"

)

EXPECT_TRUE() [4/70]

EXPECT_TRUE

(

acm->

isCollisionAllowed"link_1", "link_2"

)

EXPECT_TRUE() [5/70]

EXPECT_TRUE

(

acm.

getAllAllowedCollisions).empty(

)

EXPECT_TRUE() [6/70]

EXPECT_TRUE

(

acm.

isCollisionAllowed"base_link", "link_1"

)

EXPECT_TRUE() [7/70]

EXPECT_TRUE

(

acm.

isCollisionAllowed"base_link", "link_2"

)

EXPECT_TRUE() [8/70]

EXPECT_TRUE

(

acm.

isCollisionAllowed"base_link", "link_3"

)

EXPECT_TRUE() [9/70]

EXPECT_TRUE

(

chain_gantry_it !

= kin_info.chain_groups.end()

)

EXPECT_TRUE() [10/70]

EXPECT_TRUE

(

chain_groups.

empty()

)

EXPECT_TRUE() [11/70]

EXPECT_TRUE

(

chain_manipulator_it !

= kin_info.chain_groups.end()

)

EXPECT_TRUE() [12/70]

EXPECT_TRUE

(

chain_positioner_it !

= kin_info.chain_groups.end()

)

EXPECT_TRUE() [13/70]

EXPECT_TRUE

(

element !

= nullptr

)

EXPECT_TRUE() [14/70]

EXPECT_TRUE

(

g.

addJointjoint_1

)

EXPECT_TRUE() [15/70]

EXPECT_TRUE

(

g.

addJointjoint_2

)

EXPECT_TRUE() [16/70]

EXPECT_TRUE

(

g.

addJointjoint_3

)

EXPECT_TRUE() [17/70]

EXPECT_TRUE

(

g.

addJointjoint_4

)

EXPECT_TRUE() [18/70]

EXPECT_TRUE

(

g.

addJointjoint_5

)

EXPECT_TRUE() [19/70]

EXPECT_TRUE

(

g.

addJointjoint_6

)

EXPECT_TRUE() [20/70]

EXPECT_TRUE

(

g.

addJointjoint_7

)

EXPECT_TRUE() [21/70]

EXPECT_TRUE

(

g.

addJointjoint_tool0

)

EXPECT_TRUE() [22/70]

EXPECT_TRUE

(

g.

addLinkbase_link

)

EXPECT_TRUE() [23/70]

EXPECT_TRUE

(

g.

addLinklink_1

)

EXPECT_TRUE() [24/70]

EXPECT_TRUE

(

g.

addLinklink_2

)

EXPECT_TRUE() [25/70]

EXPECT_TRUE

(

g.

addLinklink_3

)

EXPECT_TRUE() [26/70]

EXPECT_TRUE

(

g.

addLinklink_4

)

EXPECT_TRUE() [27/70]

EXPECT_TRUE

(

g.

addLinklink_5

)

EXPECT_TRUE() [28/70]

EXPECT_TRUE

(

g.

addLinklink_6

)

EXPECT_TRUE() [29/70]

EXPECT_TRUE

(

g.

addLinklink_7

)

EXPECT_TRUE() [30/70]

EXPECT_TRUE

(

g.

addLinktool0

)

EXPECT_TRUE() [31/70]

EXPECT_TRUE

(

group_state.

empty()

)

EXPECT_TRUE() [32/70]

EXPECT_TRUE

(

group_state_it !

= kin_info.group_states.end()

)

EXPECT_TRUE() [33/70]

EXPECT_TRUE

(

group_state_it->second.find("all-zeros") !

= group_state_it->second.end()

)

EXPECT_TRUE() [34/70]

EXPECT_TRUE

(

group_tcps.

empty()

)

EXPECT_TRUE() [35/70]

EXPECT_TRUE

(

info.chain_groups.

at"manipulator" = =chain_group

)

EXPECT_TRUE() [36/70]

EXPECT_TRUE

(

info.group_states.

at"manipulator").at("all-zeros" = =group_states

)

EXPECT_TRUE() [37/70]

EXPECT_TRUE

(

info.group_tcps.

at"manipulator").at("laser").isApprox(tcp_laser, 1e-6

)

EXPECT_TRUE() [38/70]

EXPECT_TRUE

(

info.group_tcps.

at"manipulator").at("welder").isApprox(tcp_welder, 1e-6

)

EXPECT_TRUE() [39/70]

EXPECT_TRUE

(

info.

hasChainGroup"manipulator"

)

EXPECT_TRUE() [40/70]

EXPECT_TRUE

(

info.

hasGroupJointState"manipulator", "all-zeros"

)

EXPECT_TRUE() [41/70]

EXPECT_TRUE

(

info.

hasGroupTCP"manipulator", "laser"

)

EXPECT_TRUE() [42/70]

EXPECT_TRUE

(

info.

hasGroupTCP"manipulator", "welder"

)

EXPECT_TRUE() [43/70]

EXPECT_TRUE

(

info.joint_groups.

at"manipulator" = =joint_group

)

EXPECT_TRUE() [44/70]

EXPECT_TRUE

(

is_failure(str)

)

Initial value:

{  
    std::string str = R"(<robot name="abb_irb2400">
                           <group name="manipulator"/>
                         </robot>)"

EXPECT_TRUE() [45/70]

EXPECT_TRUE

(

is_failure(xml_string)

)

Initial value:

{  
    std::string xml_string =
        R"(<robot name="abb_irb2400">
             <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
             <disable_collisions link1="base_link" reason="Never" />
             <disable_collisions link1="base_link" link2="link_3" reason="Never" />
           </robot>)"

EXPECT_TRUE() [46/70]

EXPECT_TRUE

(

it !

= group_states.end()

)

EXPECT_TRUE() [47/70]

EXPECT_TRUE

(

it2 !

= it->second.end()

)

EXPECT_TRUE() [48/70]

EXPECT_TRUE

(

it3 !

= it->second.end()

)

EXPECT_TRUE() [49/70]

EXPECT_TRUE

(

joint_groups.

empty()

)

EXPECT_TRUE() [50/70]

EXPECT_TRUE

(

joint_manipulator_it !

= kin_info.joint_groups.end()

)

EXPECT_TRUE() [51/70]

EXPECT_TRUE

(

link_groups.

empty()

)

EXPECT_TRUE() [52/70]

EXPECT_TRUE

(

margin_data !

= nullptr

)

EXPECT_TRUE() [53/70]

EXPECT_TRUE

(

margin_data

= =nullptr

)

EXPECT_TRUE() [54/70]

EXPECT_TRUE

(

robot_element !

= nullptr

)

EXPECT_TRUE() [55/70]

EXPECT_TRUE

(

srdf.collision_margin_data !

= nullptr

)

EXPECT_TRUE() [56/70]

EXPECT_TRUE

(

srdf.

name = ="test_srdf"

)

EXPECT_TRUE() [57/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_states.find("All Zeros") !

["manipulator_chain"] = srdf_reload.kinematics_information.group_states["manipulator_chain"].end()

)

EXPECT_TRUE() [58/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_states.find("All Zeros") !

["manipulator_joint"] = srdf_reload.kinematics_information.group_states["manipulator_joint"].end()

)

EXPECT_TRUE() [59/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_states.find("All Zeros") !

["manipulator_link"] = srdf_reload.kinematics_information.group_states["manipulator_link"].end()

)

EXPECT_TRUE() [60/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_tcps.find("laser") !

["manipulator_chain"] = srdf_reload.kinematics_information.group_tcps["manipulator_chain"].end()

)

EXPECT_TRUE() [61/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_tcps.find("laser") !

["manipulator_joint"] = srdf_reload.kinematics_information.group_tcps["manipulator_joint"].end()

)

EXPECT_TRUE() [62/70]

EXPECT_TRUE

(

srdf_reload.kinematics_information.group_tcps.find("laser") !

["manipulator_link"] = srdf_reload.kinematics_information.group_tcps["manipulator_link"].end()

)

EXPECT_TRUE() [63/70]

EXPECT_TRUE

(

srdf_reload.

name = ="test_srdf"

)

EXPECT_TRUE() [64/70]

EXPECT_TRUE

(

srdf_save.

saveToFilesave_path

)

EXPECT_TRUE() [65/70]

EXPECT_TRUE

(

tcp_it !

= kin_info.group_tcps.end()

)

EXPECT_TRUE() [66/70]

EXPECT_TRUE

(

tcp_it->second.find("laser") !

= tcp_it->second.end()

)

EXPECT_TRUE() [67/70]

EXPECT_TRUE

(

tcp_it->second.find("welder") !

= tcp_it->second.end()

)

EXPECT_TRUE() [68/70]

EXPECT_TRUE

(

tesseract_common::fs::exists(path)

)

EXPECT_TRUE() [69/70]

EXPECT_TRUE

(

xml_doc.

Parsestr.c_str() = =tinyxml2::XML_SUCCESS

)

EXPECT_TRUE() [70/70]

EXPECT_TRUE

(

xml_doc.

Parsexml_string.c_str() = =tinyxml2::XML_SUCCESS

)

getABBSceneGraph()

tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph

(

ABBConfig

config = ABBConfig::ROBOT_ONLY

)

initFile() [1/2]

srdf initFile	(	g	,
		srdf_file	,
		locator
	)

initFile() [2/2]

srdf_reload initFile	(	g	,
		tesseract_common::getTempPath()+"test.srdf"	,
		locator
	)

insert()

info1_copy insert

(

info

)

joint_1()

Joint joint_1

(

"joint_a1"

)

joint_2()

Joint joint_2

(

"joint_a2"

)

joint_3()

Joint joint_3

(

"joint_a3"

)

joint_4()

Joint joint_4

(

"joint_a4"

)

joint_5()

Joint joint_5

(

"joint_a5"

)

joint_6()

Joint joint_6

(

"joint_a6"

)

joint_7()

Joint joint_7

(

"joint_a7"

)

joint_tool0()

Joint joint_tool0

(

"joint_tool0"

)

link_1()

Link link_1

(

"link_1"

)

link_2()

Link link_2

(

"link_2"

)

link_3()

Link link_3

(

"link_3"

)

link_4()

Link link_4

(

"link_4"

)

link_5()

Link link_5

(

"link_5"

)

link_6()

Link link_6

(

"link_6"

)

link_7()

Link link_7

(

"link_7"

)

main()

int main	(	int	argc,
		char **	argv
	)

processSRDFAllowedCollisions()

processSRDFAllowedCollisions	(	g	,
		srdf
	)

push_back() [1/2]

chain_group push_back

(

std::make_pair("base_link", "tool0")

)

push_back() [2/2]

chain_group push_back

(

std::make_pair("tool0", "base_link")

)

removeAllowedCollision()

g removeAllowedCollision	(	"link_1"	,
		"link_2"
	)

removeChainGroup()

info removeChainGroup

(

"manipulator"

)

removeGroupJointState()

info removeGroupJointState	(	"manipulator"	,
		"all-zeros"
	)

removeGroupTCP() [1/2]

info removeGroupTCP	(	"manipulator"	,
		"laser"
	)

removeGroupTCP() [2/2]

info removeGroupTCP	(	"manipulator"	,
		"welder"
	)

removeJointGroup()

info removeJointGroup

(

"manipulator"

)

removeLinkGroup()

info removeLinkGroup

(

"manipulator"

)

saveToFile() [1/2]

srdf saveToFile

(

tesseract_common::getTempPath()+"test.srdf"

)

saveToFile() [2/2]

srdf_reload saveToFile

(

tesseract_common::getTempPath()+"test_reload.srdf"

)

setName()

g setName

(

"kuka_lbr_iiwa_14_r820"

)

tool0()

Link tool0

(

"tool0"

)

translation()

joint_3 parent_to_joint_origin_transform translation

(

)

Variable Documentation

acm

tesseract_common::AllowedCollisionMatrix acm = g.getAllowedCollisionMatrix()

auto

auto[group_names, chain_groups, joint_groups, link_groups]

Initial value:

=
      parseGroups(*g, element, std::array<int, 3>({ 1, 0, 0 }))

bad_calibration_config

YAML::Node bad_calibration_config = YAML::Load(yaml_calibration_string)

bad_srdf

SRDFModel bad_srdf

calibration_config

YAML::Node calibration_config = YAML::Load(yaml_calibration_string)

calibration_info

srdf_save calibration_info = calibration_config[CalibrationInfo::CONFIG_KEY].as<CalibrationInfo>()

chain_gantry_it

auto chain_gantry_it = kin_info.chain_groups.find("gantry")

chain_group

chain_group = ChainGroup()

chain_groups

chain_groups["manipulator_chain"] = srdf.kinematics_information.chain_groups

chain_manipulator_it

auto chain_manipulator_it = kin_info.chain_groups.find("manipulator")

chain_positioner_it

auto chain_positioner_it = kin_info.chain_groups.find("positioner")

child_link_name

joint_tool0 child_link_name = "link_1"

contact_managers_plugin_config

YAML::Node contact_managers_plugin_config = YAML::Load(yaml_cm_plugins_string)

contact_managers_plugin_info

srdf_save contact_managers_plugin_info

Initial value:

=
      contact_managers_plugin_config[ContactManagersPluginInfo::CONFIG_KEY].as<ContactManagersPluginInfo>()

element

tinyxml2::XMLElement * element = xml_doc.FirstChildElement("robot")

g

tesseract_scene_graph::SceneGraph::Ptr g = buildTestSceneGraph()

group_names

EXPECT_EQ * group_names = { "manipulator" }

group_state

group_state["manipulator_link"]["All Zeros"] = srdf.kinematics_information.group_states

group_state_it

auto group_state_it = kin_info.group_states.find("manipulator")

group_states

GroupsJointState group_states = parseGroupStates(*g, group_names, element, std::array<int, 3>({ 1, 0, 0 }))

group_tcps

GroupTCPs group_tcps = srdf.kinematics_information.group_tcps

info

tesseract_common::CalibrationInfo info

Initial value:

{
  using namespace tesseract_srdf

info1_copy

KinematicsInformation info1_copy = info

is_failure

auto is_failure

Initial value:

= [g](const std::string& xml_string) {
    tinyxml2::XMLDocument xml_doc;
    EXPECT_TRUE(xml_doc.Parse(xml_string.c_str()) == tinyxml2::XML_SUCCESS);
 
    tinyxml2::XMLElement* element = xml_doc.FirstChildElement("robot");
    EXPECT_TRUE(element != nullptr);
 
    try
    {
      parseDisabledCollisions(*g, element, std::array<int, 3>({ 1, 0, 0 }));
    }
    catch (const std::exception& e)
    {
      tesseract_common::printNestedException(e);
      return true;
    }
    return false;
  }

it

auto it = group_states.find("manipulator")

it2

auto it2 = it->second.find("all-zeros")

it3

auto it3 = it->second.find("welder")

joint_group

joint_group = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }

joint_groups

joint_groups["manipulator_joint"] = srdf.kinematics_information.joint_groups

joint_manipulator_it

auto joint_manipulator_it = kin_info.joint_groups.find("manipulator_joint")

joint_state

joint_state["joint_4"] = 0

kin_info

KinematicsInformation& kin_info = srdf.kinematics_information

kinematics_plugin_config

YAML::Node kinematics_plugin_config = YAML::Load(yaml_kin_plugins_string)

kinematics_plugin_info

srdf_save kinematics_information kinematics_plugin_info

Initial value:

=
      kinematics_plugin_config[KinematicsPluginInfo::CONFIG_KEY].as<KinematicsPluginInfo>()

limits

joint_7 limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10)

link_group

link_group = { "link_1", "link_2", "link_3", "link_4", "link_5", "link_6" }

link_groups

link_groups["manipulator_link"] = srdf.kinematics_information.link_groups

locator

tesseract_common::TesseractSupportResourceLocator locator

Initial value:

{
  std::array<int, 3> version{ 1, 0, 0 }

margin_data

tesseract_common::CollisionMarginData::Ptr margin_data

Initial value:

=
        parseCollisionMargins(*g, element, std::array<int, 3>({ 1, 0, 0 }))

name

srdf name = "test_srdf"

parent_link_name

joint_tool0 parent_link_name = "base_link"

path

tesseract_common::fs::path path = tesseract_srdf::parseConfigFilePath(locator, element, version)

robot_element

tinyxml2::XMLElement * robot_element = xml_doc.FirstChildElement("robot")

save_path

save_path = tesseract_common::getTempPath() + "unit_test_save_srdf.srdf"

srdf

SRDFModel srdf

Initial value:

{  
    std::string xml_string =
        R"(<robot name="abb_irb2400" version="1.0.0">
             <group name="manipulator">
               <chain base_link="base_link" tip_link="tool0" />
             </group>
           </robot>)"

srdf_file

std::string srdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"

srdf_model

tesseract_srdf::SRDFModel srdf_model

srdf_reload

SRDFModel srdf_reload

srdf_save

SRDFModel srdf_save

str

std::string str

Initial value:

= R"(<robot name="abb_irb2400">
                         <group name="manipulator">
                           <chain base_link="base_link" tip_link="tool0" />
                         </group>
                       </robot>)"

tcp_it

auto tcp_it = kin_info.group_tcps.find("gantry")

tcp_laser

tcp_laser = Eigen::Isometry3d::Identity()

tcp_welder

tcp_welder = Eigen::Isometry3d::Identity()

type

joint_tool0 type = JointType::FIXED

xml_doc

tinyxml2::XMLDocument xml_doc

Initial value:

{  
    std::string str = R"(<robot name="abb_irb2400">
                           <collision_margins default_margin="0.025">
                             <pair_margin link1="link_6" link2="link_5" margin="0.01"/>
                             <pair_margin link1="link_5" link2="link_4" margin="0.015"/>
                           </collision_margins>
                         </robot>)"

xml_string

std::string xml_string

yaml_calibration_string

yaml_calibration_string

Initial value:

=
      R"(calibration:
           joints:
             joint_1:
               position:
                 x: 1
                 y: 2
                 z: 3
               orientation:
                 x: 0
                 y: 0
                 z: 0
                 w: 1
             joint_2:
               position:
                 x: 4
                 y: 5
                 z: 6
               orientation:
                 x: 0
                 y: 0
                 z: 0
                 w: 1)"

yaml_cm_plugins_string

std::string yaml_cm_plugins_string

Initial value:

=
      R"(contact_manager_plugins:
           search_paths:
             - /usr/local/lib
           search_libraries:
             - tesseract_collision_bullet_factories
             - tesseract_collision_fcl_factories
           discrete_plugins:
             default: BulletDiscreteBVHManager
             plugins:
               BulletDiscreteBVHManager:
                 class: BulletDiscreteBVHManagerFactory
                 default: true
               BulletDiscreteSimpleManager:
                 class: BulletDiscreteSimpleManagerFactory
               FCLDiscreteBVHManager:
                 class: FCLDiscreteBVHManagerFactory
           continuous_plugins:
             default: BulletCastBVHManager
             plugins:
               BulletCastBVHManager:
                 class: BulletCastBVHManagerFactory
                 default: true
               BulletCastSimpleManager:
                 class: BulletCastSimpleManagerFactory)"

yaml_kin_plugins_string

std::string yaml_kin_plugins_string

Initial value:

=
      R"(kinematic_plugins:
           fwd_kin_plugins:
             manipulator:
               default: KDLFwdKinChain
               plugins:
                 KDLFwdKinChain:
                   class: KDLFwdKinChainFactory
                   default: true
                   config:
                     base_link: base_link
                     tip_link: tool0
           inv_kin_plugins:
             manipulator:
               default: KDLInvKinChainLMA
               plugins:
                 KDLInvKinChainLMA:
                   class: KDLInvKinChainLMAFactory
                   default: true
                   config:
                     base_link: base_link
                     tip_link: tool0
                 KDLInvKinChainNR:
                   class: KDLInvKinChainNRFactory
                   config:
                     base_link: base_link
                     tip_link: tool0)"