39 #include <urdf_parser/urdf_parser.h>
40 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41 #include <gtest/gtest.h>
42 #include <gmock/gmock-matchers.h>
49 constexpr
double EPSILON{ 1.e-9 };
53 static bool sameStringIgnoringWS(
const std::string& s1,
const std::string& s2)
57 while (i1 < s1.size() && isspace(s1[i1]))
59 while (i2 < s2.size() && isspace(s2[i2]))
61 while (i1 < s1.size() && i2 < s2.size())
63 if (i1 < s1.size() && i2 < s2.size())
70 while (i1 < s1.size() && isspace(s1[i1]))
72 while (i2 < s2.size() && isspace(s2[i2]))
75 return i1 == s1.size() && i2 == s2.size();
79 static void expect_near(
const Eigen::MatrixXd&
x,
const Eigen::MatrixXd&
y,
80 double eps = std::numeric_limits<double>::epsilon())
82 ASSERT_EQ(
x.rows(),
y.rows());
83 ASSERT_EQ(
x.cols(),
y.cols());
84 for (
int r = 0;
r <
x.rows(); ++
r)
85 for (
int c = 0;
c <
x.cols(); ++
c)
86 EXPECT_NEAR(
x(
r,
c),
y(
r,
c), eps) <<
"(r,c) = (" <<
r <<
"," <<
c <<
")";
90 #define EXPECT_NEAR_TRACED(...) { \
91 SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
92 expect_near(__VA_ARGS__); \
99 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"floating",
"base_joint");
100 ASSERT_TRUE(builder.
isValid());
101 moveit::core::RobotModelPtr model = builder.
build();
111 EXPECT_EQ(std::string(
"myrobot"), model->getName());
114 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
115 EXPECT_EQ((
unsigned int)1, links.size());
117 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
118 EXPECT_EQ((
unsigned int)1, joints.size());
120 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
121 EXPECT_EQ((
unsigned int)0, pgroups.size());
124 TEST(LoadingAndFK, SimpleRobot)
127 geometry_msgs::msg::Pose pose;
128 pose.position.x = -0.1;
134 pose.orientation = tf2::toMsg(q);
140 pose.orientation = tf2::toMsg(q);
143 pose.position.y = 0.099;
146 pose.orientation = tf2::toMsg(q);
147 builder.
addInertial(
"base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
148 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"planar",
"base_joint");
149 builder.
addGroup({}, {
"base_joint" },
"base");
151 ASSERT_TRUE(builder.
isValid());
152 moveit::core::RobotModelPtr model = builder.
build();
159 EXPECT_EQ((
unsigned int)1, (
unsigned int)model->getJointModelCount());
160 EXPECT_EQ((
unsigned int)3, (
unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
162 std::map<std::string, double> joint_values;
163 joint_values[
"base_joint/x"] = 10.0;
164 joint_values[
"base_joint/y"] = 8.0;
167 std::vector<std::string> missing_states;
169 ASSERT_EQ(missing_states.size(), 1u);
170 EXPECT_EQ(missing_states[0], std::string(
"base_joint/theta"));
171 joint_values[
"base_joint/theta"] = 0.1;
174 ASSERT_EQ(missing_states.size(), 0u);
180 const auto new_state = std::make_unique<moveit::core::RobotState>(state);
184 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/x")] = 5.0;
185 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/y")] = 4.0;
186 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/theta")] = 0.0;
197 static const std::string MODEL2 = R
"(
198 <?xml version="1.0" ?>
199 <robot name="one_robot">
200 <link name="base_link">
203 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
204 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
206 <collision name="my_collision">
207 <origin rpy="0 0 0" xyz="0 0 0"/>
213 <origin rpy="0 0 0" xyz="0.0 0 0"/>
219 <joint name="joint_a" type="continuous">
221 <parent link="base_link"/>
222 <child link="link_a"/>
223 <origin rpy=" 0.0 0 0 " xyz="0.0 0 0 "/>
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"/>
232 <origin rpy="0 0 0" xyz="0 0 0"/>
238 <origin rpy="0 0 0" xyz="0.0 0 0"/>
244 <joint name="joint_b" type="fixed">
245 <parent link="link_a"/>
246 <child link="link_b"/>
247 <origin rpy=" 0.0 -0.42 0 " xyz="0.0 0.5 0 "/>
252 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
253 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
256 <origin rpy="0 0 0" xyz="0 0 0"/>
262 <origin rpy="0 0 0" xyz="0.0 0 0"/>
268 <joint name="joint_c" type="prismatic">
270 <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
271 <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0"
272 soft_upper_limit="0.089"/>
273 <parent link="link_b"/>
274 <child link="link_c"/>
275 <origin rpy=" 0.0 0.42 0.0 " xyz="0.0 -0.1 0 "/>
280 <origin rpy="0 0 0" xyz="0.0 0 .0"/>
281 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
284 <origin rpy="0 0 0" xyz="0 0 0"/>
290 <origin rpy="0 0 0" xyz="0.0 0 0"/>
296 <joint name="mim_f" type="prismatic">
298 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
299 <parent link="link_c"/>
300 <child link="link_d"/>
301 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
302 <mimic joint="joint_f" multiplier="1.5" offset="0.1"/>
304 <joint name="joint_f" type="prismatic">
306 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
307 <parent link="link_d"/>
308 <child link="link_e"/>
309 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
313 <origin rpy="0 0 0" xyz="0 0 0"/>
319 <origin rpy="0 1 0" xyz="0 0.1 0"/>
327 <origin rpy="0 0 0" xyz="0 0 0"/>
333 <origin rpy="0 1 0" xyz="0 0.1 0"/>
339 <link name="link/with/slash" />
340 <joint name="joint_link_with_slash" type="fixed">
341 <parent link="base_link"/>
342 <child link="link/with/slash"/>
343 <origin rpy="0 0 0" xyz="0 0 0"/>
347 static const std::string SMODEL2 = R
"(
348 <?xml version="1.0" ?>
349 <robot name="one_robot">
350 <virtual_joint name="base_joint" child_link="base_link" parent_frame="odom_combined" type="planar"/>
351 <group name="base_from_joints">
352 <joint name="base_joint"/>
353 <joint name="joint_a"/>
354 <joint name="joint_c"/>
356 <group name="mim_joints">
357 <joint name="joint_f"/>
358 <joint name="mim_f"/>
360 <group name="base_with_subgroups">
361 <group name="base_from_base_to_tip"/>
362 <joint name="joint_c"/>
364 <group name="base_from_base_to_tip">
365 <chain base_link="base_link" tip_link="link_b"/>
366 <joint name="base_joint"/>
368 <group name="base_from_base_to_e">
369 <chain base_link="base_link" tip_link="link_e"/>
370 <joint name="base_joint"/>
372 <group name="base_with_bad_subgroups">
373 <group name="error"/>
378 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
379 auto srdf_model = std::make_shared<srdf::Model>();
380 srdf_model->initString(*urdf_model, SMODEL2);
381 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
394 moveit::core::RobotModelConstPtr model = robot_model_;
403 ASSERT_TRUE(g_one !=
nullptr);
404 ASSERT_TRUE(g_two !=
nullptr);
405 ASSERT_TRUE(g_three !=
nullptr);
406 ASSERT_TRUE(g_four ==
nullptr);
408 EXPECT_THAT(g_one->
getJointModelNames(), ::testing::ElementsAreArray({
"base_joint",
"joint_a",
"joint_c" }));
409 EXPECT_THAT(g_two->
getJointModelNames(), ::testing::ElementsAreArray({
"base_joint",
"joint_a",
"joint_b" }));
411 ::testing::ElementsAreArray({
"base_joint",
"joint_a",
"joint_b",
"joint_c" }));
412 EXPECT_THAT(g_mim->
getJointModelNames(), ::testing::ElementsAreArray({
"mim_f",
"joint_f" }));
414 EXPECT_THAT(g_one->
getLinkModelNames(), ::testing::ElementsAreArray({
"base_link",
"link_a",
"link_c" }));
415 EXPECT_THAT(g_two->
getLinkModelNames(), ::testing::ElementsAreArray({
"base_link",
"link_a",
"link_b" }));
416 EXPECT_THAT(g_three->
getLinkModelNames(), ::testing::ElementsAreArray({
"base_link",
"link_a",
"link_b",
"link_c" }));
419 auto updated_link_model_names = {
"base_link",
"link_a",
"link_b",
"link_c",
"link_d",
"link_e",
"link/with/slash" };
430 std::map<std::string, double> joint_values;
431 joint_values[
"base_joint/x"] = 1.0;
432 joint_values[
"base_joint/y"] = 1.0;
433 joint_values[
"base_joint/theta"] = 0.5;
434 joint_values[
"joint_a"] = -0.5;
435 joint_values[
"joint_c"] = 0.08;
464 std::map<std::string, double> upd_a;
465 upd_a[
"joint_a"] = 0.2;
472 upd_a[
"joint_a"] = 3.2;
504 EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
525 ASSERT_TRUE(joint_model_group);
529 std::cout <<
"\nVisual inspection should show NO joints out of bounds:\n";
532 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
533 std::vector<double> single_joint(1);
534 single_joint[0] = -1.0;
538 std::cout <<
"\nVisual inspection should show TWO joint out of bounds:\n";
539 single_joint[0] = 1.0;
543 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
544 single_joint[0] = 0.19;
557 for (
size_t i = 0; i <= 10; ++i)
559 state_a.
interpolate(state_b,
static_cast<double>(i) / 10., interpolated_state,
562 <<
"Interpolation between identical states yielded a different state.";
564 for (
const auto& link_name : robot_model_->getLinkModelNames())
567 <<
"Interpolation between identical states yielded NaN value.";
572 std::map<std::string, double> joint_values;
573 joint_values[
"base_joint/x"] = 1.0;
574 joint_values[
"base_joint/y"] = 1.0;
576 joint_values[
"base_joint/x"] = 0.0;
577 joint_values[
"base_joint/y"] = 2.0;
579 EXPECT_NEAR(3 * std::sqrt(2), state_a.
distance(state_b),
EPSILON) <<
"Simple interpolation of base_joint failed.";
583 <<
"Simple interpolation of base_joint failed.";
585 <<
"Simple interpolation of base_joint failed.";
587 <<
"Simple interpolation of base_joint failed.";
590 <<
"Simple interpolation of base_joint failed.";
592 <<
"Simple interpolation of base_joint failed.";
595 joint_values[
"base_joint/x"] = 0.0;
596 joint_values[
"base_joint/y"] = 20.0;
597 joint_values[
"base_joint/theta"] = 3 * M_PI / 4;
598 joint_values[
"joint_a"] = -4 * M_PI / 5;
599 joint_values[
"joint_c"] = 0.0;
600 joint_values[
"joint_f"] = 1.0;
603 joint_values[
"base_joint/x"] = 10.0;
604 joint_values[
"base_joint/y"] = 0.0;
605 joint_values[
"base_joint/theta"] = -3 * M_PI / 4;
606 joint_values[
"joint_a"] = 4 * M_PI / 5;
607 joint_values[
"joint_c"] = 0.07;
608 joint_values[
"joint_f"] = 0.0;
611 for (
size_t i = 0; i <= 5; ++i)
613 double t =
static_cast<double>(i) / 5.;
616 <<
"Base joint interpolation failed.";
618 <<
"Base joint interpolation failed.";
622 <<
"Base joint theta interpolation failed.";
624 <<
"Continuous joint interpolation failed.";
628 EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.
getVariablePosition(
"base_joint/theta"),
630 <<
"Base joint theta interpolation failed.";
632 <<
"Continuous joint interpolation failed.";
635 <<
"Interpolation of joint_c failed.";
637 <<
"Interpolation of joint_f failed.";
639 <<
"Interpolation of mimic joint mim_f failed.";
642 bool nan_exception =
false;
647 catch (std::exception& e)
649 std::cout <<
"Caught expected exception: " << e.what() <<
'\n';
650 nan_exception =
true;
652 EXPECT_TRUE(nan_exception) <<
"NaN interpolation parameter did not create expected exception.";
659 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
664 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
669 Eigen::Isometry3d a_to_b;
673 state.
attachBody(std::make_unique<moveit::core::AttachedBody>(
674 link_b,
"object", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)), std::vector<shapes::ShapeConstPtr>{},
675 EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{},
679 Eigen::Isometry3d transform;
692 EXPECT_TRUE(link_with_slash);
695 ASSERT_TRUE(rigid_parent_of_link_with_slash);
696 EXPECT_EQ(
"base_link", rigid_parent_of_link_with_slash->
getName());
699 state.
attachBody(std::make_unique<moveit::core::AttachedBody>(
700 link_with_slash,
"object/with/slash", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)),
701 std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{}, std::set<std::string>{},
702 trajectory_msgs::msg::JointTrajectory{},
706 ASSERT_TRUE(rigid_parent_of_object);
707 EXPECT_EQ(rigid_parent_of_link_with_slash, rigid_parent_of_object);
710 int main(
int argc,
char** argv)
712 testing::InitGoogleTest(&argc, argv);
713 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 > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
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...
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.
const std::string & getName() const
The name of this link.
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 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 moveit::core::JointModelGroup *jmg=nullptr) 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(...)