moveit2
The MoveIt Motion Planning Framework for ROS 2.
robot_state_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
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>
43 #include <sstream>
44 #include <algorithm>
45 #include <ctype.h>
46 
47 namespace
48 {
49 constexpr double EPSILON{ 1.e-9 };
50 }
51 
52 #if 0 // unused function
53 static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2)
54 {
55  unsigned int i1 = 0;
56  unsigned int i2 = 0;
57  while (i1 < s1.size() && isspace(s1[i1]))
58  i1++;
59  while (i2 < s2.size() && isspace(s2[i2]))
60  i2++;
61  while (i1 < s1.size() && i2 < s2.size())
62  {
63  if (i1 < s1.size() && i2 < s2.size())
64  {
65  if (s1[i1] != s2[i2])
66  return false;
67  i1++;
68  i2++;
69  }
70  while (i1 < s1.size() && isspace(s1[i1]))
71  i1++;
72  while (i2 < s2.size() && isspace(s2[i2]))
73  i2++;
74  }
75  return i1 == s1.size() && i2 == s2.size();
76 }
77 #endif
78 
79 static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
80  double eps = std::numeric_limits<double>::epsilon())
81 {
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 << ")";
87 }
88 
89 // clang-format off
90 #define EXPECT_NEAR_TRACED(...) { \
91  SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
92  expect_near(__VA_ARGS__); \
93 }
94 // clang-format on
95 
96 TEST(Loading, SimpleRobot)
97 {
98  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
99  builder.addVirtualJoint("odom_combined", "base_link", "floating", "base_joint");
100  ASSERT_TRUE(builder.isValid());
101  moveit::core::RobotModelPtr model = builder.build();
102  moveit::core::RobotState state(model);
103 
104  state.setToDefaultValues();
105 
106  // make sure that this copy constructor works
107  moveit::core::RobotState new_state(state);
108 
109  EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
110 
111  EXPECT_EQ(std::string("myrobot"), model->getName());
112  EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
113 
114  const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
115  EXPECT_EQ((unsigned int)1, links.size());
116 
117  const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
118  EXPECT_EQ((unsigned int)1, joints.size());
119 
120  const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
121  EXPECT_EQ((unsigned int)0, pgroups.size());
122 }
123 
124 TEST(LoadingAndFK, SimpleRobot)
125 {
126  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
127  geometry_msgs::msg::Pose pose;
128  pose.position.x = -0.1;
129  pose.position.y = 0;
130  pose.position.z = 0;
131 
132  tf2::Quaternion q;
133  q.setRPY(0, 0, -1);
134  pose.orientation = tf2::toMsg(q);
135  builder.addCollisionBox("base_link", { 1, 2, 1 }, pose);
136  pose.position.x = 0;
137  pose.position.y = 0;
138  pose.position.z = 0;
139  q.setRPY(0, 0, 0);
140  pose.orientation = tf2::toMsg(q);
141  builder.addVisualBox("base_link", { 1, 2, 1 }, pose);
142  pose.position.x = 0;
143  pose.position.y = 0.099;
144  pose.position.z = 0;
145  q.setRPY(0, 0, 0);
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");
150 
151  ASSERT_TRUE(builder.isValid());
152  moveit::core::RobotModelPtr model = builder.build();
153  moveit::core::RobotState state(model);
154 
155  EXPECT_EQ((unsigned int)3, state.getVariableCount());
156 
157  state.setToDefaultValues();
158 
159  EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
160  EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
161 
162  std::map<std::string, double> joint_values;
163  joint_values["base_joint/x"] = 10.0;
164  joint_values["base_joint/y"] = 8.0;
165 
166  // testing incomplete state
167  std::vector<std::string> missing_states;
168  state.setVariablePositions(joint_values, 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;
172 
173  state.setVariablePositions(joint_values, missing_states);
174  ASSERT_EQ(missing_states.size(), 0u);
175 
176  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
177 
178  state.setVariableAcceleration("base_joint/x", 0.0);
179 
180  const auto new_state = std::make_unique<moveit::core::RobotState>(state); // making sure that values get copied
181  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
182 
183  std::vector<double> jv(state.getVariableCount(), 0.0);
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;
187 
188  state.setVariablePositions(jv);
189  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(5, 4, 0));
190 }
191 
192 class OneRobot : public testing::Test
193 {
194 protected:
195  void SetUp() override
196  {
197  static const std::string MODEL2 = R"(
198 <?xml version="1.0" ?>
199 <robot name="one_robot">
200 <link name="base_link">
201  <inertial>
202  <mass value="2.81"/>
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"/>
205  </inertial>
206  <collision name="my_collision">
207  <origin rpy="0 0 0" xyz="0 0 0"/>
208  <geometry>
209  <box size="1 2 1" />
210  </geometry>
211  </collision>
212  <visual>
213  <origin rpy="0 0 0" xyz="0.0 0 0"/>
214  <geometry>
215  <box size="1 2 1" />
216  </geometry>
217  </visual>
218 </link>
219 <joint name="joint_a" type="continuous">
220  <axis xyz="0 0 1"/>
221  <parent link="base_link"/>
222  <child link="link_a"/>
223  <origin rpy=" 0.0 0 0 " xyz="0.0 0 0 "/>
224 </joint>
225 <link name="link_a">
226  <inertial>
227  <mass value="1.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"/>
230  </inertial>
231  <collision>
232  <origin rpy="0 0 0" xyz="0 0 0"/>
233  <geometry>
234  <box size="1 2 1" />
235  </geometry>
236  </collision>
237  <visual>
238  <origin rpy="0 0 0" xyz="0.0 0 0"/>
239  <geometry>
240  <box size="1 2 1" />
241  </geometry>
242  </visual>
243 </link>
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 "/>
248 </joint>
249 <link name="link_b">
250  <inertial>
251  <mass value="1.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"/>
254  </inertial>
255  <collision>
256  <origin rpy="0 0 0" xyz="0 0 0"/>
257  <geometry>
258  <box size="1 2 1" />
259  </geometry>
260  </collision>
261  <visual>
262  <origin rpy="0 0 0" xyz="0.0 0 0"/>
263  <geometry>
264  <box size="1 2 1" />
265  </geometry>
266  </visual>
267 </link>
268  <joint name="joint_c" type="prismatic">
269  <axis xyz="1 0 0"/>
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 "/>
276  </joint>
277 <link name="link_c">
278  <inertial>
279  <mass value="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"/>
282  </inertial>
283  <collision>
284  <origin rpy="0 0 0" xyz="0 0 0"/>
285  <geometry>
286  <box size="1 2 1" />
287  </geometry>
288  </collision>
289  <visual>
290  <origin rpy="0 0 0" xyz="0.0 0 0"/>
291  <geometry>
292  <box size="1 2 1" />
293  </geometry>
294  </visual>
295 </link>
296  <joint name="mim_f" type="prismatic">
297  <axis xyz="1 0 0"/>
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"/>
303  </joint>
304  <joint name="joint_f" type="prismatic">
305  <axis xyz="1 0 0"/>
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 "/>
310  </joint>
311 <link name="link_d">
312  <collision>
313  <origin rpy="0 0 0" xyz="0 0 0"/>
314  <geometry>
315  <box size="1 2 1" />
316  </geometry>
317  </collision>
318  <visual>
319  <origin rpy="0 1 0" xyz="0 0.1 0"/>
320  <geometry>
321  <box size="1 2 1" />
322  </geometry>
323  </visual>
324 </link>
325 <link name="link_e">
326  <collision>
327  <origin rpy="0 0 0" xyz="0 0 0"/>
328  <geometry>
329  <box size="1 2 1" />
330  </geometry>
331  </collision>
332  <visual>
333  <origin rpy="0 1 0" xyz="0 0.1 0"/>
334  <geometry>
335  <box size="1 2 1" />
336  </geometry>
337  </visual>
338 </link>
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"/>
344 </joint>
345 </robot>
346 )";
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"/>
355 </group>
356 <group name="mim_joints">
357 <joint name="joint_f"/>
358 <joint name="mim_f"/>
359 </group>
360 <group name="base_with_subgroups">
361 <group name="base_from_base_to_tip"/>
362 <joint name="joint_c"/>
363 </group>
364 <group name="base_from_base_to_tip">
365 <chain base_link="base_link" tip_link="link_b"/>
366 <joint name="base_joint"/>
367 </group>
368 <group name="base_from_base_to_e">
369 <chain base_link="base_link" tip_link="link_e"/>
370 <joint name="base_joint"/>
371 </group>
372 <group name="base_with_bad_subgroups">
373 <group name="error"/>
374 </group>
375 </robot>
376 )";
377 
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);
382  }
383 
384  void TearDown() override
385  {
386  }
387 
388 protected:
389  moveit::core::RobotModelConstPtr robot_model_;
390 };
391 
393 {
394  moveit::core::RobotModelConstPtr model = robot_model_;
395 
396  // testing that the two planning groups are the same
397  const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
398  const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
399  const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
400  const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
401  const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
402 
403  ASSERT_TRUE(g_one != nullptr);
404  ASSERT_TRUE(g_two != nullptr);
405  ASSERT_TRUE(g_three != nullptr);
406  ASSERT_TRUE(g_four == nullptr);
407 
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" }));
410  EXPECT_THAT(g_three->getJointModelNames(),
411  ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_b", "joint_c" }));
412  EXPECT_THAT(g_mim->getJointModelNames(), ::testing::ElementsAreArray({ "mim_f", "joint_f" }));
413 
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" }));
417 
418  // but they should have the same links to be updated
419  auto updated_link_model_names = { "base_link", "link_a", "link_b", "link_c", "link_d", "link_e", "link/with/slash" };
420  EXPECT_THAT(g_one->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
421  EXPECT_THAT(g_two->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
422  EXPECT_THAT(g_three->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
423 
424  moveit::core::RobotState state(model);
425 
426  EXPECT_EQ(state.getVariableCount(), 7u);
427 
428  state.setToDefaultValues();
429 
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;
436  state.setVariablePositions(joint_values);
437 
438  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0));
439  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5);
440  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5);
441  EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5);
442  EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5);
443 
444  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0));
445  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5);
446  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5);
447  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5);
448  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5);
449 
450  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
451  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5);
452  EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5);
453  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5);
454  EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5);
455 
456  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
457  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5);
458  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5);
459  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5);
460  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5);
461 
462  EXPECT_TRUE(state.satisfiesBounds());
463 
464  std::map<std::string, double> upd_a;
465  upd_a["joint_a"] = 0.2;
466  state.setVariablePositions(upd_a);
467  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
468  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
469  state.enforceBounds();
470  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
471 
472  upd_a["joint_a"] = 3.2;
473  state.setVariablePositions(upd_a);
474  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
475  EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
476  state.enforceBounds();
477  EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
478  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
479 
480  // mimic joints
481  state.setToDefaultValues();
482  EXPECT_TRUE(state.dirtyLinkTransforms());
483  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
484  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.2, 0.5, 0));
485  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(0.3, 0.6, 0));
486 
487  // setVariablePosition should update corresponding mimic joints too
488  state.setVariablePosition("joint_f", 1.0);
489  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
490  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
491  EXPECT_TRUE(state.dirtyLinkTransforms());
492  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
493  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
494  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
495 
496  ASSERT_EQ(g_mim->getVariableCount(), 2u);
497  double gstate[2];
498  state.copyJointGroupPositions(g_mim, gstate);
499 
500  // setToRandomPositions() uses a different mechanism to update mimic joints
501  state.setToRandomPositions(g_mim);
502  double joint_f = state.getVariablePosition("joint_f");
503  double mim_f = state.getVariablePosition("mim_f");
504  EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
505  EXPECT_TRUE(state.dirtyLinkTransforms());
506  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
507  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.1 + mim_f, 0.5, 0));
508  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(),
509  Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
510 
511  // setJointGroupPositions() uses a different mechanism to update mimic joints
512  state.setJointGroupPositions(g_mim, gstate);
513  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
514  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
515  EXPECT_TRUE(state.dirtyLinkTransforms());
516  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
517  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
518  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
519 }
520 
521 TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits)
522 {
523  moveit::core::RobotState state(robot_model_);
524  const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
525  ASSERT_TRUE(joint_model_group);
526 
527  state.setToDefaultValues();
528 
529  std::cout << "\nVisual inspection should show NO joints out of bounds:\n";
530  state.printStatePositionsWithJointLimits(joint_model_group);
531 
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;
535  state.setJointPositions("joint_c", single_joint);
536  state.printStatePositionsWithJointLimits(joint_model_group);
537 
538  std::cout << "\nVisual inspection should show TWO joint out of bounds:\n";
539  single_joint[0] = 1.0;
540  state.setJointPositions("joint_f", single_joint);
541  state.printStatePositionsWithJointLimits(joint_model_group);
542 
543  std::cout << "\nVisual inspection should show ONE joint out of bounds:\n";
544  single_joint[0] = 0.19;
545  state.setJointPositions("joint_f", single_joint);
546  state.printStatePositionsWithJointLimits(joint_model_group);
547 }
548 
549 TEST_F(OneRobot, testInterpolation)
550 {
551  moveit::core::RobotState state_a(robot_model_);
552 
553  // Interpolate with itself
554  state_a.setToDefaultValues();
555  moveit::core::RobotState state_b(state_a);
556  moveit::core::RobotState interpolated_state(state_a);
557  for (size_t i = 0; i <= 10; ++i)
558  {
559  state_a.interpolate(state_b, static_cast<double>(i) / 10., interpolated_state,
560  robot_model_->getJointModelGroup("base_from_base_to_e"));
561  EXPECT_NEAR(state_a.distance(state_b), 0, EPSILON)
562  << "Interpolation between identical states yielded a different state.";
563 
564  for (const auto& link_name : robot_model_->getLinkModelNames())
565  {
566  EXPECT_FALSE(interpolated_state.getCollisionBodyTransform(link_name, 0).matrix().hasNaN())
567  << "Interpolation between identical states yielded NaN value.";
568  }
569  }
570 
571  // Some simple interpolation
572  std::map<std::string, double> joint_values;
573  joint_values["base_joint/x"] = 1.0;
574  joint_values["base_joint/y"] = 1.0;
575  state_a.setVariablePositions(joint_values);
576  joint_values["base_joint/x"] = 0.0;
577  joint_values["base_joint/y"] = 2.0;
578  state_b.setVariablePositions(joint_values);
579  EXPECT_NEAR(3 * std::sqrt(2), state_a.distance(state_b), EPSILON) << "Simple interpolation of base_joint failed.";
580 
581  state_a.interpolate(state_b, 0.5, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
582  EXPECT_NEAR(0., state_a.distance(interpolated_state) - state_b.distance(interpolated_state), EPSILON)
583  << "Simple interpolation of base_joint failed.";
584  EXPECT_NEAR(0.5, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
585  << "Simple interpolation of base_joint failed.";
586  EXPECT_NEAR(1.5, interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
587  << "Simple interpolation of base_joint failed.";
588  state_a.interpolate(state_b, 0.1, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
589  EXPECT_NEAR(0.9, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
590  << "Simple interpolation of base_joint failed.";
591  EXPECT_NEAR(1.1, interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
592  << "Simple interpolation of base_joint failed.";
593 
594  // Interpolate all the joints
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;
601  state_a.setVariablePositions(joint_values);
602 
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;
609  state_b.setVariablePositions(joint_values);
610 
611  for (size_t i = 0; i <= 5; ++i)
612  {
613  double t = static_cast<double>(i) / 5.;
614  state_a.interpolate(state_b, t, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
615  EXPECT_NEAR(10.0 * t, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
616  << "Base joint interpolation failed.";
617  EXPECT_NEAR(20.0 * (1 - t), interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
618  << "Base joint interpolation failed.";
619  if (t < 0.5)
620  {
621  EXPECT_NEAR(3 * M_PI / 4 + (M_PI / 2) * t, interpolated_state.getVariablePosition("base_joint/theta"), EPSILON)
622  << "Base joint theta interpolation failed.";
623  EXPECT_NEAR(-4 * M_PI / 5 - (2 * M_PI / 5) * t, interpolated_state.getVariablePosition("joint_a"), EPSILON)
624  << "Continuous joint interpolation failed.";
625  }
626  else
627  {
628  EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.getVariablePosition("base_joint/theta"),
629  EPSILON)
630  << "Base joint theta interpolation failed.";
631  EXPECT_NEAR(4 * M_PI / 5 + (2 * M_PI / 5) * (1 - t), interpolated_state.getVariablePosition("joint_a"), EPSILON)
632  << "Continuous joint interpolation failed.";
633  }
634  EXPECT_NEAR(0.07 * t, interpolated_state.getVariablePosition("joint_c"), EPSILON)
635  << "Interpolation of joint_c failed.";
636  EXPECT_NEAR(1 - t, interpolated_state.getVariablePosition("joint_f"), EPSILON)
637  << "Interpolation of joint_f failed.";
638  EXPECT_NEAR(1.5 * (1 - t) + 0.1, interpolated_state.getVariablePosition("mim_f"), EPSILON)
639  << "Interpolation of mimic joint mim_f failed.";
640  }
641 
642  bool nan_exception = false;
643  try
644  {
645  state_a.interpolate(state_b, 1. / 0., interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
646  }
647  catch (std::exception& e)
648  {
649  std::cout << "Caught expected exception: " << e.what() << '\n';
650  nan_exception = true;
651  }
652  EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception.";
653 }
654 
655 TEST_F(OneRobot, rigidlyConnectedParent)
656 {
657  // link_e is its own rigidly-connected parent
658  const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") };
659  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
660 
661  // link_b is rigidly connected to its parent link_a
662  const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") };
663  const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") };
664  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
665 
666  moveit::core::RobotState state(robot_model_);
667  state.setToDefaultValues();
668 
669  Eigen::Isometry3d a_to_b;
670  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
671 
672  // attach "object" with "subframe" to link_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{},
676  moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } }));
677 
678  // RobotState's version should resolve these too
679  Eigen::Isometry3d transform;
680  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object"));
681  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe"));
682 
683  // test failure cases
684  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));
685  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe"));
686  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel(""));
687  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/"));
688  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/"));
689 
690  // link names with '/' should still work as before
691  const moveit::core::LinkModel* link_with_slash{ robot_model_->getLinkModel("link/with/slash") };
692  EXPECT_TRUE(link_with_slash);
693  const moveit::core::LinkModel* rigid_parent_of_link_with_slash =
694  state.getRigidlyConnectedParentLinkModel("link/with/slash");
695  ASSERT_TRUE(rigid_parent_of_link_with_slash);
696  EXPECT_EQ("base_link", rigid_parent_of_link_with_slash->getName());
697 
698  // the last /-separated component of an object might be a subframe
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{},
703  moveit::core::FixedTransformsMap{ { "sub/frame", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } }));
704  const moveit::core::LinkModel* rigid_parent_of_object =
705  state.getRigidlyConnectedParentLinkModel("object/with/slash/sub/frame");
706  ASSERT_TRUE(rigid_parent_of_object);
707  EXPECT_EQ(rigid_parent_of_link_with_slash, rigid_parent_of_object);
708 }
709 
710 int main(int argc, char** argv)
711 {
712  testing::InitGoogleTest(&argc, argv);
713  return RUN_ALL_TESTS();
714 }
void TearDown() override
void SetUp() override
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.
Definition: link_model.h:72
const std::string & getName() const
The name of this link.
Definition: link_model.h:86
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.
Definition: robot_state.h:90
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.
Definition: robot_state.h:381
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...
Definition: robot_state.h:1380
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...
Definition: robot_state.h:1340
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.
Definition: robot_state.h:188
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...
Definition: robot_state.h:605
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...
Definition: robot_state.h:691
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:104
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.
Definition: robot_state.h:134
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.
Definition: robot_state.h:1458
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
bool dirtyLinkTransforms() const
Definition: robot_state.h:1435
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:207
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.
Definition: robot_state.h:110
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)
Definition: robot_state.h:515
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
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...
Definition: transforms.h:53
x
Definition: pick.py:64
y
Definition: pick.py:65
r
Definition: plan.py:56
int main(int argc, char **argv)
TEST(Loading, SimpleRobot)
#define EXPECT_NEAR_TRACED(...)
TEST_F(OneRobot, FK)