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>
41 #include <gtest/gtest.h>
42 #include <sstream>
43 #include <algorithm>
44 #include <ctype.h>
45 
46 static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2)
47 {
48  unsigned int i1 = 0;
49  unsigned int i2 = 0;
50  while (i1 < s1.size() && isspace(s1[i1]))
51  i1++;
52  while (i2 < s2.size() && isspace(s2[i2]))
53  i2++;
54  while (i1 < s1.size() && i2 < s2.size())
55  {
56  if (i1 < s1.size() && i2 < s2.size())
57  {
58  if (s1[i1] != s2[i2])
59  return false;
60  i1++;
61  i2++;
62  }
63  while (i1 < s1.size() && isspace(s1[i1]))
64  i1++;
65  while (i2 < s2.size() && isspace(s2[i2]))
66  i2++;
67  }
68  return i1 == s1.size() && i2 == s2.size();
69 }
70 static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
71  double eps = std::numeric_limits<double>::epsilon())
72 {
73  ASSERT_EQ(x.rows(), y.rows());
74  ASSERT_EQ(x.cols(), y.cols());
75  for (int r = 0; r < x.rows(); ++r)
76  for (int c = 0; c < x.cols(); ++c)
77  EXPECT_NEAR(x(r, c), y(r, c), eps) << "(r,c) = (" << r << "," << c << ")";
78 }
79 
80 // clang-format off
81 #define EXPECT_NEAR_TRACED(...) { \
82  SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
83  expect_near(__VA_ARGS__); \
84 }
85 // clang-format on
86 
87 TEST(Loading, SimpleRobot)
88 {
89  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
90  builder.addVirtualJoint("odom_combined", "base_link", "floating", "base_joint");
91  ASSERT_TRUE(builder.isValid());
92  moveit::core::RobotModelPtr model = builder.build();
93  moveit::core::RobotState state(model);
94 
95  state.setToDefaultValues();
96 
97  // make sure that this copy constructor works
98  moveit::core::RobotState new_state(state);
99 
100  EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
101 
102  EXPECT_EQ(std::string("myrobot"), model->getName());
103  EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
104 
105  const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
106  EXPECT_EQ((unsigned int)1, links.size());
107 
108  const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
109  EXPECT_EQ((unsigned int)1, joints.size());
110 
111  const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
112  EXPECT_EQ((unsigned int)0, pgroups.size());
113 }
114 
115 TEST(LoadingAndFK, SimpleRobot)
116 {
117  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
118  geometry_msgs::Pose pose;
119  tf2::toMsg(tf2::Vector3(-0.1, 0, 0), pose.position);
120  tf2::Quaternion q;
121  q.setRPY(0, 0, -1);
122  pose.orientation = tf2::toMsg(q);
123  builder.addCollisionBox("base_link", { 1, 2, 1 }, pose);
124  tf2::toMsg(tf2::Vector3(0, 0, 0), pose.position);
125  q.setRPY(0, 0, 0);
126  pose.orientation = tf2::toMsg(q);
127  builder.addVisualBox("base_link", { 1, 2, 1 }, pose);
128  tf2::toMsg(tf2::Vector3(0, 0.099, 0), pose.position);
129  q.setRPY(0, 0, 0);
130  pose.orientation = tf2::toMsg(q);
131  builder.addInertial("base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
132  builder.addVirtualJoint("odom_combined", "base_link", "planar", "base_joint");
133  builder.addGroup({}, { "base_joint" }, "base");
134 
135  ASSERT_TRUE(builder.isValid());
136  moveit::core::RobotModelPtr model = builder.build();
137  moveit::core::RobotState state(model);
138 
139  EXPECT_EQ((unsigned int)3, state.getVariableCount());
140 
141  state.setToDefaultValues();
142 
143  EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
144  EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
145 
146  std::map<std::string, double> joint_values;
147  joint_values["base_joint/x"] = 10.0;
148  joint_values["base_joint/y"] = 8.0;
149 
150  // testing incomplete state
151  std::vector<std::string> missing_states;
152  state.setVariablePositions(joint_values, missing_states);
153  ASSERT_EQ(missing_states.size(), 1u);
154  EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
155  joint_values["base_joint/theta"] = 0.1;
156 
157  state.setVariablePositions(joint_values, missing_states);
158  ASSERT_EQ(missing_states.size(), 0u);
159 
160  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
161 
162  state.setVariableAcceleration("base_joint/x", 0.0);
163 
164  // making sure that values get copied
165  moveit::core::RobotState* new_state = new moveit::core::RobotState(state);
166  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
167  delete new_state;
168 
169  std::vector<double> jv(state.getVariableCount(), 0.0);
170  jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0;
171  jv[state.getRobotModel()->getVariableIndex("base_joint/y")] = 4.0;
172  jv[state.getRobotModel()->getVariableIndex("base_joint/theta")] = 0.0;
173 
174  state.setVariablePositions(jv);
175  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(5, 4, 0));
176 }
177 
178 class OneRobot : public testing::Test
179 {
180 protected:
181  void SetUp() override
182  {
183  static const std::string MODEL2 =
184  "<?xml version=\"1.0\" ?>"
185  "<robot name=\"one_robot\">"
186  "<link name=\"base_link\">"
187  " <inertial>"
188  " <mass value=\"2.81\"/>"
189  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
190  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
191  " </inertial>"
192  " <collision name=\"my_collision\">"
193  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
194  " <geometry>"
195  " <box size=\"1 2 1\" />"
196  " </geometry>"
197  " </collision>"
198  " <visual>"
199  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
200  " <geometry>"
201  " <box size=\"1 2 1\" />"
202  " </geometry>"
203  " </visual>"
204  "</link>"
205  "<joint name=\"joint_a\" type=\"continuous\">"
206  " <axis xyz=\"0 0 1\"/>"
207  " <parent link=\"base_link\"/>"
208  " <child link=\"link_a\"/>"
209  " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
210  "</joint>"
211  "<link name=\"link_a\">"
212  " <inertial>"
213  " <mass value=\"1.0\"/>"
214  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
215  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
216  " </inertial>"
217  " <collision>"
218  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
219  " <geometry>"
220  " <box size=\"1 2 1\" />"
221  " </geometry>"
222  " </collision>"
223  " <visual>"
224  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
225  " <geometry>"
226  " <box size=\"1 2 1\" />"
227  " </geometry>"
228  " </visual>"
229  "</link>"
230  "<joint name=\"joint_b\" type=\"fixed\">"
231  " <parent link=\"link_a\"/>"
232  " <child link=\"link_b\"/>"
233  " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
234  "</joint>"
235  "<link name=\"link_b\">"
236  " <inertial>"
237  " <mass value=\"1.0\"/>"
238  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
239  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
240  " </inertial>"
241  " <collision>"
242  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
243  " <geometry>"
244  " <box size=\"1 2 1\" />"
245  " </geometry>"
246  " </collision>"
247  " <visual>"
248  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
249  " <geometry>"
250  " <box size=\"1 2 1\" />"
251  " </geometry>"
252  " </visual>"
253  "</link>"
254  " <joint name=\"joint_c\" type=\"prismatic\">"
255  " <axis xyz=\"1 0 0\"/>"
256  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
257  " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
258  "soft_upper_limit=\"0.089\"/>"
259  " <parent link=\"link_b\"/>"
260  " <child link=\"link_c\"/>"
261  " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
262  " </joint>"
263  "<link name=\"link_c\">"
264  " <inertial>"
265  " <mass value=\"1.0\"/>"
266  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
267  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
268  " </inertial>"
269  " <collision>"
270  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
271  " <geometry>"
272  " <box size=\"1 2 1\" />"
273  " </geometry>"
274  " </collision>"
275  " <visual>"
276  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
277  " <geometry>"
278  " <box size=\"1 2 1\" />"
279  " </geometry>"
280  " </visual>"
281  "</link>"
282  " <joint name=\"mim_f\" type=\"prismatic\">"
283  " <axis xyz=\"1 0 0\"/>"
284  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
285  " <parent link=\"link_c\"/>"
286  " <child link=\"link_d\"/>"
287  " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
288  " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
289  " </joint>"
290  " <joint name=\"joint_f\" type=\"prismatic\">"
291  " <axis xyz=\"1 0 0\"/>"
292  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
293  " <parent link=\"link_d\"/>"
294  " <child link=\"link_e\"/>"
295  " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
296  " </joint>"
297  "<link name=\"link_d\">"
298  " <collision>"
299  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
300  " <geometry>"
301  " <box size=\"1 2 1\" />"
302  " </geometry>"
303  " </collision>"
304  " <visual>"
305  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
306  " <geometry>"
307  " <box size=\"1 2 1\" />"
308  " </geometry>"
309  " </visual>"
310  "</link>"
311  "<link name=\"link_e\">"
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  "</robot>";
326 
327  static const std::string SMODEL2 =
328  "<?xml version=\"1.0\" ?>"
329  "<robot name=\"one_robot\">"
330  "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
331  "<group name=\"base_from_joints\">"
332  "<joint name=\"base_joint\"/>"
333  "<joint name=\"joint_a\"/>"
334  "<joint name=\"joint_c\"/>"
335  "</group>"
336  "<group name=\"mim_joints\">"
337  "<joint name=\"joint_f\"/>"
338  "<joint name=\"mim_f\"/>"
339  "</group>"
340  "<group name=\"base_with_subgroups\">"
341  "<group name=\"base_from_base_to_tip\"/>"
342  "<joint name=\"joint_c\"/>"
343  "</group>"
344  "<group name=\"base_from_base_to_tip\">"
345  "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
346  "<joint name=\"base_joint\"/>"
347  "</group>"
348  "<group name=\"base_from_base_to_e\">"
349  "<chain base_link=\"base_link\" tip_link=\"link_e\"/>"
350  "<joint name=\"base_joint\"/>"
351  "</group>"
352  "<group name=\"base_with_bad_subgroups\">"
353  "<group name=\"error\"/>"
354  "</group>"
355  "</robot>";
356 
357  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
358  srdf::ModelSharedPtr srdf_model(new srdf::Model());
359  srdf_model->initString(*urdf_model, SMODEL2);
360  robot_model_.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
361  }
362 
363  void TearDown() override
364  {
365  }
366 
367 protected:
368  moveit::core::RobotModelConstPtr robot_model_;
369 };
370 
372 {
373  moveit::core::RobotModelConstPtr model = robot_model_;
374 
375  // testing that the two planning groups are the same
376  const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
377  const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
378  const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
379  const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
380  const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
381 
382  ASSERT_TRUE(g_one != nullptr);
383  ASSERT_TRUE(g_two != nullptr);
384  ASSERT_TRUE(g_three != nullptr);
385  ASSERT_TRUE(g_four == nullptr);
386 
387  // joint_b is a fixed joint, so no one should have it
388  ASSERT_EQ(g_one->getJointModelNames().size(), 3u);
389  ASSERT_EQ(g_two->getJointModelNames().size(), 3u);
390  ASSERT_EQ(g_three->getJointModelNames().size(), 4u);
391  ASSERT_EQ(g_mim->getJointModelNames().size(), 2u);
392 
393  // only the links in between the joints, and the children of the leafs
394  ASSERT_EQ(g_one->getLinkModelNames().size(), 3u);
395  // g_two only has three links
396  ASSERT_EQ(g_two->getLinkModelNames().size(), 3u);
397  ASSERT_EQ(g_three->getLinkModelNames().size(), 4u);
398 
399  std::vector<std::string> jmn = g_one->getJointModelNames();
400  std::sort(jmn.begin(), jmn.end());
401  EXPECT_EQ(jmn[0], "base_joint");
402  EXPECT_EQ(jmn[1], "joint_a");
403  EXPECT_EQ(jmn[2], "joint_c");
404  jmn = g_two->getJointModelNames();
405  std::sort(jmn.begin(), jmn.end());
406  EXPECT_EQ(jmn[0], "base_joint");
407  EXPECT_EQ(jmn[1], "joint_a");
408  EXPECT_EQ(jmn[2], "joint_b");
409  jmn = g_three->getJointModelNames();
410  std::sort(jmn.begin(), jmn.end());
411  EXPECT_EQ(jmn[0], "base_joint");
412  EXPECT_EQ(jmn[1], "joint_a");
413  EXPECT_EQ(jmn[2], "joint_b");
414  EXPECT_EQ(jmn[3], "joint_c");
415 
416  // but they should have the same links to be updated
417  ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6u);
418  ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6u);
419  ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6u);
420 
421  EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(), "base_link");
422  EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(), "link_a");
423  EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(), "link_b");
424  EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(), "link_c");
425 
426  EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(), "base_link");
427  EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(), "link_a");
428  EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(), "link_b");
429  EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(), "link_c");
430 
431  EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(), "base_link");
432  EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(), "link_a");
433  EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(), "link_b");
434  EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(), "link_c");
435 
436  // bracketing so the state gets destroyed before we bring down the model
437 
438  moveit::core::RobotState state(model);
439 
440  EXPECT_EQ((unsigned int)7, state.getVariableCount());
441 
442  state.setToDefaultValues();
443 
444  std::map<std::string, double> joint_values;
445  joint_values["base_joint/x"] = 1.0;
446  joint_values["base_joint/y"] = 1.0;
447  joint_values["base_joint/theta"] = 0.5;
448  joint_values["joint_a"] = -0.5;
449  joint_values["joint_c"] = 0.08;
450  state.setVariablePositions(joint_values);
451 
452  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0));
453  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5);
454  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5);
455  EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5);
456  EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5);
457 
458  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0));
459  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5);
460  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5);
461  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5);
462  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5);
463 
464  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
465  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5);
466  EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5);
467  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5);
468  EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5);
469 
470  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
471  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5);
472  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5);
473  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5);
474  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5);
475 
476  EXPECT_TRUE(state.satisfiesBounds());
477 
478  std::map<std::string, double> upd_a;
479  upd_a["joint_a"] = 0.2;
480  state.setVariablePositions(upd_a);
481  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
482  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
483  state.enforceBounds();
484  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
485 
486  upd_a["joint_a"] = 3.2;
487  state.setVariablePositions(upd_a);
488  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
489  EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
490  state.enforceBounds();
491  EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
492  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
493 
494  // mimic joints
495  state.setToDefaultValues();
496  EXPECT_TRUE(state.dirtyLinkTransforms());
497  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
498  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.2, 0.5, 0));
499  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(0.3, 0.6, 0));
500 
501  // setVariablePosition should update corresponding mimic joints too
502  state.setVariablePosition("joint_f", 1.0);
503  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
504  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
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(1.7, 0.5, 0));
508  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
509 
510  ASSERT_EQ(g_mim->getVariableCount(), 2u);
511  double gstate[2];
512  state.copyJointGroupPositions(g_mim, gstate);
513 
514  // setToRandomPositions() uses a different mechanism to update mimic joints
515  state.setToRandomPositions(g_mim);
516  double joint_f = state.getVariablePosition("joint_f");
517  double mim_f = state.getVariablePosition("mim_f");
518  EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
519  EXPECT_TRUE(state.dirtyLinkTransforms());
520  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
521  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.1 + mim_f, 0.5, 0));
522  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(),
523  Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
524 
525  // setJointGroupPositions() uses a different mechanism to update mimic joints
526  state.setJointGroupPositions(g_mim, gstate);
527  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
528  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
529  EXPECT_TRUE(state.dirtyLinkTransforms());
530  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
531  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
532  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
533 }
534 
535 std::size_t generateTestTraj(std::vector<std::shared_ptr<robot_state::RobotState>>& traj,
536  const moveit::core::RobotModelConstPtr& robot_model,
537  const robot_model::JointModelGroup* joint_model_group)
538 {
539  traj.clear();
540 
541  std::shared_ptr<robot_state::RobotState> robot_state(new robot_state::RobotState(robot_model));
542  robot_state->setToDefaultValues();
543  double ja, jc;
544 
545  // 3 waypoints with default joints
546  for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix)
547  {
548  // robot_state.reset(new robot_state::RobotState(*robot_state));
549  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
550  }
551 
552  ja = robot_state->getVariablePosition("joint_a");
553  jc = robot_state->getVariablePosition("joint_c");
554 
555  // 4th waypoint with a small jump of 0.01 in revolute joint and prismatic joint. This should not be considered a jump
556  ja = ja - 0.01;
557  robot_state->setVariablePosition("joint_a", ja);
558  jc = jc - 0.01;
559  robot_state->setVariablePosition("joint_c", jc);
560  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
561 
562  // 5th waypoint with a large jump of 1.01 in first revolute joint
563  ja = ja + 1.01;
564  robot_state->setVariablePosition("joint_a", ja);
565  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
566 
567  // 6th waypoint with a large jump of 1.01 in first prismatic joint
568  jc = jc + 1.01;
569  robot_state->setVariablePosition("joint_c", jc);
570  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
571 
572  // 7th waypoint with no jump
573  traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state)));
574 
575  return traj.size();
576 }
577 
578 TEST_F(OneRobot, testGenerateTrajectory)
579 {
580  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
581  ASSERT_TRUE(joint_model_group);
582  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
583 
584  // The full trajectory should be of length 7
585  const std::size_t expected_full_traj_len = 7;
586 
587  // Generate a test trajectory
588  std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group);
589 
590  // Test the generateTestTraj still generates a trajectory of length 7
591  EXPECT_EQ(full_traj_len, expected_full_traj_len); // full traj should be 7 waypoints long
592 }
593 
594 TEST_F(OneRobot, testAbsoluteJointSpaceJump)
595 {
596  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
597  ASSERT_TRUE(joint_model_group);
598  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
599 
600  // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint
601  const std::size_t expected_revolute_jump_traj_len = 4;
602  const std::size_t expected_prismatic_jump_traj_len = 5;
603 
604  // Pre-compute expected results for tests
605  std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group);
606  const double expected_revolute_jump_fraction = (double)expected_revolute_jump_traj_len / (double)full_traj_len;
607  const double expected_prismatic_jump_fraction = (double)expected_prismatic_jump_traj_len / (double)full_traj_len;
608 
609  // Container for results
610  double fraction;
611 
612  // Direct call of absolute version
613  fraction = robot_state::RobotState::testAbsoluteJointSpaceJump(joint_model_group, traj, 1.0, 1.0);
614  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut
615  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
616 
617  // Indirect call using testJointSpaceJumps
618  generateTestTraj(traj, robot_model_, joint_model_group);
619  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(1.0, 1.0));
620  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
621  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
622 
623  // Test revolute joints
624  generateTestTraj(traj, robot_model_, joint_model_group);
625  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(1.0, 0.0));
626  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
627  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
628 
629  // Test prismatic joints
630  generateTestTraj(traj, robot_model_, joint_model_group);
631  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(0.0, 1.0));
632  EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump
633  EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01);
634 
635  // Ignore all absolute jumps
636  generateTestTraj(traj, robot_model_, joint_model_group);
637  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(0.0, 0.0));
638  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
639  EXPECT_NEAR(1.0, fraction, 0.01);
640 }
641 
642 TEST_F(OneRobot, testRelativeJointSpaceJump)
643 {
644  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
645  ASSERT_TRUE(joint_model_group);
646  std::vector<std::shared_ptr<robot_state::RobotState>> traj;
647 
648  // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4
649  const std::size_t expected_relative_jump_traj_len = 4;
650 
651  // Pre-compute expected results for tests
652  std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group);
653  const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (double)full_traj_len;
654 
655  // Container for results
656  double fraction;
657 
658  // Direct call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6.
659  fraction = robot_state::RobotState::testRelativeJointSpaceJump(joint_model_group, traj, 2.97);
660  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
661  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
662 
663  // Indirect call of relative version using testJointSpaceJumps
664  generateTestTraj(traj, robot_model_, joint_model_group);
665  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(2.97));
666  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
667  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
668 
669  // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6.
670  generateTestTraj(traj, robot_model_, joint_model_group);
671  fraction = robot_state::RobotState::testJointSpaceJump(joint_model_group, traj, robot_state::JumpThreshold(2.98));
672  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
673  EXPECT_NEAR(1.0, fraction, 0.01);
674 }
675 
676 TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits)
677 {
679  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
680  ASSERT_TRUE(joint_model_group);
681 
682  state.setToDefaultValues();
683 
684  std::cout << "\nVisual inspection should show NO joints out of bounds:" << std::endl;
685  state.printStatePositionsWithJointLimits(joint_model_group);
686 
687  std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl;
688  std::vector<double> single_joint(1);
689  single_joint[0] = -1.0;
690  state.setJointPositions("joint_c", single_joint);
691  state.printStatePositionsWithJointLimits(joint_model_group);
692 
693  std::cout << "\nVisual inspection should show TWO joint out of bounds:" << std::endl;
694  single_joint[0] = 1.0;
695  state.setJointPositions("joint_f", single_joint);
696  state.printStatePositionsWithJointLimits(joint_model_group);
697 
698  std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl;
699  single_joint[0] = 0.19;
700  state.setJointPositions("joint_f", single_joint);
701  state.printStatePositionsWithJointLimits(joint_model_group);
702 }
703 
704 int main(int argc, char** argv)
705 {
706  testing::InitGoogleTest(&argc, argv);
707  return RUN_ALL_TESTS();
708 }
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:527
moveit::core::RobotModelConstPtr robot_model_
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Core components of MoveIt!
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
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
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.
void SetUp() override
#define EXPECT_NEAR(a, b, prec)
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot&#39;s joint limits.
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.
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...
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
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)
double z
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.
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:138
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)
B toMsg(const A &a)
bool isValid()
Returns true if the building process so far has been valid.
TEST(Loading, SimpleRobot)
std::shared_ptr< Model > ModelSharedPtr
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
void TearDown() override
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1419
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::Pose origin)
Adds a visual box to a specific link.
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.
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
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
int main(int argc, char **argv)
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::Pose origin)
Adds a collision box to a specific link.
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())
void addInertial(const std::string &link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
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)
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 20 2019 03:54:37