39 #include <urdf_parser/urdf_parser.h>
40 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41 #include <gtest/gtest.h>
48 constexpr
double EPSILON{ 1.e-9 };
52 static bool sameStringIgnoringWS(
const std::string& s1,
const std::string& s2)
56 while (i1 < s1.size() && isspace(s1[i1]))
58 while (i2 < s2.size() && isspace(s2[i2]))
60 while (i1 < s1.size() && i2 < s2.size())
62 if (i1 < s1.size() && i2 < s2.size())
69 while (i1 < s1.size() && isspace(s1[i1]))
71 while (i2 < s2.size() && isspace(s2[i2]))
74 return i1 == s1.size() && i2 == s2.size();
78 static void expect_near(
const Eigen::MatrixXd&
x,
const Eigen::MatrixXd&
y,
79 double eps = std::numeric_limits<double>::epsilon())
81 ASSERT_EQ(
x.rows(),
y.rows());
82 ASSERT_EQ(
x.cols(),
y.cols());
83 for (
int r = 0;
r <
x.rows(); ++
r)
84 for (
int c = 0;
c <
x.cols(); ++
c)
85 EXPECT_NEAR(
x(
r,
c),
y(
r,
c), eps) <<
"(r,c) = (" <<
r <<
"," <<
c <<
")";
89 #define EXPECT_NEAR_TRACED(...) { \
90 SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
91 expect_near(__VA_ARGS__); \
98 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"floating",
"base_joint");
100 moveit::core::RobotModelPtr model = builder.
build();
110 EXPECT_EQ(std::string(
"myrobot"), model->getName());
113 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
114 EXPECT_EQ((
unsigned int)1, links.size());
116 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
117 EXPECT_EQ((
unsigned int)1, joints.size());
119 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
120 EXPECT_EQ((
unsigned int)0, pgroups.size());
123 TEST(LoadingAndFK, SimpleRobot)
126 geometry_msgs::msg::Pose pose;
127 pose.position.x = -0.1;
133 pose.orientation = tf2::toMsg(q);
139 pose.orientation = tf2::toMsg(q);
142 pose.position.y = 0.099;
145 pose.orientation = tf2::toMsg(q);
146 builder.
addInertial(
"base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
147 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"planar",
"base_joint");
148 builder.
addGroup({}, {
"base_joint" },
"base");
150 ASSERT_TRUE(builder.
isValid());
151 moveit::core::RobotModelPtr model = builder.
build();
158 EXPECT_EQ((
unsigned int)1, (
unsigned int)model->getJointModelCount());
159 EXPECT_EQ((
unsigned int)3, (
unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
161 std::map<std::string, double> joint_values;
162 joint_values[
"base_joint/x"] = 10.0;
163 joint_values[
"base_joint/y"] = 8.0;
166 std::vector<std::string> missing_states;
168 ASSERT_EQ(missing_states.size(), 1u);
169 EXPECT_EQ(missing_states[0], std::string(
"base_joint/theta"));
170 joint_values[
"base_joint/theta"] = 0.1;
173 ASSERT_EQ(missing_states.size(), 0u);
179 const auto new_state = std::make_unique<moveit::core::RobotState>(state);
183 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/x")] = 5.0;
184 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/y")] = 4.0;
185 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/theta")] = 0.0;
196 static const std::string MODEL2 =
197 "<?xml version=\"1.0\" ?>"
198 "<robot name=\"one_robot\">"
199 "<link name=\"base_link\">"
201 " <mass value=\"2.81\"/>"
202 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
203 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
205 " <collision name=\"my_collision\">"
206 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
208 " <box size=\"1 2 1\" />"
212 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
214 " <box size=\"1 2 1\" />"
218 "<joint name=\"joint_a\" type=\"continuous\">"
219 " <axis xyz=\"0 0 1\"/>"
220 " <parent link=\"base_link\"/>"
221 " <child link=\"link_a\"/>"
222 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
224 "<link name=\"link_a\">"
226 " <mass value=\"1.0\"/>"
227 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
228 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
231 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
233 " <box size=\"1 2 1\" />"
237 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
239 " <box size=\"1 2 1\" />"
243 "<joint name=\"joint_b\" type=\"fixed\">"
244 " <parent link=\"link_a\"/>"
245 " <child link=\"link_b\"/>"
246 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
248 "<link name=\"link_b\">"
250 " <mass value=\"1.0\"/>"
251 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
252 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
255 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
257 " <box size=\"1 2 1\" />"
261 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
263 " <box size=\"1 2 1\" />"
267 " <joint name=\"joint_c\" type=\"prismatic\">"
268 " <axis xyz=\"1 0 0\"/>"
269 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
270 " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
271 "soft_upper_limit=\"0.089\"/>"
272 " <parent link=\"link_b\"/>"
273 " <child link=\"link_c\"/>"
274 " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
276 "<link name=\"link_c\">"
278 " <mass value=\"1.0\"/>"
279 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
280 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
283 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
285 " <box size=\"1 2 1\" />"
289 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
291 " <box size=\"1 2 1\" />"
295 " <joint name=\"mim_f\" type=\"prismatic\">"
296 " <axis xyz=\"1 0 0\"/>"
297 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
298 " <parent link=\"link_c\"/>"
299 " <child link=\"link_d\"/>"
300 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
301 " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
303 " <joint name=\"joint_f\" type=\"prismatic\">"
304 " <axis xyz=\"1 0 0\"/>"
305 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
306 " <parent link=\"link_d\"/>"
307 " <child link=\"link_e\"/>"
308 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
310 "<link name=\"link_d\">"
312 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
314 " <box size=\"1 2 1\" />"
318 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
320 " <box size=\"1 2 1\" />"
324 "<link name=\"link_e\">"
326 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
328 " <box size=\"1 2 1\" />"
332 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
334 " <box size=\"1 2 1\" />"
340 static const std::string SMODEL2 =
341 "<?xml version=\"1.0\" ?>"
342 "<robot name=\"one_robot\">"
343 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
344 "<group name=\"base_from_joints\">"
345 "<joint name=\"base_joint\"/>"
346 "<joint name=\"joint_a\"/>"
347 "<joint name=\"joint_c\"/>"
349 "<group name=\"mim_joints\">"
350 "<joint name=\"joint_f\"/>"
351 "<joint name=\"mim_f\"/>"
353 "<group name=\"base_with_subgroups\">"
354 "<group name=\"base_from_base_to_tip\"/>"
355 "<joint name=\"joint_c\"/>"
357 "<group name=\"base_from_base_to_tip\">"
358 "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
359 "<joint name=\"base_joint\"/>"
361 "<group name=\"base_from_base_to_e\">"
362 "<chain base_link=\"base_link\" tip_link=\"link_e\"/>"
363 "<joint name=\"base_joint\"/>"
365 "<group name=\"base_with_bad_subgroups\">"
366 "<group name=\"error\"/>"
370 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
371 auto srdf_model = std::make_shared<srdf::Model>();
372 srdf_model->initString(*urdf_model, SMODEL2);
373 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
386 moveit::core::RobotModelConstPtr model = robot_model_;
395 ASSERT_TRUE(g_one !=
nullptr);
396 ASSERT_TRUE(g_two !=
nullptr);
397 ASSERT_TRUE(g_three !=
nullptr);
398 ASSERT_TRUE(g_four ==
nullptr);
413 std::sort(jmn.begin(), jmn.end());
414 EXPECT_EQ(jmn[0],
"base_joint");
415 EXPECT_EQ(jmn[1],
"joint_a");
416 EXPECT_EQ(jmn[2],
"joint_c");
418 std::sort(jmn.begin(), jmn.end());
419 EXPECT_EQ(jmn[0],
"base_joint");
420 EXPECT_EQ(jmn[1],
"joint_a");
421 EXPECT_EQ(jmn[2],
"joint_b");
423 std::sort(jmn.begin(), jmn.end());
424 EXPECT_EQ(jmn[0],
"base_joint");
425 EXPECT_EQ(jmn[1],
"joint_a");
426 EXPECT_EQ(jmn[2],
"joint_b");
427 EXPECT_EQ(jmn[3],
"joint_c");
457 std::map<std::string, double> joint_values;
458 joint_values[
"base_joint/x"] = 1.0;
459 joint_values[
"base_joint/y"] = 1.0;
460 joint_values[
"base_joint/theta"] = 0.5;
461 joint_values[
"joint_a"] = -0.5;
462 joint_values[
"joint_c"] = 0.08;
491 std::map<std::string, double> upd_a;
492 upd_a[
"joint_a"] = 0.2;
499 upd_a[
"joint_a"] = 3.2;
531 EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
552 ASSERT_TRUE(joint_model_group);
556 std::cout <<
"\nVisual inspection should show NO joints out of bounds:\n";
559 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
560 std::vector<double> single_joint(1);
561 single_joint[0] = -1.0;
565 std::cout <<
"\nVisual inspection should show TWO joint out of bounds:\n";
566 single_joint[0] = 1.0;
570 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
571 single_joint[0] = 0.19;
584 for (
size_t i = 0; i <= 10; ++i)
586 state_a.
interpolate(state_b,
static_cast<double>(i) / 10., interpolated_state,
589 <<
"Interpolation between identical states yielded a different state.";
591 for (
const auto& link_name : robot_model_->getLinkModelNames())
594 <<
"Interpolation between identical states yielded NaN value.";
599 std::map<std::string, double> joint_values;
600 joint_values[
"base_joint/x"] = 1.0;
601 joint_values[
"base_joint/y"] = 1.0;
603 joint_values[
"base_joint/x"] = 0.0;
604 joint_values[
"base_joint/y"] = 2.0;
606 EXPECT_NEAR(3 * std::sqrt(2), state_a.
distance(state_b),
EPSILON) <<
"Simple interpolation of base_joint failed.";
610 <<
"Simple interpolation of base_joint failed.";
612 <<
"Simple interpolation of base_joint failed.";
614 <<
"Simple interpolation of base_joint failed.";
617 <<
"Simple interpolation of base_joint failed.";
619 <<
"Simple interpolation of base_joint failed.";
622 joint_values[
"base_joint/x"] = 0.0;
623 joint_values[
"base_joint/y"] = 20.0;
624 joint_values[
"base_joint/theta"] = 3 * M_PI / 4;
625 joint_values[
"joint_a"] = -4 * M_PI / 5;
626 joint_values[
"joint_c"] = 0.0;
627 joint_values[
"joint_f"] = 1.0;
630 joint_values[
"base_joint/x"] = 10.0;
631 joint_values[
"base_joint/y"] = 0.0;
632 joint_values[
"base_joint/theta"] = -3 * M_PI / 4;
633 joint_values[
"joint_a"] = 4 * M_PI / 5;
634 joint_values[
"joint_c"] = 0.07;
635 joint_values[
"joint_f"] = 0.0;
638 for (
size_t i = 0; i <= 5; ++i)
640 double t =
static_cast<double>(i) / 5.;
643 <<
"Base joint interpolation failed.";
645 <<
"Base joint interpolation failed.";
649 <<
"Base joint theta interpolation failed.";
651 <<
"Continuous joint interpolation failed.";
655 EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.
getVariablePosition(
"base_joint/theta"),
657 <<
"Base joint theta interpolation failed.";
659 <<
"Continuous joint interpolation failed.";
662 <<
"Interpolation of joint_c failed.";
664 <<
"Interpolation of joint_f failed.";
666 <<
"Interpolation of mimic joint mim_f failed.";
669 bool nan_exception =
false;
674 catch (std::exception& e)
676 std::cout <<
"Caught expected exception: " << e.what() <<
'\n';
677 nan_exception =
true;
679 EXPECT_TRUE(nan_exception) <<
"NaN interpolation parameter did not create expected exception.";
686 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
691 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
699 state.
attachBody(std::make_unique<moveit::core::AttachedBody>(
700 link_b,
"object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
701 EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{},
716 int main(
int argc,
char** argv)
718 testing::InitGoogleTest(&argc, argv);
719 return RUN_ALL_TESTS();
moveit::core::RobotModelConstPtr robot_model_
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
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...
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....
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
A link from the robot. Contains the constant transform applied to the link and its geometry.
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
void addInertial(const std::string &link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::msg::Pose origin)
Adds a collision box to a specific link.
bool isValid()
Returns true if the building process so far has been valid.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::msg::Pose origin)
Adds a visual box to a specific link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
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.
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
void interpolate(const RobotState &to, double t, RobotState &state) const
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
bool dirtyLinkTransforms() const
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
std::size_t getVariableCount() const
Get the number of variables that make up this state.
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
void setJointPositions(const std::string &joint_name, const double *position)
Vec3fX< details::Vec3Data< double > > Vector3d
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
int main(int argc, char **argv)
TEST(Loading, SimpleRobot)
#define EXPECT_NEAR_TRACED(...)