moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
Classes | Functions | Variables
test_ompl_constraints.cpp File Reference
#include "load_test_robot.h"
#include <memory>
#include <string>
#include <iostream>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/utils/robot_model_test_utils.h>
#include <moveit/ompl_interface/detail/ompl_constraints.h>
#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

constexpr 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

constexpr 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

constexpr 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.