#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, 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 970 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/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIK
	)

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

Definition at line 330 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [2/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidFrameId
	)

Test computePoseIK for invalid frame_id.

Currently only robot_model_->getModelFrame() == frame_id

Definition at line 412 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [3/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidGroupName
	)

Test computePoseIK for invalid group_name.

Definition at line 378 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [4/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKInvalidLinkName
	)

Test computePoseIK for invalid link_name.

Definition at line 394 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [5/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testComputePoseIKSelfCollisionForInvalidPose
	)

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

Definition at line 501 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [6/21]

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 765 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [7/21]

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 737 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [8/21]

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 799 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [9/21]

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 688 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [10/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKRobotState
	)

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

Definition at line 272 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [11/21]

TEST_F	(	TrajectoryFunctionsTestFlangeAndGripper	,
		testIKSolver
	)

Test the inverse kinematics directly through ikfast solver.

Definition at line 217 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [12/21]

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 895 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [13/21]

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 838 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [14/21]

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 864 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [15/21]

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 954 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [16/21]

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 931 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [17/21]

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 598 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [18/21]

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 641 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [19/21]

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 565 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [20/21]

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 543 of file unittest_trajectory_functions.cpp.

Here is the call graph for this function:

◆ TEST_F() [21/21]

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 187 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/21]

◆ TEST_F() [2/21]

◆ TEST_F() [3/21]

◆ TEST_F() [4/21]

◆ TEST_F() [5/21]

◆ TEST_F() [6/21]

◆ TEST_F() [7/21]

◆ TEST_F() [8/21]

◆ TEST_F() [9/21]

◆ TEST_F() [10/21]

◆ TEST_F() [11/21]

◆ TEST_F() [12/21]

◆ TEST_F() [13/21]

◆ TEST_F() [14/21]

◆ TEST_F() [15/21]

◆ TEST_F() [16/21]

◆ TEST_F() [17/21]

◆ TEST_F() [18/21]

◆ TEST_F() [19/21]

◆ TEST_F() [20/21]

◆ TEST_F() [21/21]