test_ompl_constraints.cpp File Reference

#include "load_test_robot.hpp"
#include <memory>
#include <string>
#include <iostream>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <moveit/robot_model/robot_model.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <moveit/robot_state/conversions.hpp>
#include <moveit/utils/robot_model_test_utils.hpp>
#include <moveit/ompl_interface/detail/ompl_constraints.hpp>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit/utils/logger.hpp>
#include <ompl/util/Exception.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
#include <ompl/base/ConstrainedSpaceInformation.h>

Include dependency graph for test_ompl_constraints.cpp:

Go to the source code of this file.

Classes
class	TestOMPLConstraints
class	PandaConstraintTest
class	FanucConstraintTest
class	PR2LeftArmConstraintTest

Functions
rclcpp::Logger	getLogger ()
moveit_msgs::msg::PositionConstraint	createPositionConstraint (std::string &base_link, std::string &ee_link)
	Helper function to create a specific position constraint.
	TEST_F (PandaConstraintTest, InitPositionConstraint)
	TEST_F (PandaConstraintTest, PositionConstraintJacobian)
	TEST_F (PandaConstraintTest, PositionConstraintOMPLCheck)
	TEST_F (PandaConstraintTest, EqualityPositionConstraints)
	TEST_F (FanucConstraintTest, InitPositionConstraint)
	TEST_F (FanucConstraintTest, PositionConstraintJacobian)
	TEST_F (FanucConstraintTest, PositionConstraintOMPLCheck)
	TEST_F (FanucConstraintTest, EqualityPositionConstraints)
	TEST_F (PR2LeftArmConstraintTest, InitPositionConstraint)
	TEST_F (PR2LeftArmConstraintTest, PositionConstraintJacobian)
	TEST_F (PR2LeftArmConstraintTest, PositionConstraintOMPLCheck)
	TEST_F (PR2LeftArmConstraintTest, EqualityPositionConstraints)
int	main (int argc, char **argv)

Variables
constexpr int	NUM_RANDOM_TESTS = 10
	Number of times to run a test that uses randomly generated input.
constexpr unsigned int	DIFFERENT_LINK_OFFSET = 2
	Select a robot link at (num_dofs - different_link_offset) to test another link than the end-effector.
constexpr double	JAC_ERROR_TOLERANCE = 1e-4
	Allowed error when comparing Jacobian matrix error.

Function Documentation

createPositionConstraint()

moveit_msgs::msg::PositionConstraint createPositionConstraint	(	std::string &	base_link,
		std::string &	ee_link )

Helper function to create a specific position constraint.

Definition at line 85 of file test_ompl_constraints.cpp.

Here is the caller graph for this function:

getLogger()

rclcpp::Logger getLogger

(

)

This file tests the implementation of constraints inheriting from the ompl::base::Constraint class in the file /detail/ompl_constraint.h/cpp. These are used to create an ompl::base::ConstrainedStateSpace to plan with path constraints.

NOTE q = joint positions

Definition at line 66 of file test_ompl_constraints.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

main()

int main	(	int	argc,
		char **	argv )

Definition at line 440 of file test_ompl_constraints.cpp.

TEST_F() [1/12]

TEST_F	(	FanucConstraintTest	,
		EqualityPositionConstraints	)

Definition at line 386 of file test_ompl_constraints.cpp.

TEST_F() [2/12]

TEST_F	(	FanucConstraintTest	,
		InitPositionConstraint	)

Definition at line 359 of file test_ompl_constraints.cpp.

TEST_F() [3/12]

TEST_F	(	FanucConstraintTest	,
		PositionConstraintJacobian	)

Definition at line 365 of file test_ompl_constraints.cpp.

TEST_F() [4/12]

TEST_F	(	FanucConstraintTest	,
		PositionConstraintOMPLCheck	)

Definition at line 375 of file test_ompl_constraints.cpp.

TEST_F() [5/12]

TEST_F	(	PandaConstraintTest	,
		EqualityPositionConstraints	)

Definition at line 342 of file test_ompl_constraints.cpp.

TEST_F() [6/12]

TEST_F	(	PandaConstraintTest	,
		InitPositionConstraint	)

Definition at line 315 of file test_ompl_constraints.cpp.

TEST_F() [7/12]

TEST_F	(	PandaConstraintTest	,
		PositionConstraintJacobian	)

Definition at line 321 of file test_ompl_constraints.cpp.

TEST_F() [8/12]

TEST_F	(	PandaConstraintTest	,
		PositionConstraintOMPLCheck	)

Definition at line 331 of file test_ompl_constraints.cpp.

TEST_F() [9/12]

TEST_F	(	PR2LeftArmConstraintTest	,
		EqualityPositionConstraints	)

Definition at line 430 of file test_ompl_constraints.cpp.

TEST_F() [10/12]

TEST_F	(	PR2LeftArmConstraintTest	,
		InitPositionConstraint	)

Definition at line 404 of file test_ompl_constraints.cpp.

TEST_F() [11/12]

TEST_F	(	PR2LeftArmConstraintTest	,
		PositionConstraintJacobian	)

Definition at line 410 of file test_ompl_constraints.cpp.

TEST_F() [12/12]

TEST_F	(	PR2LeftArmConstraintTest	,
		PositionConstraintOMPLCheck	)

Definition at line 420 of file test_ompl_constraints.cpp.

Variable Documentation

DIFFERENT_LINK_OFFSET

unsigned int DIFFERENT_LINK_OFFSET = 2

constexpr

Select a robot link at (num_dofs - different_link_offset) to test another link than the end-effector.

Definition at line 75 of file test_ompl_constraints.cpp.

JAC_ERROR_TOLERANCE

double JAC_ERROR_TOLERANCE = 1e-4

constexpr

Allowed error when comparing Jacobian matrix error.

High tolerance because of high finite difference error. (And it is the L1-norm over the whole matrix difference.)

Definition at line 82 of file test_ompl_constraints.cpp.

NUM_RANDOM_TESTS

int NUM_RANDOM_TESTS = 10

constexpr

Number of times to run a test that uses randomly generated input.

Definition at line 72 of file test_ompl_constraints.cpp.