kinematics_factory_unit.cpp File Reference

Tesseract kinematics factory test. More...

#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <fstream>
#include "kinematics_test_utils.h"
#include <tesseract_kinematics/core/kinematics_plugin_factory.h>
#include <tesseract_state_solver/kdl/kdl_state_solver.h>

Include dependency graph for kinematics_factory_unit.cpp:

Functions
void	runKinematicsFactoryTest (const tesseract_common::fs::path &config_path)
	runKinematicsFactoryTest (config_path)
	runKinematicsFactoryTest (export_config_path)
	EXPECT_FALSE (factory.getSearchPaths().empty())
	EXPECT_EQ (factory.getSearchPaths().size(), 1)
	EXPECT_EQ (factory.getSearchLibraries().size(), 4)
	EXPECT_EQ (factory.getFwdKinPlugins().size(), 0)
factory	addSearchPath ("/usr/local/lib")
	EXPECT_EQ (factory.getSearchPaths().size(), 2)
factory	addSearchLibrary ("tesseract_collision")
	EXPECT_EQ (factory.getSearchLibraries().size(), 5)
	EXPECT_TRUE (map.find("manipulator")==map.end())
factory	addFwdKinPlugin ("manipulator", "KDLFwdKin", pi)
	EXPECT_TRUE (map.find("manipulator") !=map.end())
	EXPECT_TRUE (map.at("manipulator").plugins.find("KDLFwdKin") !=map.at("manipulator").plugins.end())
	EXPECT_EQ (map.find("manipulator") ->second.plugins.size(), 1)
	EXPECT_EQ (factory.getDefaultFwdKinPlugin("manipulator"), "KDLFwdKin")
factory	addFwdKinPlugin ("manipulator", "default", pi2)
	EXPECT_TRUE (map.at("manipulator").plugins.find("default") !=map.at("manipulator").plugins.end())
	EXPECT_EQ (map.find("manipulator") ->second.plugins.size(), 2)
factory	setDefaultFwdKinPlugin ("manipulator", "default")
	EXPECT_EQ (factory.getDefaultFwdKinPlugin("manipulator"), "default")
factory	removeFwdKinPlugin ("manipulator", "default")
factory	addInvKinPlugin ("manipulator", "KDLInvKin", pi)
	EXPECT_TRUE (map.at("manipulator").plugins.find("KDLInvKin") !=map.at("manipulator").plugins.end())
	EXPECT_EQ (factory.getDefaultInvKinPlugin("manipulator"), "KDLInvKin")
factory	addInvKinPlugin ("manipulator", "default", pi2)
factory	setDefaultInvKinPlugin ("manipulator", "default")
factory	removeInvKinPlugin ("manipulator", "default")
plugin	remove ("OPWInvKin")
	EXPECT_TRUE (inv_kin==nullptr)
plugin	remove ("class")
	EXPECT_ANY_THROW (KinematicsPluginFactory factory(config))
plugin	remove ("default")
	EXPECT_TRUE (inv_kin !=nullptr)
KinematicsPluginFactory	factory (YAML::Load(yaml_string))
	EXPECT_EQ (inv_kin->getSolverName(), "OPWInvKin")
plugin	remove ("config")
plugin["config"]	remove ("base_link")
plugin["config"]	remove ("tip_link")
plugin["config"]	remove ("params")
plugin["config"]["params"]	remove ("a1")
plugin["config"]["params"]	remove ("a2")
plugin["config"]["params"]	remove ("b")
plugin["config"]["params"]	remove ("c1")
plugin["config"]["params"]	remove ("c2")
plugin["config"]["params"]	remove ("c3")
plugin["config"]["params"]	remove ("c4")
plugin["config"]["params"]	remove ("offset")
plugin["config"]["params"]	remove ("sign_corrections")
plugin["config"]["params"]["offsets"]	push_back (0)
	EXPECT_EQ (inv_kin->getSolverName(), "URInvKin")
KinematicsPluginFactory	factory (YAML::Load(yaml_model_string))
plugin["config"]	remove ("model")
plugin["config"]["params"]	remove ("d1")
plugin["config"]["params"]	remove ("a3")
plugin["config"]["params"]	remove ("d4")
plugin["config"]["params"]	remove ("d5")
plugin["config"]["params"]	remove ("d6")
	EXPECT_EQ (inv_kin->getSolverName(), "REPInvKin")
plugin["config"]	remove ("manipulator_reach")
plugin["config"]	remove ("positioner_sample_resolution")
plugin["config"]["positioner_sample_resolution"][0]	remove ("name")
plugin["config"]["positioner_sample_resolution"][0]	remove ("value")
plugin["config"]	remove ("positioner")
plugin["config"]	remove ("manipulator")
	EXPECT_EQ (inv_kin->getSolverName(), "ROPInvKin")
	EXPECT_TRUE (fwd_kin !=nullptr)
	EXPECT_EQ (fwd_kin->getSolverName(), "KDLFwdKinChain")
	EXPECT_EQ (inv_kin->getSolverName(), "KDLInvKinChainLMA")
	EXPECT_EQ (inv_kin->getSolverName(), "KDLInvKinChainNR")
	EXPECT_TRUE (kin==nullptr)
int	main (int argc, char **argv)

Variables
tesseract_common::fs::path	config_path = file_path.parent_path() / "kinematic_plugins.yaml"
tesseract_common::fs::path	export_config_path
tesseract_common::PluginInfo	pi
pi	class_name = "KDLFwdKin"
	map = factory.getFwdKinPlugins()
tesseract_common::PluginInfo	pi2
tesseract_scene_graph::SceneGraph::UPtr	scene_graph = getSceneGraphABB()
tesseract_scene_graph::SceneState	scene_state = state_solver.getState()
std::string	yaml_string
auto	plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]
KinematicsPluginFactory	factory (config)
auto	inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state)
std::string	yaml_model_string
std::string	yaml_params_string
	config ["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR10e"
auto	fwd_kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state)
auto	kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state)

Detailed Description

Tesseract kinematics factory test.

Author

Levi Armstrong

Date

Feb 4, 2021

Version

TODO

Bug:

No known bugs

Copyright

Copyright (c) 2021, Southwest Research Institute

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

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

Function Documentation

addFwdKinPlugin() [1/2]

factory addFwdKinPlugin	(	"manipulator"	,
		"default"	,
		pi2
	)

addFwdKinPlugin() [2/2]

factory addFwdKinPlugin	(	"manipulator"	,
		"KDLFwdKin"	,
		pi
	)

addInvKinPlugin() [1/2]

factory addInvKinPlugin	(	"manipulator"	,
		"default"	,
		pi2
	)

addInvKinPlugin() [2/2]

factory addInvKinPlugin	(	"manipulator"	,
		"KDLInvKin"	,
		pi
	)

addSearchLibrary()

factory addSearchLibrary

(

"tesseract_collision"

)

addSearchPath()

factory addSearchPath

(

"/usr/local/lib"

)

EXPECT_ANY_THROW()

EXPECT_ANY_THROW

(

KinematicsPluginFactory

factoryconfig

)

EXPECT_EQ() [1/17]

EXPECT_EQ	(	factory.	getDefaultFwdKinPlugin"manipulator",
		"default"
	)

EXPECT_EQ() [2/17]

EXPECT_EQ	(	factory.	getDefaultFwdKinPlugin"manipulator",
		"KDLFwdKin"
	)

EXPECT_EQ() [3/17]

EXPECT_EQ	(	factory.	getDefaultInvKinPlugin"manipulator",
		"KDLInvKin"
	)

EXPECT_EQ() [4/17]

EXPECT_EQ	(	factory.	getFwdKinPlugins).size(,
		0
	)

EXPECT_EQ() [5/17]

EXPECT_EQ	(	factory.	getSearchLibraries).size(,
		4
	)

EXPECT_EQ() [6/17]

EXPECT_EQ	(	factory.	getSearchLibraries).size(,
		5
	)

EXPECT_EQ() [7/17]

EXPECT_EQ	(	factory.	getSearchPaths).size(,
		1
	)

EXPECT_EQ() [8/17]

EXPECT_EQ	(	factory.	getSearchPaths).size(,
		2
	)

EXPECT_EQ() [9/17]

EXPECT_EQ	(	fwd_kin->	getSolverName(),
		"KDLFwdKinChain"
	)

EXPECT_EQ() [10/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"KDLInvKinChainLMA"
	)

EXPECT_EQ() [11/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"KDLInvKinChainNR"
	)

EXPECT_EQ() [12/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"OPWInvKin"
	)

EXPECT_EQ() [13/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"REPInvKin"
	)

EXPECT_EQ() [14/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"ROPInvKin"
	)

EXPECT_EQ() [15/17]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		"URInvKin"
	)

EXPECT_EQ() [16/17]

EXPECT_EQ	(	map.	find"manipulator") ->second.plugins.size(,
		1
	)

EXPECT_EQ() [17/17]

EXPECT_EQ	(	map.	find"manipulator") ->second.plugins.size(,
		2
	)

EXPECT_FALSE()

EXPECT_FALSE

(

factory.

getSearchPaths).empty(

)

EXPECT_TRUE() [1/9]

EXPECT_TRUE

(

fwd_kin !

= nullptr

)

EXPECT_TRUE() [2/9]

EXPECT_TRUE

(

inv_kin !

= nullptr

)

EXPECT_TRUE() [3/9]

EXPECT_TRUE

(

inv_kin

= =nullptr

)

EXPECT_TRUE() [4/9]

EXPECT_TRUE

(

kin

= =nullptr

)

EXPECT_TRUE() [5/9]

EXPECT_TRUE

(

map.at("manipulator").plugins.find("default") !

= map.at("manipulator").plugins.end()

)

EXPECT_TRUE() [6/9]

EXPECT_TRUE

(

map.at("manipulator").plugins.find("KDLFwdKin") !

= map.at("manipulator").plugins.end()

)

EXPECT_TRUE() [7/9]

EXPECT_TRUE

(

map.at("manipulator").plugins.find("KDLInvKin") !

= map.at("manipulator").plugins.end()

)

EXPECT_TRUE() [8/9]

EXPECT_TRUE

(

map.find("manipulator") !

= map.end()

)

EXPECT_TRUE() [9/9]

EXPECT_TRUE

(

map.

find"manipulator" = =map.end()

)

Initial value:

{
    std::map<std::string, tesseract_common::PluginInfoContainer> map = factory.getInvKinPlugins()

factory() [1/2]

KinematicsPluginFactory factory

(

YAML::Load(yaml_model_string)

)

factory() [2/2]

KinematicsPluginFactory factory

(

YAML::Load(yaml_string)

)

main()

int main	(	int	argc,
		char **	argv
	)

push_back()

plugin["config"]["params"]["offsets"] push_back

(

0

)

remove() [1/28]

plugin["config"]["params"] remove

(

"a1"

)

remove() [2/28]

plugin["config"]["params"] remove

(

"a2"

)

remove() [3/28]

plugin["config"]["params"] remove

(

"a3"

)

remove() [4/28]

plugin["config"]["params"] remove

(

"b"

)

remove() [5/28]

plugin["config"] remove

(

"base_link"

)

remove() [6/28]

plugin["config"]["params"] remove

(

"c1"

)

remove() [7/28]

plugin["config"]["params"] remove

(

"c2"

)

remove() [8/28]

plugin["config"]["params"] remove

(

"c3"

)

remove() [9/28]

plugin["config"]["params"] remove

(

"c4"

)

remove() [10/28]

plugin remove

(

"class"

)

remove() [11/28]

plugin remove

(

"config"

)

remove() [12/28]

plugin["config"]["params"] remove

(

"d1"

)

remove() [13/28]

plugin["config"]["params"] remove

(

"d4"

)

remove() [14/28]

plugin["config"]["params"] remove

(

"d5"

)

remove() [15/28]

plugin["config"]["params"] remove

(

"d6"

)

remove() [16/28]

plugin remove

(

"default"

)

remove() [17/28]

plugin["config"] remove

(

"manipulator"

)

remove() [18/28]

plugin["config"] remove

(

"manipulator_reach"

)

remove() [19/28]

plugin["config"] remove

(

"model"

)

remove() [20/28]

plugin["config"]["positioner_sample_resolution"][0] remove

(

"name"

)

remove() [21/28]

plugin["config"]["params"] remove

(

"offset"

)

remove() [22/28]

plugin remove

(

"OPWInvKin"

)

remove() [23/28]

plugin["config"] remove

(

"params"

)

remove() [24/28]

plugin["config"] remove

(

"positioner"

)

remove() [25/28]

plugin["config"] remove

(

"positioner_sample_resolution"

)

remove() [26/28]

plugin["config"]["params"] remove

(

"sign_corrections"

)

remove() [27/28]

plugin["config"] remove

(

"tip_link"

)

remove() [28/28]

plugin["config"]["positioner_sample_resolution"][0] remove

(

"value"

)

removeFwdKinPlugin()

factory removeFwdKinPlugin	(	"manipulator"	,
		"default"
	)

removeInvKinPlugin()

factory removeInvKinPlugin	(	"manipulator"	,
		"default"
	)

runKinematicsFactoryTest() [1/3]

runKinematicsFactoryTest

(

config_path

)

runKinematicsFactoryTest() [2/3]

void runKinematicsFactoryTest

(

const tesseract_common::fs::path &

config_path

)

runKinematicsFactoryTest() [3/3]

runKinematicsFactoryTest

(

export_config_path

)

setDefaultFwdKinPlugin()

factory setDefaultFwdKinPlugin	(	"manipulator"	,
		"default"
	)

setDefaultInvKinPlugin()

factory setDefaultInvKinPlugin	(	"manipulator"	,
		"default"
	)

Variable Documentation

class_name

pi2 class_name = "KDLFwdKin"

config

config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR10e"

config_path

tesseract_common::fs::path config_path = file_path.parent_path() / "kinematic_plugins.yaml"

export_config_path

tesseract_common::fs::path export_config_path

Initial value:

= tesseract_common::fs::path(tesseract_common::getTempPath()) / "kinema"
                                                                                                                "tic_"
                                                                                                                "plugin"
                                                                                                                "s_"
                                                                                                                "export"
                                                                                                                ".yaml"

factory

KinematicsPluginFactory factory(config)

(

config

)

fwd_kin

auto fwd_kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state)

inv_kin

auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state)

kin

auto kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state)

map

map = factory.getFwdKinPlugins()

pi

tesseract_common::PluginInfo pi

pi2

tesseract_common::PluginInfo pi2

plugin

auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]

scene_graph

tesseract_scene_graph::KDLStateSolver state_solver* scene_graph = getSceneGraphABB()

scene_state

tesseract_scene_graph::SceneState scene_state = state_solver.getState()

yaml_model_string

std::string yaml_model_string

Initial value:

=
      R"(kinematic_plugins:
           inv_kin_plugins:
             manipulator:
               default: URInvKin
               plugins:
                 URInvKin:
                   class: URInvKinFactory
                   config:
                     base_link: base_link
                     tip_link: tool0
                     model: UR10)"

yaml_params_string

std::string yaml_params_string

Initial value:

=
      R"(kinematic_plugins:
           inv_kin_plugins:
             manipulator:
               default: URInvKin
               plugins:
                 URInvKin:
                   class: URInvKinFactory
                   config:
                     base_link: base_link
                     tip_link: tool0
                     params:
                       d1: 0.1273
                       a2: -0.612
                       a3: -0.5723
                       d4: 0.163941
                       d5: 0.1157
                       d6: 0.0922)"

yaml_string

std::string yaml_string

Initial value:

=
      R"(kinematic_plugins:
           inv_kin_plugins:
             manipulator:
               default: OPWInvKin
               plugins:
                 OPWInvKin:
                   class: OPWInvKinFactory
                   config:
                     base_link: base_link
                     tip_link: tool0
                     params:
                       a1: 0.100
                       a2: -0.135
                       b: 0.00
                       c1: 0.615
                       c2: 0.705
                       c3: 0.755
                       c4: 0.086
                       offsets: [0, 0, -1.57079632679, 0, 0, 0]
                       sign_corrections: [1, 1, 1, -1, 1, 1])"