#include <cstdint>
#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.hpp>
#include <moveit/robot_model/joint_model_group.hpp>
#include <moveit/robot_model/robot_model.hpp>
#include <moveit/robot_model_loader/robot_model_loader.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory.hpp>
#include <pilz_industrial_motion_planner/cartesian_trajectory_point.hpp>
#include <pilz_industrial_motion_planner/limits_container.hpp>
#include <pilz_industrial_motion_planner/trajectory_functions.hpp>
#include "test_utils.hpp"

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)
	Parametrized class for tests, that only run with a gripper.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
	Test the inverse kinematics directly through ikfast solver.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
	Test the inverse kinematics using RobotState class (setFromIK) using robot model.
	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.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
	Test computePoseIK for invalid group_name.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
	Test computePoseIK for invalid link_name.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
	Test computePoseIK for invalid frame_id.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
	Test if activated self collision for a pose that would be in self collision without the check results in a valid ik solution.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
	Check that function VerifySampleJointLimits() returns 'false' in case of very small sample duration.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a velocity violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a acceleration violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
	Check that function VerifySampleJointLimits() returns 'false' in case of a deceleration violation.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
	Check that function generateJointTrajectory() returns 'false' if a joint trajectory cannot be computed from a cartesian trajectory.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
	Check that function determineAndCheckSamplingTime() returns 'false' if both of the needed vectors have an incorrect vector size.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'true' if sampling time is correct.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
	Check that function determineAndCheckSamplingTime() returns 'false' if sampling time is violated.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
	Check that function isRobotStateEqual() returns 'false' if the positions of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
	Check that function isRobotStateEqual() returns 'false' if the velocity of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
	Check that function isRobotStateEqual() returns 'false' if the acceleration of the robot states are not equal.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint velocities are not equal to zero.
	TEST_F (TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
	Check that function isRobotStateStationary() returns 'false' if the joint acceleration are not equal to zero.
int	main (int argc, char **argv)

Macro Definition Documentation

◆ _USE_MATH_DEFINES

#define _USE_MATH_DEFINES

Definition at line 63 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 1166 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 526 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 608 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 574 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 590 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [5/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKSelfCollisionForInvalidPose	)

Test if activated self collision for a pose that would be in self collision without the check results in a valid ik solution.

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

Definition at line 697 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 961 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 933 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 995 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 884 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 350 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 408 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 462 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 434 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 489 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 287 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 1091 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 1034 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 1060 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 1150 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 1127 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 794 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 837 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 761 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 739 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [25/25]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		TipLinkFK	)

Parametrized class for tests, that only run with a gripper.

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

Definition at line 257 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]