52 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
56 status_initial = servo_test_instance_->getStatus();
60 status_curr = servo_test_instance_->getStatus();
64 status_next = servo_test_instance_->getStatus();
69 constexpr double tol = 0.00001;
70 ASSERT_NEAR(delta, 0.02, tol);
78 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
81 status_initial = servo_test_instance_->getStatus();
84 status_curr = servo_test_instance_->getStatus();
88 status_next = servo_test_instance_->getStatus();
92 constexpr double expected_delta = -0.001693;
94 constexpr double tol = 0.00001;
95 ASSERT_NEAR(delta, expected_delta, tol);
103 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
106 status_initial = servo_test_instance_->getStatus();
109 status_curr = servo_test_instance_->getStatus();
113 status_next = servo_test_instance_->getStatus();
117 constexpr double expected_delta = 0.001693;
119 constexpr double tol = 0.00001;
120 ASSERT_NEAR(delta, expected_delta, tol);
128 zero_pose.
pose = getCurrentPose(
"panda_link8");
130 non_zero_pose.
frame_id =
"panda_link0";
131 non_zero_pose.
pose = getCurrentPose(
"panda_link8");
132 non_zero_pose.
pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()));
135 status_initial = servo_test_instance_->getStatus();
138 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
140 status_curr = servo_test_instance_->getStatus();
144 status_next = servo_test_instance_->getStatus();
148 constexpr double expected_delta = 0.057420;
150 constexpr double tol = 0.00001;
151 ASSERT_NEAR(delta, expected_delta, tol);
158 rclcpp::init(argc, argv);
159 ::testing::InitGoogleTest(&argc, argv);
160 int result = RUN_ALL_TESTS();
Eigen::VectorXd positions
TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
Continuous self collision checks are not supported yet by the bullet integration.
int main(int argc, char **argv)