#include <gtest/gtest.h>
#include <map>
#include <math.h>
#include <string>
#include <vector>
#include <Eigen/Geometry>
#include <kdl/frames.hpp>
#include <kdl/path_roundedcomposite.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory.h>
#include <pilz_industrial_motion_planner/cartesian_trajectory_point.h>
#include <pilz_industrial_motion_planner/limits_container.h>
#include <pilz_industrial_motion_planner/trajectory_functions.h>
#include "test_utils.h"

Include dependency graph for unittest_trajectory_functions.cpp:

Classes
class	TrajectoryFunctionsTestBase
	test fixtures base class More...

class	TrajectoryFunctionsTestFlangeAndGripper
	Parametrized class for tests with and without gripper. More...

Macros
#define	_USE_MATH_DEFINES

Functions
const std::string	PARAM_PLANNING_GROUP_NAME ("planning_group")

const std::string	GROUP_TIP_LINK_NAME ("group_tip_link")

const std::string	ROBOT_TCP_LINK_NAME ("tcp_link")

const std::string	IK_FAST_LINK_NAME ("ik_fast_link")

const std::string	RANDOM_TEST_NUMBER ("random_test_number")

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
	Test the forward kinematics function with simple robot poses for robot tip link using robot model without gripper. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
	Test the inverse kinematics directly through ikfast solver. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
	Test the inverse kinematics using RobotState class (setFromIK) using robot model. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityCollisionObject)

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedCollisionObject)

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentitySubframe)

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedSubframe)

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
	Test the wrapper function to compute inverse kinematics using robot model. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
	Test computePoseIK for invalid group_name. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
	Test computePoseIK for invalid link_name. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
	Test computePoseIK for invalid frame_id. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
	Test if self collision is considered by using a pose that always has self collision. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
	Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
	Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
	Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
	Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
	Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
	Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero. More...

	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero. More...

int	main (int argc, char **argv)

Macro Definition Documentation

◆ _USE_MATH_DEFINES

#define _USE_MATH_DEFINES

Definition at line 62 of file unittest_trajectory_functions.cpp.

Function Documentation

◆ GROUP_TIP_LINK_NAME()

const std::string GROUP_TIP_LINK_NAME ( "group_tip_link" )

◆ IK_FAST_LINK_NAME()

const std::string IK_FAST_LINK_NAME ( "ik_fast_link" )

◆ main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 1156 of file unittest_trajectory_functions.cpp.

◆ PARAM_PLANNING_GROUP_NAME()

const std::string PARAM_PLANNING_GROUP_NAME ( "planning_group" )

◆ RANDOM_TEST_NUMBER()

const std::string RANDOM_TEST_NUMBER ( "random_test_number" )

◆ ROBOT_TCP_LINK_NAME()

const std::string ROBOT_TCP_LINK_NAME ( "tcp_link" )

◆ TEST_F() [1/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIK
	)

Test the wrapper function to compute inverse kinematics using robot model.

Definition at line 516 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [2/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidFrameId
	)

Test computePoseIK for invalid frame_id.

Currently only robot_model_->getModelFrame() == frame_id

Definition at line 598 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [3/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidGroupName
	)

Test computePoseIK for invalid group_name.

Definition at line 564 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [4/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidLinkName
	)

Test computePoseIK for invalid link_name.

Definition at line 580 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [5/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKSelfCollisionForInvalidPose
	)

Test if self collision is considered by using a pose that always has self collision.

Definition at line 687 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [6/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeCorrectSamplingTime
	)

Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct.

Test Sequence:

Call function with trajectories which do NOT violate sampling time.

Expected Results:

Function returns 'true'.

Definition at line 951 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [7/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeInvalidVectorSize
	)

Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size.

Test Sequence:

Call function with vectors of incorrect size.

Expected Results:

Function returns 'false'.

Definition at line 923 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [8/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testDetermineAndCheckSamplingTimeViolateSamplingTime
	)

Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated.

Test Sequence:

Call function with trajectories which violate sampling time.

Expected Results:

Function returns 'false'.

Definition at line 985 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [9/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testGenerateJointTrajectoryWithInvalidCartesianTrajectory
	)

Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory.

Please note: Both function variants are tested in this test.

Test Sequence:

Call function with a cartesian trajectory which cannot be transformed into a joint trajectory by using an invalid group_name.

Expected Results:

Function returns 'false'.

Definition at line 874 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [10/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotState
	)

Test the inverse kinematics using RobotState class (setFromIK) using robot model.

Definition at line 344 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [11/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithIdentityCollisionObject
	)

Definition at line 398 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [12/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithIdentitySubframe
	)

Definition at line 452 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [13/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithTransformedCollisionObject
	)

Definition at line 424 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [14/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotStateWithTransformedSubframe
	)

Definition at line 479 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [15/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKSolver
	)

Test the inverse kinematics directly through ikfast solver.

Definition at line 285 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [16/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualAccelerationUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal.

Test Sequence:

Call function with robot states with different acceleration.

Expected Results:

Function returns 'false'.

Definition at line 1081 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [17/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualPositionUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal.

Test Sequence:

Call function with robot states with different positions.

Expected Results:

Function returns 'false'.

Definition at line 1024 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [18/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateEqualVelocityUnequal
	)

Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal.

Test Sequence:

Call function with robot states with different velocities.

Expected Results:

Function returns 'false'.

Definition at line 1050 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [19/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateStationaryAccelerationUnequal
	)

Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero.

Test Sequence:

Call function with robot state with joint acceleration != 0.

Expected Results:

Function returns 'false'.

Definition at line 1140 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [20/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIsRobotStateStationaryVelocityUnequal
	)

Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero.

Test Sequence:

Call function with robot state with joint velocities != 0.

Expected Results:

Function returns 'false'.

Definition at line 1117 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [21/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsAccelerationViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation.

Test Sequence:

Call function with a acceleration violation.

Expected Results:

Function returns 'false'.

Definition at line 784 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [22/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsDecelerationViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation.

Test Sequence:

Call function with a deceleration violation.

Expected Results:

Function returns 'false'.

Definition at line 827 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [23/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsVelocityViolation
	)

Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation.

Test Sequence:

Call function with a velocity violation.

Expected Results:

Function returns 'false'.

Definition at line 751 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [24/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testVerifySampleJointLimitsWithSmallDuration
	)

Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration.

Test Sequence:

Call function with very small sample duration.

Expected Results:

Function returns 'false'.

Definition at line 729 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [25/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		TipLinkFK
	)

Test the forward kinematics function with simple robot poses for robot tip link using robot model without gripper.

Definition at line 255 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

Classes

Macros

Functions

Macro Definition Documentation

◆ _USE_MATH_DEFINES

Function Documentation

◆ GROUP_TIP_LINK_NAME()

◆ IK_FAST_LINK_NAME()

◆ main()

◆ PARAM_PLANNING_GROUP_NAME()

◆ RANDOM_TEST_NUMBER()

◆ ROBOT_TCP_LINK_NAME()

◆ TEST_F() [1/25]

◆ TEST_F() [2/25]

◆ TEST_F() [3/25]

◆ TEST_F() [4/25]

◆ TEST_F() [5/25]

◆ TEST_F() [6/25]

◆ TEST_F() [7/25]

◆ TEST_F() [8/25]

◆ TEST_F() [9/25]

◆ TEST_F() [10/25]

◆ TEST_F() [11/25]

◆ TEST_F() [12/25]

◆ TEST_F() [13/25]

◆ TEST_F() [14/25]

◆ TEST_F() [15/25]

◆ TEST_F() [16/25]

◆ TEST_F() [17/25]

◆ TEST_F() [18/25]

◆ TEST_F() [19/25]

◆ TEST_F() [20/25]

◆ TEST_F() [21/25]

◆ TEST_F() [22/25]

◆ TEST_F() [23/25]

◆ TEST_F() [24/25]

◆ TEST_F() [25/25]