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/kinematics_plugin_factory.h>
#include <tesseract/state_solver/kdl/kdl_state_solver.h>
#include <tesseract/common/resource_locator.h>
#include <tesseract/common/yaml_utils.h>

Functions
void	runKinematicsFactoryTest (const std::filesystem::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")
	EXPECT_ANY_THROW (factory.setDefaultFwdKinPlugin("group_does_not_exist", "default"))
	EXPECT_ANY_THROW (factory.setDefaultFwdKinPlugin("manipulator", "solver_does_not_exist"))
	EXPECT_ANY_THROW (factory.removeFwdKinPlugin("group_does_not_exist", "default"))
	EXPECT_TRUE (factory.createFwdKin("group_does_not_exist", "default", *scene_graph, scene_state)==nullptr)
	EXPECT_TRUE (factory.createFwdKin("manipulator", "solver_does_not_exist", *scene_graph, scene_state)==nullptr)
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")
KinematicsPluginFactory	factory (config, locator)
	EXPECT_TRUE (inv_kin==nullptr)
plugin	remove ("class")
	EXPECT_ANY_THROW (KinematicsPluginFactory factory(config, locator))
plugin	remove ("default")
	EXPECT_TRUE (inv_kin !=nullptr)
factory	addSearchPath (std::string(PLUGIN_DIR))
	EXPECT_EQ (inv_kin->getSolverName(), "AbbIRB2400IKFast")
plugin	remove ("config")
plugin["config"]	remove ("base_link")
plugin["config"]	remove ("tip_link")
KinematicsPluginFactory	factory (yaml_string, locator)
	EXPECT_EQ (inv_kin->getSolverName(), "OPWInvKin")
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_model_string, locator)
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_EQ (inv_kin->getSolverName(), "KDLInvKinChainNR_JL")
	EXPECT_TRUE (kin==nullptr)
int	main (int argc, char **argv)

Variables
std::filesystem::path	config_path = file_path.parent_path() / "kinematic_plugins.yaml"
std::filesystem::path	export_config_path
tesseract::scene_graph::SceneGraph::UPtr	scene_graph = getSceneGraphABB(locator)
tesseract::scene_graph::SceneState	scene_state = state_solver.getState()
KinematicsPluginFactory	factory
tesseract::common::PluginInfo	pi
pi	class_name = "KDLFwdKin"
	map = factory.getFwdKinPlugins()
tesseract::common::PluginInfo	pi2
tesseract::common::GeneralResourceLocator	locator
std::string	yaml_string
auto	plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]
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

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

EXPECT_TRUE()

EXPECT_TRUE

(

map.

find"manipulator" = =map.end()

)

Initial value:

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

addSearchPath()

factory addSearchPath

(

std::string(PLUGIN_DIR)

)

Initial value:

{
    KinematicsPluginFactory factory(yaml_string, locator)

Variable Documentation

export_config_path

std::filesystem::path export_config_path

Initial value:

= std::filesystem::path(tesseract::common::getTempPath()) / "kinema"
                                                                                                       "tic_"
                                                                                                       "plugin"
                                                                                                       "s_"
                                                                                                       "export"
                                                                                                       ".yaml"

locator

tesseract::common::GeneralResourceLocator locator

Initial value:

{
  using namespace tesseract::scene_graph

Todo:

Add test to test IKFast with free joints

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

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