50 auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
58 status_initial = servo_test_instance_->getStatus();
62 status_curr = servo_test_instance_->getStatus();
66 status_next = servo_test_instance_->getStatus();
71 constexpr double tol = 1.0e-5;
72 ASSERT_NEAR(delta, 0.01, tol);
78 auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
85 status_initial = servo_test_instance_->getStatus();
88 status_curr = servo_test_instance_->getStatus();
92 status_next = servo_test_instance_->getStatus();
96 constexpr double expected_delta = -0.000338;
98 constexpr double tol = 1.0e-5;
99 ASSERT_NEAR(delta, expected_delta, tol);
105 auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
112 status_initial = servo_test_instance_->getStatus();
115 status_curr = servo_test_instance_->getStatus();
119 status_next = servo_test_instance_->getStatus();
123 constexpr double expected_delta = 0.000338;
125 constexpr double tol = 1.0e-5;
126 ASSERT_NEAR(delta, expected_delta, tol);
132 auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
137 zero_pose.
pose = getCurrentPose(
"panda_link8");
139 non_zero_pose.
frame_id =
"panda_link0";
140 non_zero_pose.
pose = getCurrentPose(
"panda_link8");
141 non_zero_pose.
pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()));
144 status_initial = servo_test_instance_->getStatus();
148 status_curr = servo_test_instance_->getStatus();
152 status_next = servo_test_instance_->getStatus();
156 constexpr double expected_delta = 0.003364;
158 constexpr double tol = 1.0e-5;
159 ASSERT_NEAR(delta, expected_delta, tol);
165 auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
167 const auto& joint_bounds = robot_state->getRobotModel()->getVariableBounds(
"panda_joint7");
168 const auto& margins = servo_params_.joint_limit_margins;
169 const double joint_margin = margins.size() == 1 ? margins.front() : margins.at(6);
170 robot_state->setVariablePosition(
"panda_joint7", joint_bounds.max_position_ - joint_margin - 1.0e-4);
171 robot_state->update();
175 pose.
pose = robot_state->getGlobalLinkTransform(
"panda_link8");
176 pose.
pose.translation().x() += 0.01;
177 pose.
pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()));
180 ASSERT_TRUE(servo_params_.halt_all_joints_in_cartesian_mode);
188 for (Eigen::Index index = 0; index < current_state.
positions.size(); ++index)
191 EXPECT_DOUBLE_EQ(next_state.
velocities[index], 0.0);
199 rclcpp::init(argc, argv);
200 ::testing::InitGoogleTest(&argc, argv);
201 int result = RUN_ALL_TESTS();
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
KinematicState extractRobotState(const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
Extract the state from a RobotStatePtr instance.
Eigen::VectorXd positions
Eigen::VectorXd velocities
TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
Continuous self collision checks are not supported yet by the bullet integration.
int main(int argc, char **argv)