#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <fstream>
#include <tesseract_urdf/urdf_parser.h>
#include "kinematics_test_utils.h"
#include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
#include <tesseract_kinematics/core/rep_inv_kin.h>
#include <tesseract_kinematics/opw/opw_inv_kin.h>
#include <tesseract_kinematics/core/utils.h>
#include <tesseract_kinematics/core/kinematic_group.h>
#include <tesseract_state_solver/kdl/kdl_state_solver.h>
#include <opw_kinematics/opw_parameters.h>

Include dependency graph for rep_kinematics_unit.cpp:

Functions
opw_kinematics::Parameters< double >	getOPWKinematicsParamABB ()

ForwardKinematics::UPtr	getRobotFwdKinematics (const tesseract_scene_graph::SceneGraph &scene_graph)

ForwardKinematics::UPtr	getFullFwdKinematics (const tesseract_scene_graph::SceneGraph &scene_graph)

ForwardKinematics::UPtr	getPositionerFwdKinematics (const tesseract_scene_graph::SceneGraph &scene_graph)

InverseKinematics::UPtr	getFullInvKinematics (const tesseract_scene_graph::SceneGraph &scene_graph)

	EXPECT_TRUE (tesseract_common::isIdentical(fwd_joint_names, inv_joint_names, false))

pose	setIdentity ()

pose	translation ()[0]=0

	EXPECT_TRUE (inv_kin !=nullptr)

	EXPECT_EQ (inv_kin->getSolverName(), DEFAULT_REP_INV_KIN_SOLVER_NAME)

	EXPECT_EQ (inv_kin->numJoints(), 8)

	EXPECT_EQ (inv_kin->getBaseLinkName(), base_link_name)

	EXPECT_EQ (inv_kin->getWorkingFrame(), working_frame)

	EXPECT_EQ (inv_kin->getTipLinkNames().size(), 1)

	EXPECT_EQ (inv_kin->getTipLinkNames()[0], tip_link_name)

	EXPECT_EQ (inv_kin->getJointNames(), joint_names)

KinematicGroup	kin_group (manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state)

	EXPECT_EQ (kin_group.getBaseLinkName(), scene_graph->getRoot())

	runInvKinTest (kin_group, pose, working_frame, tip_link_name, seed)

	runKinGroupJacobianABBExternalPositionerTest (kin_group)

	runActiveLinkNamesABBExternalPositionerTest (kin_group)

	runKinJointLimitsTest (kin_group.getLimits(), target_limits)

	runKinSetJointLimitsTest (kin_group)

	EXPECT_EQ (kin_group.getName(), manip_name)

	EXPECT_EQ (kin_group.getJointNames(), joint_names)

	EXPECT_EQ (kin_group.getAllPossibleTipLinkNames().size(), 1)

	EXPECT_EQ (kin_group.getAllPossibleTipLinkNames()[0], tip_link_name)

	EXPECT_EQ (kin_group.getAllValidWorkingFrames()[0], working_frame)

	EXPECT_EQ (kin_group_copy.getBaseLinkName(), scene_graph->getRoot())

	runInvKinTest (kin_group_copy, pose, working_frame, tip_link_name, seed)

	runKinGroupJacobianABBExternalPositionerTest (kin_group_copy)

	runActiveLinkNamesABBExternalPositionerTest (kin_group_copy)

	runKinJointLimitsTest (kin_group_copy.getLimits(), target_limits)

	runKinSetJointLimitsTest (kin_group_copy)

	EXPECT_EQ (kin_group_copy.getName(), manip_name)

	EXPECT_EQ (kin_group_copy.getJointNames(), joint_names)

	EXPECT_EQ (kin_group_copy.getAllPossibleTipLinkNames().size(), 1)

	EXPECT_EQ (kin_group_copy.getAllPossibleTipLinkNames()[0], tip_link_name)

	EXPECT_EQ (kin_group_copy.getAllValidWorkingFrames()[0], working_frame)

	EXPECT_TRUE (inv_kin2 !=nullptr)

	EXPECT_EQ (inv_kin2->getSolverName(), DEFAULT_REP_INV_KIN_SOLVER_NAME)

	EXPECT_EQ (inv_kin2->numJoints(), 8)

	EXPECT_EQ (inv_kin2->getBaseLinkName(), base_link_name)

	EXPECT_EQ (inv_kin2->getWorkingFrame(), working_frame)

	EXPECT_EQ (inv_kin2->getTipLinkNames().size(), 1)

	EXPECT_EQ (inv_kin2->getTipLinkNames()[0], tip_link_name)

	EXPECT_EQ (inv_kin2->getJointNames(), joint_names)

KinematicGroup	kin_group2 (manip_name, joint_names, std::move(inv_kin2), *scene_graph, scene_state)

	EXPECT_EQ (kin_group2.getBaseLinkName(), scene_graph->getRoot())

	runInvKinTest (kin_group2, pose, working_frame, tip_link_name, seed)

	runKinGroupJacobianABBExternalPositionerTest (kin_group2)

	runActiveLinkNamesABBExternalPositionerTest (kin_group2)

	runKinJointLimitsTest (kin_group2.getLimits(), target_limits)

	runKinSetJointLimitsTest (kin_group2)

int	main (int argc, char **argv)

Variables
tesseract_scene_graph::KDLStateSolver state_solver *	scene_graph

tesseract_scene_graph::SceneState	scene_state = state_solver.getState()

std::string	manip_name = "robot_external_positioner"

std::string	base_link_name = "base_link"

std::string	working_frame = "positioner_tool0"

std::string	tip_link_name = "tool0"

std::vector< std::string >	joint_names

tesseract_common::KinematicLimits	target_limits = getTargetLimits(*scene_graph, joint_names)

auto	fwd_kin = getFullFwdKinematics(*scene_graph)

auto	inv_kin = getFullInvKinematics(*scene_graph)

auto	inv_kin2 = inv_kin->clone()

std::vector< std::string >	fwd_joint_names = fwd_kin->getJointNames()

std::vector< std::string >	inv_joint_names = inv_kin->getJointNames()

Eigen::Isometry3d	pose

Eigen::VectorXd	seed = Eigen::VectorXd::Zero(fwd_kin->numJoints())

KinematicGroup	kin_group_copy (kin_group)

Function Documentation

◆ EXPECT_EQ() [1/27]

EXPECT_EQ	(	inv_kin->	getBaseLinkName(),
		base_link_name
	)

◆ EXPECT_EQ() [2/27]

EXPECT_EQ	(	inv_kin->	getJointNames(),
		joint_names
	)

◆ EXPECT_EQ() [3/27]

EXPECT_EQ	(	inv_kin->	getSolverName(),
		DEFAULT_REP_INV_KIN_SOLVER_NAME
	)

◆ EXPECT_EQ() [4/27]

EXPECT_EQ	(	inv_kin->	getTipLinkNames).size(,
		1
	)

◆ EXPECT_EQ() [5/27]

EXPECT_EQ	(	inv_kin->	getTipLinkNames()[0],
		tip_link_name
	)

◆ EXPECT_EQ() [6/27]

EXPECT_EQ	(	inv_kin->	getWorkingFrame(),
		working_frame
	)

◆ EXPECT_EQ() [7/27]

EXPECT_EQ	(	inv_kin->	numJoints(),
		8
	)

◆ EXPECT_EQ() [8/27]

EXPECT_EQ	(	inv_kin2->	getBaseLinkName(),
		base_link_name
	)

◆ EXPECT_EQ() [9/27]

EXPECT_EQ	(	inv_kin2->	getJointNames(),
		joint_names
	)

◆ EXPECT_EQ() [10/27]

EXPECT_EQ	(	inv_kin2->	getSolverName(),
		DEFAULT_REP_INV_KIN_SOLVER_NAME
	)

◆ EXPECT_EQ() [11/27]

EXPECT_EQ	(	inv_kin2->	getTipLinkNames).size(,
		1
	)

◆ EXPECT_EQ() [12/27]

EXPECT_EQ	(	inv_kin2->	getTipLinkNames()[0],
		tip_link_name
	)

◆ EXPECT_EQ() [13/27]

EXPECT_EQ	(	inv_kin2->	getWorkingFrame(),
		working_frame
	)

◆ EXPECT_EQ() [14/27]

EXPECT_EQ	(	inv_kin2->	numJoints(),
		8
	)

◆ EXPECT_EQ() [15/27]

EXPECT_EQ	(	kin_group.	getAllPossibleTipLinkNames).size(,
		1
	)

◆ EXPECT_EQ() [16/27]

EXPECT_EQ	(	kin_group.	getAllPossibleTipLinkNames()[0],
		tip_link_name
	)

◆ EXPECT_EQ() [17/27]

EXPECT_EQ	(	kin_group.	getAllValidWorkingFrames()[0],
		working_frame
	)

◆ EXPECT_EQ() [18/27]

EXPECT_EQ	(	kin_group.	getBaseLinkName(),
		scene_graph->	getRoot()
	)

◆ EXPECT_EQ() [19/27]

EXPECT_EQ	(	kin_group.	getJointNames(),
		joint_names
	)

◆ EXPECT_EQ() [20/27]

EXPECT_EQ	(	kin_group.	getName(),
		manip_name
	)

◆ EXPECT_EQ() [21/27]

EXPECT_EQ	(	kin_group2.	getBaseLinkName(),
		scene_graph->	getRoot()
	)

◆ EXPECT_EQ() [22/27]

EXPECT_EQ	(	kin_group_copy.	getAllPossibleTipLinkNames).size(,
		1
	)

◆ EXPECT_EQ() [23/27]

EXPECT_EQ	(	kin_group_copy.	getAllPossibleTipLinkNames()[0],
		tip_link_name
	)

◆ EXPECT_EQ() [24/27]

EXPECT_EQ	(	kin_group_copy.	getAllValidWorkingFrames()[0],
		working_frame
	)

◆ EXPECT_EQ() [25/27]

EXPECT_EQ	(	kin_group_copy.	getBaseLinkName(),
		scene_graph->	getRoot()
	)

◆ EXPECT_EQ() [26/27]

EXPECT_EQ	(	kin_group_copy.	getJointNames(),
		joint_names
	)

◆ EXPECT_EQ() [27/27]

EXPECT_EQ	(	kin_group_copy.	getName(),
		manip_name
	)

◆ EXPECT_TRUE() [1/3]

EXPECT_TRUE ( inv_kin ! = nullptr )

◆ EXPECT_TRUE() [2/3]

EXPECT_TRUE ( inv_kin2 ! = nullptr )

◆ EXPECT_TRUE() [3/3]

EXPECT_TRUE ( tesseract_common::isIdentical(fwd_joint_names, inv_joint_names, false) )

◆ getFullFwdKinematics()

ForwardKinematics::UPtr getFullFwdKinematics ( const tesseract_scene_graph::SceneGraph & scene_graph )

◆ getFullInvKinematics()

InverseKinematics::UPtr getFullInvKinematics ( const tesseract_scene_graph::SceneGraph & scene_graph )

◆ getOPWKinematicsParamABB()

opw_kinematics::Parameters< double > getOPWKinematicsParamABB ( )

inline

◆ getPositionerFwdKinematics()

ForwardKinematics::UPtr getPositionerFwdKinematics ( const tesseract_scene_graph::SceneGraph & scene_graph )

◆ getRobotFwdKinematics()

ForwardKinematics::UPtr getRobotFwdKinematics ( const tesseract_scene_graph::SceneGraph & scene_graph )

◆ kin_group()

KinematicGroup kin_group	(	manip_name	,
		joint_names	,
		std::move(inv_kin)	,
		*	scene_graph,
		scene_state
	)

◆ kin_group2()

KinematicGroup kin_group2	(	manip_name	,
		joint_names	,
		std::move(inv_kin2)	,
		*	scene_graph,
		scene_state
	)

◆ main()

int main	(	int	argc,
		char **	argv
	)

◆ runActiveLinkNamesABBExternalPositionerTest() [1/3]

runActiveLinkNamesABBExternalPositionerTest ( kin_group )

◆ runActiveLinkNamesABBExternalPositionerTest() [2/3]

runActiveLinkNamesABBExternalPositionerTest ( kin_group2 )

◆ runActiveLinkNamesABBExternalPositionerTest() [3/3]

runActiveLinkNamesABBExternalPositionerTest ( kin_group_copy )

◆ runInvKinTest() [1/3]

runInvKinTest	(	kin_group	,
		pose	,
		working_frame	,
		tip_link_name	,
		seed
	)

◆ runInvKinTest() [2/3]

runInvKinTest	(	kin_group2	,
		pose	,
		working_frame	,
		tip_link_name	,
		seed
	)

◆ runInvKinTest() [3/3]

runInvKinTest	(	kin_group_copy	,
		pose	,
		working_frame	,
		tip_link_name	,
		seed
	)

◆ runKinGroupJacobianABBExternalPositionerTest() [1/3]

runKinGroupJacobianABBExternalPositionerTest ( kin_group )

◆ runKinGroupJacobianABBExternalPositionerTest() [2/3]

runKinGroupJacobianABBExternalPositionerTest ( kin_group2 )

◆ runKinGroupJacobianABBExternalPositionerTest() [3/3]

runKinGroupJacobianABBExternalPositionerTest ( kin_group_copy )

◆ runKinJointLimitsTest() [1/3]

runKinJointLimitsTest	(	kin_group.	getLimits(),
		target_limits
	)

◆ runKinJointLimitsTest() [2/3]

runKinJointLimitsTest	(	kin_group2.	getLimits(),
		target_limits
	)

◆ runKinJointLimitsTest() [3/3]

runKinJointLimitsTest	(	kin_group_copy.	getLimits(),
		target_limits
	)

◆ runKinSetJointLimitsTest() [1/3]

runKinSetJointLimitsTest ( kin_group )

◆ runKinSetJointLimitsTest() [2/3]

runKinSetJointLimitsTest ( kin_group2 )

◆ runKinSetJointLimitsTest() [3/3]

runKinSetJointLimitsTest ( kin_group_copy )

◆ setIdentity()

pose setIdentity ( )

◆ translation()

pose translation ( )

pure virtual

Variable Documentation

◆ base_link_name

std::string base_link_name = "base_link"

◆ fwd_joint_names

std::vector<std::string> fwd_joint_names = fwd_kin->getJointNames()

◆ fwd_kin

auto fwd_kin = getFullFwdKinematics(*scene_graph)

◆ inv_joint_names

std::vector<std::string> inv_joint_names = inv_kin->getJointNames()

◆ inv_kin

auto inv_kin = getFullInvKinematics(*scene_graph)

◆ inv_kin2

auto inv_kin2 = inv_kin->clone()

◆ joint_names

std::vector<std::string> joint_names

Initial value:

{
    "positioner_joint_1", "positioner_joint_2", "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
  }

◆ kin_group_copy

KinematicGroup kin_group_copy(kin_group) ( kin_group )

◆ manip_name

std::string manip_name = "robot_external_positioner"

◆ pose

Eigen::Isometry3d pose

◆ scene_graph

tesseract_scene_graph::KDLStateSolver state_solver* scene_graph