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 */
36 #include <moveit_resources/config.h>
39 #include <urdf_parser/urdf_parser.h>
40 #include <gtest/gtest.h>
41 #include <sstream>
42 #include <algorithm>
43 #include <ctype.h>
44 
45 static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2)
46 {
47  unsigned int i1 = 0;
48  unsigned int i2 = 0;
49  while (i1 < s1.size() && isspace(s1[i1]))
50  i1++;
51  while (i2 < s2.size() && isspace(s2[i2]))
52  i2++;
53  while (i1 < s1.size() && i2 < s2.size())
54  {
55  if (i1 < s1.size() && i2 < s2.size())
56  {
57  if (s1[i1] != s2[i2])
58  return false;
59  i1++;
60  i2++;
61  }
62  while (i1 < s1.size() && isspace(s1[i1]))
63  i1++;
64  while (i2 < s2.size() && isspace(s2[i2]))
65  i2++;
66  }
67  return i1 == s1.size() && i2 == s2.size();
68 }
69 static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
70  double eps = std::numeric_limits<double>::epsilon())
71 {
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 << ")";
77 }
78 
79 // clang-format off
80 #define EXPECT_NEAR_TRACED(...) { \
81  SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
82  expect_near(__VA_ARGS__); \
83 }
84 // clang-format on
85 
86 TEST(Loading, SimpleRobot)
87 {
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\"/>"
95  " </geometry>"
96  " </collision>"
97  " </link>"
98  "</robot>";
99 
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\"/>"
104  "</robot>";
105 
106  urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL0);
107  srdf::ModelSharedPtr srdfModel(new srdf::Model());
108  srdfModel->initString(*urdfModel, SMODEL0);
109 
110  EXPECT_TRUE(srdfModel->getVirtualJoints().size() == 1);
111 
112  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
113  moveit::core::RobotState state(model);
114 
115  state.setToDefaultValues();
116 
117  // make sure that this copy constructor works
118  moveit::core::RobotState new_state(state);
119 
120  EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
121 
122  EXPECT_EQ(std::string("myrobot"), model->getName());
123  EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
124 
125  const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
126  EXPECT_EQ((unsigned int)1, links.size());
127 
128  const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
129  EXPECT_EQ((unsigned int)1, joints.size());
130 
131  const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
132  EXPECT_EQ((unsigned int)0, pgroups.size());
133 }
134 
135 TEST(LoadingAndFK, SimpleRobot)
136 {
137  static const std::string MODEL1 =
138  "<?xml version=\"1.0\" ?>"
139  "<robot name=\"myrobot\">"
140  "<link name=\"base_link\">"
141  " <inertial>"
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\"/>"
145  " </inertial>"
146  " <collision name=\"my_collision\">"
147  " <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
148  " <geometry>"
149  " <box size=\"1 2 1\" />"
150  " </geometry>"
151  " </collision>"
152  " <visual>"
153  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
154  " <geometry>"
155  " <box size=\"1 2 1\" />"
156  " </geometry>"
157  " </visual>"
158  "</link>"
159  "</robot>";
160 
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\"/>"
167  "</group>"
168  "</robot>";
169 
170  urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL1);
171 
172  srdf::ModelSharedPtr srdfModel(new srdf::Model());
173  srdfModel->initString(*urdfModel, SMODEL1);
174 
175  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
176  moveit::core::RobotState state(model);
177 
178  EXPECT_EQ((unsigned int)3, state.getVariableCount());
179 
180  state.setToDefaultValues();
181 
182  EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
183  EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
184 
185  std::map<std::string, double> joint_values;
186  joint_values["base_joint/x"] = 10.0;
187  joint_values["base_joint/y"] = 8.0;
188 
189  // testing incomplete state
190  std::vector<std::string> missing_states;
191  state.setVariablePositions(joint_values, 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;
195 
196  state.setVariablePositions(joint_values, missing_states);
197  ASSERT_EQ(missing_states.size(), 0);
198 
199  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
200 
201  state.setVariableAcceleration("base_joint/x", 0.0);
202 
203  // making sure that values get copied
204  moveit::core::RobotState* new_state = new moveit::core::RobotState(state);
205  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
206  delete new_state;
207 
208  std::vector<double> jv(state.getVariableCount(), 0.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;
212 
213  state.setVariablePositions(jv);
214  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(5, 4, 0));
215 }
216 
217 class OneRobot : public testing::Test
218 {
219 protected:
220  void SetUp() override
221  {
222  static const std::string MODEL2 =
223  "<?xml version=\"1.0\" ?>"
224  "<robot name=\"one_robot\">"
225  "<link name=\"base_link\">"
226  " <inertial>"
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\"/>"
230  " </inertial>"
231  " <collision name=\"my_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_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 \"/>"
249  "</joint>"
250  "<link name=\"link_a\">"
251  " <inertial>"
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\"/>"
255  " </inertial>"
256  " <collision>"
257  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
258  " <geometry>"
259  " <box size=\"1 2 1\" />"
260  " </geometry>"
261  " </collision>"
262  " <visual>"
263  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
264  " <geometry>"
265  " <box size=\"1 2 1\" />"
266  " </geometry>"
267  " </visual>"
268  "</link>"
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 \"/>"
273  "</joint>"
274  "<link name=\"link_b\">"
275  " <inertial>"
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\"/>"
279  " </inertial>"
280  " <collision>"
281  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
282  " <geometry>"
283  " <box size=\"1 2 1\" />"
284  " </geometry>"
285  " </collision>"
286  " <visual>"
287  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
288  " <geometry>"
289  " <box size=\"1 2 1\" />"
290  " </geometry>"
291  " </visual>"
292  "</link>"
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 \"/>"
301  " </joint>"
302  "<link name=\"link_c\">"
303  " <inertial>"
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\"/>"
307  " </inertial>"
308  " <collision>"
309  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
310  " <geometry>"
311  " <box size=\"1 2 1\" />"
312  " </geometry>"
313  " </collision>"
314  " <visual>"
315  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
316  " <geometry>"
317  " <box size=\"1 2 1\" />"
318  " </geometry>"
319  " </visual>"
320  "</link>"
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\"/>"
328  " </joint>"
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 \"/>"
335  " </joint>"
336  "<link name=\"link_d\">"
337  " <collision>"
338  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
339  " <geometry>"
340  " <box size=\"1 2 1\" />"
341  " </geometry>"
342  " </collision>"
343  " <visual>"
344  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
345  " <geometry>"
346  " <box size=\"1 2 1\" />"
347  " </geometry>"
348  " </visual>"
349  "</link>"
350  "<link name=\"link_e\">"
351  " <collision>"
352  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
353  " <geometry>"
354  " <box size=\"1 2 1\" />"
355  " </geometry>"
356  " </collision>"
357  " <visual>"
358  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
359  " <geometry>"
360  " <box size=\"1 2 1\" />"
361  " </geometry>"
362  " </visual>"
363  "</link>"
364  "</robot>";
365 
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\"/>"
374  "</group>"
375  "<group name=\"mim_joints\">"
376  "<joint name=\"joint_f\"/>"
377  "<joint name=\"mim_f\"/>"
378  "</group>"
379  "<group name=\"base_with_subgroups\">"
380  "<group name=\"base_from_base_to_tip\"/>"
381  "<joint name=\"joint_c\"/>"
382  "</group>"
383  "<group name=\"base_from_base_to_tip\">"
384  "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
385  "<joint name=\"base_joint\"/>"
386  "</group>"
387  "<group name=\"base_from_base_to_e\">"
388  "<chain base_link=\"base_link\" tip_link=\"link_e\"/>"
389  "<joint name=\"base_joint\"/>"
390  "</group>"
391  "<group name=\"base_with_bad_subgroups\">"
392  "<group name=\"error\"/>"
393  "</group>"
394  "</robot>";
395 
396  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
397  srdf::ModelSharedPtr srdf_model(new srdf::Model());
398  srdf_model->initString(*urdf_model, SMODEL2);
399  robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
400  }
401 
402  void TearDown() override
403  {
404  }
405 
406 protected:
407  moveit::core::RobotModelConstPtr robot_model;
408 };
409 
411 {
412  moveit::core::RobotModelConstPtr model = robot_model;
413 
414  // testing that the two planning groups are the same
415  const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
416  const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
417  const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
418  const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
419  const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
420 
421  ASSERT_TRUE(g_one != nullptr);
422  ASSERT_TRUE(g_two != nullptr);
423  ASSERT_TRUE(g_three != nullptr);
424  ASSERT_TRUE(g_four == nullptr);
425 
426  // joint_b is a fixed joint, so no one should have it
427  ASSERT_EQ(g_one->getJointModelNames().size(), 3);
428  ASSERT_EQ(g_two->getJointModelNames().size(), 3);
429  ASSERT_EQ(g_three->getJointModelNames().size(), 4);
430  ASSERT_EQ(g_mim->getJointModelNames().size(), 2);
431 
432  // only the links in between the joints, and the children of the leafs
433  ASSERT_EQ(g_one->getLinkModelNames().size(), 3);
434  // g_two only has three links
435  ASSERT_EQ(g_two->getLinkModelNames().size(), 3);
436  ASSERT_EQ(g_three->getLinkModelNames().size(), 4);
437 
438  std::vector<std::string> jmn = g_one->getJointModelNames();
439  std::sort(jmn.begin(), jmn.end());
440  EXPECT_EQ(jmn[0], "base_joint");
441  EXPECT_EQ(jmn[1], "joint_a");
442  EXPECT_EQ(jmn[2], "joint_c");
443  jmn = g_two->getJointModelNames();
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");
448  jmn = g_three->getJointModelNames();
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");
454 
455  // but they should have the same links to be updated
456  ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6);
457  ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6);
458  ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6);
459 
460  EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(), "base_link");
461  EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(), "link_a");
462  EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(), "link_b");
463  EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(), "link_c");
464 
465  EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(), "base_link");
466  EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(), "link_a");
467  EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(), "link_b");
468  EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(), "link_c");
469 
470  EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(), "base_link");
471  EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(), "link_a");
472  EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(), "link_b");
473  EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(), "link_c");
474 
475  // bracketing so the state gets destroyed before we bring down the model
476 
477  moveit::core::RobotState state(model);
478 
479  EXPECT_EQ((unsigned int)7, state.getVariableCount());
480 
481  state.setToDefaultValues();
482 
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);
490 
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);
496 
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);
502 
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);
508 
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);
514 
515  EXPECT_TRUE(state.satisfiesBounds());
516 
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);
524 
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")));
532 
533  // mimic joints
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));
539 
540  // setVariablePosition should update corresponding mimic joints too
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));
548 
549  ASSERT_EQ(g_mim->getVariableCount(), 2);
550  double gstate[2];
551  state.copyJointGroupPositions(g_mim, gstate);
552 
553  // setToRandomPositions() uses a different mechanism to update mimic joints
554  state.setToRandomPositions(g_mim);
555  double joint_f = state.getVariablePosition("joint_f");
556  double mim_f = state.getVariablePosition("mim_f");
557  EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
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));
561  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(),
562  Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
563 
564  // setJointGroupPositions() uses a different mechanism to update mimic joints
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));
572 }
573 
574 std::size_t generateTestTraj(std::vector<std::shared_ptr<robot_state::RobotState>>& traj,
575  const moveit::core::RobotModelConstPtr& robot_model,
576  const robot_model::JointModelGroup* joint_model_group)
577 {
578  traj.clear();
579 
580  std::shared_ptr<robot_state::RobotState> robot_state(new robot_state::RobotState(robot_model));
581  robot_state->setToDefaultValues();
582  double ja, jc;
583 
584  // 3 waypoints with default joints
585  for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix)
586  {
587  // robot_state.reset(new robot_state::RobotState(*robot_state));
588  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
589  }
590 
591  ja = robot_state->getVariablePosition("joint_a");
592  jc = robot_state->getVariablePosition("joint_c");
593 
594  // 4th waypoint with a small jump of 0.01 in revolute joint and prismatic joint. This should not be considered a jump
595  ja = ja - 0.01;
596  robot_state->setVariablePosition("joint_a", ja);
597  jc = jc - 0.01;
598  robot_state->setVariablePosition("joint_c", jc);
599  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
600 
601  // 5th waypoint with a large jump of 1.01 in first revolute joint
602  ja = ja + 1.01;
603  robot_state->setVariablePosition("joint_a", ja);
604  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
605 
606  // 6th waypoint with a large jump of 1.01 in first prismatic joint
607  jc = jc + 1.01;
608  robot_state->setVariablePosition("joint_c", jc);
609  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
610 
611  // 7th waypoint with no jump
612  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
613 
614  return traj.size();
615 }
616 
617 TEST_F(OneRobot, testGenerateTrajectory)
618 {
619  const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup("base_from_base_to_e");
620  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
621 
622  // The full trajectory should be of length 7
623  const std::size_t expected_full_traj_len = 7;
624 
625  // Generate a test trajectory
626  std::size_t full_traj_len = generateTestTraj(traj, robot_model, joint_model_group);
627 
628  // Test the generateTestTraj still generates a trajectory of length 7
629  EXPECT_EQ(full_traj_len, expected_full_traj_len); // full traj should be 7 waypoints long
630 }
631 
632 TEST_F(OneRobot, testAbsoluteJointSpaceJump)
633 {
634  const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup("base_from_base_to_e");
635  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
636 
637  // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint
638  const std::size_t expected_revolute_jump_traj_len = 4;
639  const std::size_t expected_prismatic_jump_traj_len = 5;
640 
641  // Pre-compute expected results for tests
642  std::size_t full_traj_len = generateTestTraj(traj, robot_model, joint_model_group);
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;
645 
646  // Container for results
647  double fraction;
648 
649  // Direct call of absolute version
650  fraction = robot_state::RobotState::testAbsoluteJointSpaceJump(joint_model_group, traj, 1.0, 1.0);
651  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut
652  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
653 
654  // Indirect call using testJointSpaceJumps
655  generateTestTraj(traj, robot_model, joint_model_group);
656  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(1.0, 1.0));
657  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
658  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
659 
660  // Test revolute joints
661  generateTestTraj(traj, robot_model, joint_model_group);
662  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(1.0, 0.0));
663  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
664  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
665 
666  // Test prismatic joints
667  generateTestTraj(traj, robot_model, joint_model_group);
668  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(0.0, 1.0));
669  EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump
670  EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01);
671 
672  // Ignore all absolute jumps
673  generateTestTraj(traj, robot_model, joint_model_group);
674  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(0.0, 0.0));
675  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
676  EXPECT_NEAR(1.0, fraction, 0.01);
677 }
678 
679 TEST_F(OneRobot, testRelativeJointSpaceJump)
680 {
681  const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup("base_from_base_to_e");
682  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
683 
684  // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4
685  const std::size_t expected_relative_jump_traj_len = 4;
686 
687  // Pre-compute expected results for tests
688  std::size_t full_traj_len = generateTestTraj(traj, robot_model, joint_model_group);
689  const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (double)full_traj_len;
690 
691  // Container for results
692  double fraction;
693 
694  // Direct call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6.
695  fraction = robot_state::RobotState::testRelativeJointSpaceJump(joint_model_group, traj, 2.97);
696  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
697  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
698 
699  // Indirect call of relative version using testJointSpaceJumps
700  generateTestTraj(traj, robot_model, joint_model_group);
701  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(2.97));
702  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
703  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
704 
705  // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6.
706  generateTestTraj(traj, robot_model, joint_model_group);
707  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(2.98));
708  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
709  EXPECT_NEAR(1.0, fraction, 0.01);
710 }
711 
712 int main(int argc, char** argv)
713 {
714  testing::InitGoogleTest(&argc, argv);
715  return RUN_ALL_TESTS();
716 }
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)
Definition: robot_state.h:1368
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
void SetUp() override
#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...
Definition: robot_model.h:67
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...
#define EXPECT_EQ(a, b)
Struct for containing jump_threshold.
Definition: robot_state.h:71
static double testJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
TEST_F(OneRobot, FK)
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...
double z
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.
Definition: robot_state.h:138
TEST(Loading, SimpleRobot)
void TearDown() override
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
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:406
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:241
int main(int argc, char **argv)
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
r
#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)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05