36 #include <moveit_resources/config.h> 39 #include <urdf_parser/urdf_parser.h> 40 #include <gtest/gtest.h> 49 while (i1 < s1.size() && isspace(s1[i1]))
51 while (i2 < s2.size() && isspace(s2[i2]))
53 while (i1 < s1.size() && i2 < s2.size())
55 if (i1 < s1.size() && i2 < s2.size())
62 while (i1 < s1.size() && isspace(s1[i1]))
64 while (i2 < s2.size() && isspace(s2[i2]))
67 return i1 == s1.size() && i2 == s2.size();
69 static void expect_near(
const Eigen::MatrixXd& x,
const Eigen::MatrixXd& y,
70 double eps = std::numeric_limits<double>::epsilon())
72 ASSERT_EQ(x.rows(), y.rows());
73 ASSERT_EQ(x.cols(), y.cols());
74 for (
int r = 0;
r < x.rows(); ++
r)
75 for (
int c = 0; c < x.cols(); ++c)
76 EXPECT_NEAR(x(
r, c), y(
r, c), eps) <<
"(r,c) = (" <<
r <<
"," << c <<
")";
80 #define EXPECT_NEAR_TRACED(...) { \ 81 SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \ 82 expect_near(__VA_ARGS__); \ 88 static const std::string MODEL0 =
"<?xml version=\"1.0\" ?>" 89 "<robot name=\"myrobot\">" 90 " <link name=\"base_link\">" 91 " <collision name=\"base_collision\">" 92 " <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>" 93 " <geometry name=\"base_collision_geom\">" 94 " <box size=\"0.65 0.65 0.23\"/>" 100 static const std::string SMODEL0 =
101 "<?xml version=\"1.0\" ?>" 102 "<robot name=\"myrobot\">" 103 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"floating\"/>" 106 urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL0);
108 srdfModel->initString(*urdfModel, SMODEL0);
110 EXPECT_TRUE(srdfModel->getVirtualJoints().size() == 1);
122 EXPECT_EQ(std::string(
"myrobot"), model->getName());
125 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
126 EXPECT_EQ((
unsigned int)1, links.size());
128 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
129 EXPECT_EQ((
unsigned int)1, joints.size());
131 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
132 EXPECT_EQ((
unsigned int)0, pgroups.size());
135 TEST(LoadingAndFK, SimpleRobot)
137 static const std::string MODEL1 =
138 "<?xml version=\"1.0\" ?>" 139 "<robot name=\"myrobot\">" 140 "<link name=\"base_link\">" 142 " <mass value=\"2.81\"/>" 143 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>" 144 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" 146 " <collision name=\"my_collision\">" 147 " <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>" 149 " <box size=\"1 2 1\" />" 153 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" 155 " <box size=\"1 2 1\" />" 161 static const std::string SMODEL1 =
162 "<?xml version=\"1.0\" ?>" 163 "<robot name=\"myrobot\">" 164 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>" 165 "<group name=\"base\">" 166 "<joint name=\"base_joint\"/>" 170 urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL1);
173 srdfModel->initString(*urdfModel, SMODEL1);
182 EXPECT_EQ((
unsigned int)1, (
unsigned int)model->getJointModelCount());
183 EXPECT_EQ((
unsigned int)3, (
unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
185 std::map<std::string, double> joint_values;
186 joint_values[
"base_joint/x"] = 10.0;
187 joint_values[
"base_joint/y"] = 8.0;
190 std::vector<std::string> missing_states;
192 ASSERT_EQ(missing_states.size(), 1);
193 EXPECT_EQ(missing_states[0], std::string(
"base_joint/theta"));
194 joint_values[
"base_joint/theta"] = 0.1;
197 ASSERT_EQ(missing_states.size(), 0);
209 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/x")] = 5.0;
210 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/y")] = 4.0;
211 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/theta")] = 0.0;
222 static const std::string MODEL2 =
223 "<?xml version=\"1.0\" ?>" 224 "<robot name=\"one_robot\">" 225 "<link name=\"base_link\">" 227 " <mass value=\"2.81\"/>" 228 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" 229 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" 231 " <collision name=\"my_collision\">" 232 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 234 " <box size=\"1 2 1\" />" 238 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" 240 " <box size=\"1 2 1\" />" 244 "<joint name=\"joint_a\" type=\"continuous\">" 245 " <axis xyz=\"0 0 1\"/>" 246 " <parent link=\"base_link\"/>" 247 " <child link=\"link_a\"/>" 248 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>" 250 "<link name=\"link_a\">" 252 " <mass value=\"1.0\"/>" 253 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" 254 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" 257 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 259 " <box size=\"1 2 1\" />" 263 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" 265 " <box size=\"1 2 1\" />" 269 "<joint name=\"joint_b\" type=\"fixed\">" 270 " <parent link=\"link_a\"/>" 271 " <child link=\"link_b\"/>" 272 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>" 274 "<link name=\"link_b\">" 276 " <mass value=\"1.0\"/>" 277 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" 278 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" 281 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 283 " <box size=\"1 2 1\" />" 287 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" 289 " <box size=\"1 2 1\" />" 293 " <joint name=\"joint_c\" type=\"prismatic\">" 294 " <axis xyz=\"1 0 0\"/>" 295 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>" 296 " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" " 297 "soft_upper_limit=\"0.089\"/>" 298 " <parent link=\"link_b\"/>" 299 " <child link=\"link_c\"/>" 300 " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>" 302 "<link name=\"link_c\">" 304 " <mass value=\"1.0\"/>" 305 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>" 306 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" 309 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 311 " <box size=\"1 2 1\" />" 315 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" 317 " <box size=\"1 2 1\" />" 321 " <joint name=\"mim_f\" type=\"prismatic\">" 322 " <axis xyz=\"1 0 0\"/>" 323 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>" 324 " <parent link=\"link_c\"/>" 325 " <child link=\"link_d\"/>" 326 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>" 327 " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>" 329 " <joint name=\"joint_f\" type=\"prismatic\">" 330 " <axis xyz=\"1 0 0\"/>" 331 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>" 332 " <parent link=\"link_d\"/>" 333 " <child link=\"link_e\"/>" 334 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>" 336 "<link name=\"link_d\">" 338 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 340 " <box size=\"1 2 1\" />" 344 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>" 346 " <box size=\"1 2 1\" />" 350 "<link name=\"link_e\">" 352 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" 354 " <box size=\"1 2 1\" />" 358 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>" 360 " <box size=\"1 2 1\" />" 366 static const std::string SMODEL2 =
367 "<?xml version=\"1.0\" ?>" 368 "<robot name=\"one_robot\">" 369 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>" 370 "<group name=\"base_from_joints\">" 371 "<joint name=\"base_joint\"/>" 372 "<joint name=\"joint_a\"/>" 373 "<joint name=\"joint_c\"/>" 375 "<group name=\"mim_joints\">" 376 "<joint name=\"joint_f\"/>" 377 "<joint name=\"mim_f\"/>" 379 "<group name=\"base_with_subgroups\">" 380 "<group name=\"base_from_base_to_tip\"/>" 381 "<joint name=\"joint_c\"/>" 383 "<group name=\"base_from_base_to_tip\">" 384 "<chain base_link=\"base_link\" tip_link=\"link_b\"/>" 385 "<joint name=\"base_joint\"/>" 387 "<group name=\"base_from_base_to_e\">" 388 "<chain base_link=\"base_link\" tip_link=\"link_e\"/>" 389 "<joint name=\"base_joint\"/>" 391 "<group name=\"base_with_bad_subgroups\">" 392 "<group name=\"error\"/>" 396 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
398 srdf_model->initString(*urdf_model, SMODEL2);
412 moveit::core::RobotModelConstPtr model =
robot_model;
421 ASSERT_TRUE(g_one !=
nullptr);
422 ASSERT_TRUE(g_two !=
nullptr);
423 ASSERT_TRUE(g_three !=
nullptr);
424 ASSERT_TRUE(g_four ==
nullptr);
439 std::sort(jmn.begin(), jmn.end());
441 EXPECT_EQ(jmn[1],
"joint_a");
442 EXPECT_EQ(jmn[2],
"joint_c");
444 std::sort(jmn.begin(), jmn.end());
445 EXPECT_EQ(jmn[0],
"base_joint");
446 EXPECT_EQ(jmn[1],
"joint_a");
447 EXPECT_EQ(jmn[2],
"joint_b");
449 std::sort(jmn.begin(), jmn.end());
450 EXPECT_EQ(jmn[0],
"base_joint");
451 EXPECT_EQ(jmn[1],
"joint_a");
452 EXPECT_EQ(jmn[2],
"joint_b");
453 EXPECT_EQ(jmn[3],
"joint_c");
479 EXPECT_EQ((
unsigned int)7, state.getVariableCount());
481 state.setToDefaultValues();
483 std::map<std::string, double> joint_values;
484 joint_values[
"base_joint/x"] = 1.0;
485 joint_values[
"base_joint/y"] = 1.0;
486 joint_values[
"base_joint/theta"] = 0.5;
487 joint_values[
"joint_a"] = -0.5;
488 joint_values[
"joint_c"] = 0.08;
489 state.setVariablePositions(joint_values);
491 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"base_link").translation(), Eigen::Vector3d(1, 1, 0));
492 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"base_link").linear()).x(), 1e-5);
493 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"base_link").linear()).y(), 1e-5);
494 EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform(
"base_link").linear()).
z(), 1e-5);
495 EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform(
"base_link").linear()).w(), 1e-5);
497 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_a").translation(), Eigen::Vector3d(1, 1, 0));
498 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_a").linear()).x(), 1e-5);
499 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_a").linear()).y(), 1e-5);
500 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_a").linear()).
z(), 1e-5);
501 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_a").linear()).w(), 1e-5);
503 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
504 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_b").linear()).x(), 1e-5);
505 EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_b").linear()).y(), 1e-5);
506 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_b").linear()).
z(), 1e-5);
507 EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_b").linear()).w(), 1e-5);
509 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
510 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_c").linear()).x(), 1e-5);
511 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_c").linear()).y(), 1e-5);
512 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_c").linear()).
z(), 1e-5);
513 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform(
"link_c").linear()).w(), 1e-5);
517 std::map<std::string, double> upd_a;
518 upd_a[
"joint_a"] = 0.2;
519 state.setVariablePositions(upd_a);
520 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel(
"joint_a")));
521 EXPECT_NEAR(state.getVariablePosition(
"joint_a"), 0.2, 1e-3);
522 state.enforceBounds();
523 EXPECT_NEAR(state.getVariablePosition(
"joint_a"), 0.2, 1e-3);
525 upd_a[
"joint_a"] = 3.2;
526 state.setVariablePositions(upd_a);
527 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel(
"joint_a")));
528 EXPECT_NEAR(state.getVariablePosition(
"joint_a"), 3.2, 1e-3);
529 state.enforceBounds();
530 EXPECT_NEAR(state.getVariablePosition(
"joint_a"), -3.083185, 1e-3);
531 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel(
"joint_a")));
534 state.setToDefaultValues();
535 EXPECT_TRUE(state.dirtyLinkTransforms());
536 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
537 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_d").translation(), Eigen::Vector3d(0.2, 0.5, 0));
538 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_e").translation(), Eigen::Vector3d(0.3, 0.6, 0));
541 state.setVariablePosition(
"joint_f", 1.0);
542 EXPECT_EQ(state.getVariablePosition(
"joint_f"), 1.0);
543 EXPECT_EQ(state.getVariablePosition(
"mim_f"), 1.6);
544 EXPECT_TRUE(state.dirtyLinkTransforms());
545 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
546 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
547 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
551 state.copyJointGroupPositions(g_mim, gstate);
554 state.setToRandomPositions(g_mim);
555 double joint_f = state.getVariablePosition(
"joint_f");
556 double mim_f = state.getVariablePosition(
"mim_f");
558 EXPECT_TRUE(state.dirtyLinkTransforms());
559 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
560 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_d").translation(), Eigen::Vector3d(0.1 + mim_f, 0.5, 0));
562 Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
565 state.setJointGroupPositions(g_mim, gstate);
566 EXPECT_EQ(state.getVariablePosition(
"joint_f"), 1.0);
567 EXPECT_EQ(state.getVariablePosition(
"mim_f"), 1.6);
568 EXPECT_TRUE(state.dirtyLinkTransforms());
569 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
570 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
571 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform(
"link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
575 const moveit::core::RobotModelConstPtr&
robot_model,
581 robot_state->setToDefaultValues();
585 for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix)
591 ja = robot_state->getVariablePosition(
"joint_a");
592 jc = robot_state->getVariablePosition(
"joint_c");
596 robot_state->setVariablePosition(
"joint_a", ja);
598 robot_state->setVariablePosition(
"joint_c", jc);
603 robot_state->setVariablePosition(
"joint_a", ja);
608 robot_state->setVariablePosition(
"joint_c", jc);
620 std::vector<std::shared_ptr<robot_state::RobotState>> traj;
623 const std::size_t expected_full_traj_len = 7;
629 EXPECT_EQ(full_traj_len, expected_full_traj_len);
635 std::vector<std::shared_ptr<robot_state::RobotState>> traj;
638 const std::size_t expected_revolute_jump_traj_len = 4;
639 const std::size_t expected_prismatic_jump_traj_len = 5;
643 const double expected_revolute_jump_fraction = (double)expected_revolute_jump_traj_len / (
double)full_traj_len;
644 const double expected_prismatic_jump_fraction = (double)expected_prismatic_jump_traj_len / (
double)full_traj_len;
651 EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());
652 EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
657 EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());
658 EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
663 EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());
664 EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
669 EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size());
670 EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01);
676 EXPECT_NEAR(1.0, fraction, 0.01);
682 std::vector<std::shared_ptr<robot_state::RobotState>> traj;
685 const std::size_t expected_relative_jump_traj_len = 4;
689 const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (
double)full_traj_len;
696 EXPECT_EQ(expected_relative_jump_traj_len, traj.size());
697 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
702 EXPECT_EQ(expected_relative_jump_traj_len, traj.size());
703 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
709 EXPECT_NEAR(1.0, fraction, 0.01);
712 int main(
int argc,
char** argv)
714 testing::InitGoogleTest(&argc, argv);
715 return RUN_ALL_TESTS();
const std::vector< const LinkModel * > & getUpdatedLinkModels() const
Get the names of the links that are to be updated when the state of this group changes. This includes links that are in the kinematic model but outside this group, if those links are descendants of joints in this group that have their values updated. The order is the correct order for updating the corresponding states.
Core components of MoveIt!
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
static double testAbsoluteJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
Tests for absolute joint space jumps of the trajectory traj.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Struct for containing jump_threshold.
static double testJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
static double testRelativeJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double jump_threshold_factor)
Tests for relative joint space jumps of the trajectory traj.
static bool sameStringIgnoringWS(const std::string &s1, const std::string &s2)
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
TEST(Loading, SimpleRobot)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
void setVariableAcceleration(const std::string &variable, double value)
Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
int main(int argc, char **argv)
std::size_t getVariableCount() const
Get the number of variables that make up this state.
#define EXPECT_TRUE(args)
#define EXPECT_NEAR_TRACED(...)
static void expect_near(const Eigen::MatrixXd &x, const Eigen::MatrixXd &y, double eps=std::numeric_limits< double >::epsilon())
moveit::core::RobotModelConstPtr robot_model
std::size_t generateTestTraj(std::vector< std::shared_ptr< robot_state::RobotState >> &traj, const moveit::core::RobotModelConstPtr &robot_model, const robot_model::JointModelGroup *joint_model_group)