kinematics_core_unit.cpp File Reference

#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
#include <tesseract_kinematics/core/utils.h>
#include "kinematics_test_utils.h"

Include dependency graph for kinematics_core_unit.cpp:

Functions
	EXPECT_NEAR (q[0], M_PI_4, 1e-6)
	EXPECT_NEAR (q[1], -M_PI_4, 1e-6)
	EXPECT_NEAR (q[0], -3 *M_PI_4, 1e-6)
	EXPECT_NEAR (q[1], 3 *M_PI_4, 1e-6)
template<typename FloatType >
void	runRedundantSolutionsTest ()
	runRedundantSolutionsTest< double > ()
	EXPECT_TRUE (tesseract_kinematics::isNearSingularity(jacobian, 0.001))
	EXPECT_TRUE (tesseract_kinematics::isNearSingularity(jacobian))
	EXPECT_FALSE (tesseract_kinematics::isNearSingularity(jacobian))
	EXPECT_TRUE (tesseract_kinematics::isNearSingularity(jacobian, 0.02))
	EXPECT_EQ (m.m.eigen_values.size(), 6)
	EXPECT_NEAR (m.m.volume, 0, 1e-6)
	EXPECT_GT (m.m.condition, 1e+20)
	EXPECT_EQ (m.m_linear.eigen_values.size(), 3)
	EXPECT_NEAR (m.m_linear.eigen_values[0], 0.18153054745434696, 1e-6)
	EXPECT_NEAR (m.m_linear.eigen_values[1], 0.8835999999999999, 1e-6)
	EXPECT_NEAR (m.m_linear.eigen_values[2], 1.960719452545653, 1e-6)
	EXPECT_NEAR (m.m_linear.condition, 10.801044122002406, 1e-6)
	EXPECT_NEAR (m.m_linear.measure, 3.286494199295414, 1e-6)
	EXPECT_NEAR (m.m_linear.volume, 0.5608031457314142, 1e-6)
	EXPECT_EQ (m.m_angular.eigen_values.size(), 3)
	EXPECT_NEAR (m.m_angular.eigen_values[0], 1.0, 1e-6)
	EXPECT_NEAR (m.m_angular.eigen_values[1], 2.0, 1e-6)
	EXPECT_NEAR (m.m_angular.eigen_values[2], 3.0, 1e-6)
	EXPECT_NEAR (m.m_angular.condition, 3.0, 1e-6)
	EXPECT_NEAR (m.m_angular.measure, 1.7320508075688772, 1e-6)
	EXPECT_NEAR (m.m_angular.volume, 2.449489742783178, 1e-6)
	EXPECT_EQ (m.f.eigen_values.size(), 6)
	EXPECT_EQ (m.f_linear.eigen_values.size(), 3)
	EXPECT_NEAR (m.f_linear.eigen_values[0], 0.5100168709509535, 1e-6)
	EXPECT_NEAR (m.f_linear.eigen_values[1], 1.1317338162064283, 1e-6)
	EXPECT_NEAR (m.f_linear.eigen_values[2], 5.508714726106856, 1e-6)
	EXPECT_NEAR (m.f_linear.condition, 10.801044122002406, 1e-6)
	EXPECT_NEAR (m.f_linear.measure, 3.286494199295414, 1e-6)
	EXPECT_NEAR (m.f_linear.volume, 1.783156902045858, 1e-6)
	EXPECT_EQ (m.f_angular.eigen_values.size(), 3)
	EXPECT_NEAR (m.f_angular.eigen_values[0], 0.3333333333333333, 1e-6)
	EXPECT_NEAR (m.f_angular.eigen_values[1], 0.5, 1e-6)
	EXPECT_NEAR (m.f_angular.eigen_values[2], 1.0, 1e-6)
	EXPECT_NEAR (m.f_angular.condition, 3.0, 1e-6)
	EXPECT_NEAR (m.f_angular.measure, 1.7320508075688774, 1e-6)
	EXPECT_NEAR (m.f_angular.volume, 0.408248290463863, 1e-6)
int	main (int argc, char **argv)

Variables
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP static const std::string	FACTORY_NAME = "TestFactory"
	q [0] = (4 * M_PI) + M_PI_4
tesseract_kinematics::KDLFwdKinChain fwd_kin *	scene_graph
Eigen::VectorXd	jv = Eigen::VectorXd::Zero(6)
Eigen::MatrixXd	jacobian = fwd_kin.calcJacobian(jv, "tool0")
tesseract_kinematics::Manipulability	m = tesseract_kinematics::calcManipulability(jacobian)

Function Documentation

EXPECT_EQ() [1/6]

EXPECT_EQ	(	m.f.eigen_values.	size(),
		6
	)

EXPECT_EQ() [2/6]

EXPECT_EQ	(	m.f_angular.eigen_values.	size(),
		3
	)

EXPECT_EQ() [3/6]

EXPECT_EQ	(	m.f_linear.eigen_values.	size(),
		3
	)

EXPECT_EQ() [4/6]

EXPECT_EQ	(	m.m.eigen_values.	size(),
		6
	)

EXPECT_EQ() [5/6]

EXPECT_EQ	(	m.m_angular.eigen_values.	size(),
		3
	)

EXPECT_EQ() [6/6]

EXPECT_EQ	(	m.m_linear.eigen_values.	size(),
		3
	)

EXPECT_FALSE()

EXPECT_FALSE

(

tesseract_kinematics::isNearSingularity(jacobian)

)

EXPECT_GT()

EXPECT_GT	(	m.m.	condition,
		1e+	20
	)

EXPECT_NEAR() [1/29]

EXPECT_NEAR	(	m.f_angular.	condition,
		3.	0,
		1e-	6
	)

EXPECT_NEAR() [2/29]

EXPECT_NEAR	(	m.f_angular.	eigen_values[0],
		0.	3333333333333333,
		1e-	6
	)

EXPECT_NEAR() [3/29]

EXPECT_NEAR	(	m.f_angular.	eigen_values[1],
		0.	5,
		1e-	6
	)

EXPECT_NEAR() [4/29]

EXPECT_NEAR	(	m.f_angular.	eigen_values[2],
		1.	0,
		1e-	6
	)

EXPECT_NEAR() [5/29]

EXPECT_NEAR	(	m.f_angular.	measure,
		1.	7320508075688774,
		1e-	6
	)

EXPECT_NEAR() [6/29]

EXPECT_NEAR	(	m.f_angular.	volume,
		0.	408248290463863,
		1e-	6
	)

EXPECT_NEAR() [7/29]

EXPECT_NEAR	(	m.f_linear.	condition,
		10.	801044122002406,
		1e-	6
	)

EXPECT_NEAR() [8/29]

EXPECT_NEAR	(	m.f_linear.	eigen_values[0],
		0.	5100168709509535,
		1e-	6
	)

EXPECT_NEAR() [9/29]

EXPECT_NEAR	(	m.f_linear.	eigen_values[1],
		1.	1317338162064283,
		1e-	6
	)

EXPECT_NEAR() [10/29]

EXPECT_NEAR	(	m.f_linear.	eigen_values[2],
		5.	508714726106856,
		1e-	6
	)

EXPECT_NEAR() [11/29]

EXPECT_NEAR	(	m.f_linear.	measure,
		3.	286494199295414,
		1e-	6
	)

EXPECT_NEAR() [12/29]

EXPECT_NEAR	(	m.f_linear.	volume,
		1.	783156902045858,
		1e-	6
	)

EXPECT_NEAR() [13/29]

EXPECT_NEAR	(	m.m.	volume,
		0	,
		1e-	6
	)

EXPECT_NEAR() [14/29]

EXPECT_NEAR	(	m.m_angular.	condition,
		3.	0,
		1e-	6
	)

EXPECT_NEAR() [15/29]

EXPECT_NEAR	(	m.m_angular.	eigen_values[0],
		1.	0,
		1e-	6
	)

EXPECT_NEAR() [16/29]

EXPECT_NEAR	(	m.m_angular.	eigen_values[1],
		2.	0,
		1e-	6
	)

EXPECT_NEAR() [17/29]

EXPECT_NEAR	(	m.m_angular.	eigen_values[2],
		3.	0,
		1e-	6
	)

EXPECT_NEAR() [18/29]

EXPECT_NEAR	(	m.m_angular.	measure,
		1.	7320508075688772,
		1e-	6
	)

EXPECT_NEAR() [19/29]

EXPECT_NEAR	(	m.m_angular.	volume,
		2.	449489742783178,
		1e-	6
	)

EXPECT_NEAR() [20/29]

EXPECT_NEAR	(	m.m_linear.	condition,
		10.	801044122002406,
		1e-	6
	)

EXPECT_NEAR() [21/29]

EXPECT_NEAR	(	m.m_linear.	eigen_values[0],
		0.	18153054745434696,
		1e-	6
	)

EXPECT_NEAR() [22/29]

EXPECT_NEAR	(	m.m_linear.	eigen_values[1],
		0.	8835999999999999,
		1e-	6
	)

EXPECT_NEAR() [23/29]

EXPECT_NEAR	(	m.m_linear.	eigen_values[2],
		1.	960719452545653,
		1e-	6
	)

EXPECT_NEAR() [24/29]

EXPECT_NEAR	(	m.m_linear.	measure,
		3.	286494199295414,
		1e-	6
	)

EXPECT_NEAR() [25/29]

EXPECT_NEAR	(	m.m_linear.	volume,
		0.	5608031457314142,
		1e-	6
	)

EXPECT_NEAR() [26/29]

EXPECT_NEAR	(	q	[0],
		-3 *	M_PI_4,
		1e-	6
	)

EXPECT_NEAR() [27/29]

EXPECT_NEAR	(	q	[0],
		M_PI_4	,
		1e-	6
	)

EXPECT_NEAR() [28/29]

EXPECT_NEAR	(	q	[1],
		-	M_PI_4,
		1e-	6
	)

EXPECT_NEAR() [29/29]

EXPECT_NEAR	(	q	[1],
		3 *	M_PI_4,
		1e-	6
	)

EXPECT_TRUE() [1/3]

EXPECT_TRUE

(

tesseract_kinematics::isNearSingularity(jacobian)

)

EXPECT_TRUE() [2/3]

EXPECT_TRUE

(

tesseract_kinematics::isNearSingularity(jacobian, 0.001)

)

EXPECT_TRUE() [3/3]

EXPECT_TRUE

(

tesseract_kinematics::isNearSingularity(jacobian, 0.02)

)

main()

int main	(	int	argc,
		char **	argv
	)

runRedundantSolutionsTest()

template<typename FloatType >

void runRedundantSolutionsTest

(

)

runRedundantSolutionsTest< double >()

runRedundantSolutionsTest< double >

(

)

Variable Documentation

FACTORY_NAME

TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP static const std::string FACTORY_NAME = "TestFactory"

static

jacobian

Eigen::MatrixXd jacobian = fwd_kin.calcJacobian(jv, "tool0")

jv

Eigen::VectorXd jv = Eigen::VectorXd::Zero(6)

m

Collision m = tesseract_kinematics::calcManipulability(jacobian)

q

q[1] = (4 * M_PI) + M_PI_4

scene_graph

tesseract_kinematics::KDLFwdKinChain fwd_kin* scene_graph

Initial value:

{
  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB()