kinematics_factory_unit.cpp File Reference

Tesseract kinematics factory test. More...

#include <tesseract/common/macros.h>
#include <gtest/gtest.h>
#include <stdexcept>
#include "kinematics_test_utils.h"
#include <tesseract/kinematics/kinematics_plugin_factory.h>
#include <tesseract/kinematics/rop_factory.h>
#include <tesseract/kinematics/rep_factory.h>
#include <tesseract/kinematics/kdl/kdl_factories.h>
#include <tesseract/kinematics/opw/opw_factory.h>
#include <tesseract/kinematics/ur/ur_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)
	FAIL ()<< "Expected plugin discovery validation to fail"
	catch (const std::runtime_error &error)
std::filesystem::path	file_path (__FILE__)
	EXPECT_EQ (inv_kin_factory.create("ROPInvKin", *rop_sg, state, factory, YAML::Load(rop_config)), nullptr)
	EXPECT_EQ (inv_kin_factory.create("REPInvKin", *rep_sg, state, factory, YAML::Load(rep_config)), nullptr)
scene_graph	addLink (tesseract::scene_graph::Link("unparented_link"))
	ASSERT_FALSE (scene_graph->isTree())
	EXPECT_EQ (factory.create("KDLFwdKinChain", *scene_graph, scene_state, plugin_factory, config), nullptr)
	EXPECT_EQ (factory.create("KDLInvKinChainLMA", *scene_graph, scene_state, plugin_factory, config), nullptr)
	EXPECT_EQ (factory.create("KDLInvKinChainNR", *scene_graph, scene_state, plugin_factory, config), nullptr)
	EXPECT_EQ (factory.create("KDLInvKinChainNR_JL", *scene_graph, scene_state, plugin_factory, config), nullptr)
	ASSERT_EQ (scene_graph->getShortestPath("base_link", "tool0").active_joints.size(), 7)
	EXPECT_EQ (factory.create("OPWInvKin", *scene_graph, scene_state, plugin_factory, config), nullptr)
	EXPECT_EQ (factory.create("URInvKin", *scene_graph, scene_state, plugin_factory, config), nullptr)
	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_THROW (KinematicsPluginFactory factory(config, locator), std::runtime_error)
plugin	remove ("default")
	EXPECT_TRUE (inv_kin !=nullptr)
	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")
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::common::GeneralResourceLocator	locator
	try
KinematicsPluginFactory	factory (config_path, locator) = kdl_factory
const char *	rop_config
const char *	rep_config
tesseract::scene_graph::KDLStateSolver ss *	rop_sg
auto	state = ss.getState()
const ROPInvKinFactory	rop_factory
const InvKinFactory &	inv_kin_factory = rop_factory
tesseract::scene_graph::KDLStateSolver ss *	rep_sg
const REPInvKinFactory	rep_factory
const char *	manipulator_config
tesseract::scene_graph::SceneGraph::UPtr	scene_graph = getSceneGraphIIWA(locator)
const tesseract::scene_graph::SceneState	scene_state = state_solver.getState()
const KinematicsPluginFactory	plugin_factory
YAML::Node	config
const OPWInvKinFactory	opw_factory
const URInvKinFactory	ur_factory
tesseract::common::PluginInfo	pi
pi	class_name = "KDLFwdKin"
	map = factory.getFwdKinPlugins()
tesseract::common::PluginInfo	pi2
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
auto	fwd_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()

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:

{
  const std::string config = R"(
kinematic_plugins:
  search_paths: /tmp/plugins
  search_libraries: [library_that_does_not_exist]
)"

Todo:

Add test to test IKFast with free joints

try

try

Initial value:

{
    KinematicsPluginFactory factory(config, locator)

rop_config

const std::string rop_config

Initial value:

= R"(
manipulator_reach: 2.0
positioner_sample_resolution:
  - name: joint_1
    value: 0.1
positioner:
  class: KDLFwdKinChainFactory
  config:
    base_link: positioner_base_link
    tip_link: positioner_tool0
manipulator:
  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]
)"

rep_config

const std::string rep_config

Initial value:

= R"(
manipulator_reach: 2.0
positioner_sample_resolution:
  - name: joint_1
    value: 0.1
  - name: joint_2
    value: 0.1
positioner:
  class: KDLFwdKinChainFactory
  config:
    base_link: positioner_base_link
    tip_link: positioner_tool0
manipulator:
  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]
)"

rop_sg

tesseract::scene_graph::KDLStateSolver ss * rop_sg

Initial value:

{
    auto rop_sg = getSceneGraphABBOnPositioner(locator)

rep_sg

tesseract::scene_graph::KDLStateSolver ss * rep_sg

Initial value:

{
    auto rep_sg = getSceneGraphABBExternalPositioner(locator)

manipulator_config

const char* manipulator_config

Initial value:

= R"(
  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]
)"

config

config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]["config"]["manipulator"]["class"]

Initial value:

= YAML::Load(R"(base_link: base_link
tip_link: tool0)")

opw_factory

const OPWInvKinFactory opw_factory

Initial value:

{
    YAML::Node config = YAML::Load(R"(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)")

ur_factory

const URInvKinFactory ur_factory

Initial value:

{
    YAML::Node config = YAML::Load(R"(base_link: base_link
tip_link: tool0
model: UR10)")

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