test_kinematic.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 
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 
70 TEST(Loading, SimpleRobot)
71 {
72  static const std::string MODEL0 = "<?xml version=\"1.0\" ?>"
73  "<robot name=\"myrobot\">"
74  " <link name=\"base_link\">"
75  " <collision name=\"base_collision\">"
76  " <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>"
77  " <geometry name=\"base_collision_geom\">"
78  " <box size=\"0.65 0.65 0.23\"/>"
79  " </geometry>"
80  " </collision>"
81  " </link>"
82  "</robot>";
83 
84  static const std::string SMODEL0 =
85  "<?xml version=\"1.0\" ?>"
86  "<robot name=\"myrobot\">"
87  "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"floating\"/>"
88  "</robot>";
89 
90  urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL0);
91  srdf::ModelSharedPtr srdfModel(new srdf::Model());
92  srdfModel->initString(*urdfModel, SMODEL0);
93 
94  EXPECT_TRUE(srdfModel->getVirtualJoints().size() == 1);
95 
96  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
97  moveit::core::RobotState state(model);
98 
99  state.setToDefaultValues();
100 
101  // make sure that this copy constructor works
102  moveit::core::RobotState new_state(state);
103 
104  EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
105 
106  EXPECT_EQ(std::string("myrobot"), model->getName());
107  EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
108 
109  const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
110  EXPECT_EQ((unsigned int)1, links.size());
111 
112  const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
113  EXPECT_EQ((unsigned int)1, joints.size());
114 
115  const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
116  EXPECT_EQ((unsigned int)0, pgroups.size());
117 }
118 
119 TEST(LoadingAndFK, SimpleRobot)
120 {
121  static const std::string MODEL1 =
122  "<?xml version=\"1.0\" ?>"
123  "<robot name=\"myrobot\">"
124  "<link name=\"base_link\">"
125  " <inertial>"
126  " <mass value=\"2.81\"/>"
127  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
128  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
129  " </inertial>"
130  " <collision name=\"my_collision\">"
131  " <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
132  " <geometry>"
133  " <box size=\"1 2 1\" />"
134  " </geometry>"
135  " </collision>"
136  " <visual>"
137  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
138  " <geometry>"
139  " <box size=\"1 2 1\" />"
140  " </geometry>"
141  " </visual>"
142  "</link>"
143  "</robot>";
144 
145  static const std::string SMODEL1 =
146  "<?xml version=\"1.0\" ?>"
147  "<robot name=\"myrobot\">"
148  "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
149  "<group name=\"base\">"
150  "<joint name=\"base_joint\"/>"
151  "</group>"
152  "</robot>";
153 
154  urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL1);
155 
156  srdf::ModelSharedPtr srdfModel(new srdf::Model());
157  srdfModel->initString(*urdfModel, SMODEL1);
158 
159  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
160  moveit::core::RobotState state(model);
161 
162  EXPECT_EQ((unsigned int)3, state.getVariableCount());
163 
164  state.setToDefaultValues();
165 
166  EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
167  EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
168 
169  std::map<std::string, double> joint_values;
170  joint_values["base_joint/x"] = 10.0;
171  joint_values["base_joint/y"] = 8.0;
172 
173  // testing incomplete state
174  std::vector<std::string> missing_states;
175  state.setVariablePositions(joint_values, missing_states);
176  ASSERT_EQ(missing_states.size(), 1);
177  EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
178  joint_values["base_joint/theta"] = 0.1;
179 
180  state.setVariablePositions(joint_values, missing_states);
181  ASSERT_EQ(missing_states.size(), 0);
182 
183  EXPECT_NEAR(10.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
184  EXPECT_NEAR(8.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
185  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
186 
187  state.setVariableAcceleration("base_joint/x", 0.0);
188 
189  // making sure that values get copied
190  moveit::core::RobotState* new_state = new moveit::core::RobotState(state);
191  EXPECT_NEAR(10.0, new_state->getGlobalLinkTransform("base_link").translation().x(), 1e-5);
192  EXPECT_NEAR(8.0, new_state->getGlobalLinkTransform("base_link").translation().y(), 1e-5);
193  EXPECT_NEAR(0.0, new_state->getGlobalLinkTransform("base_link").translation().z(), 1e-5);
194  delete new_state;
195 
196  std::vector<double> jv(state.getVariableCount(), 0.0);
197  jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 10.0;
198  jv[state.getRobotModel()->getVariableIndex("base_joint/y")] = 8.0;
199  jv[state.getRobotModel()->getVariableIndex("base_joint/theta")] = 0.0;
200 
201  state.setVariablePositions(jv);
202  EXPECT_NEAR(10.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
203  EXPECT_NEAR(8.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
204  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
205 }
206 
207 TEST(FK, OneRobot)
208 {
209  static const std::string MODEL2 =
210  "<?xml version=\"1.0\" ?>"
211  "<robot name=\"one_robot\">"
212  "<link name=\"base_link\">"
213  " <inertial>"
214  " <mass value=\"2.81\"/>"
215  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
216  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
217  " </inertial>"
218  " <collision name=\"my_collision\">"
219  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
220  " <geometry>"
221  " <box size=\"1 2 1\" />"
222  " </geometry>"
223  " </collision>"
224  " <visual>"
225  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
226  " <geometry>"
227  " <box size=\"1 2 1\" />"
228  " </geometry>"
229  " </visual>"
230  "</link>"
231  "<joint name=\"joint_a\" type=\"continuous\">"
232  " <axis xyz=\"0 0 1\"/>"
233  " <parent link=\"base_link\"/>"
234  " <child link=\"link_a\"/>"
235  " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
236  "</joint>"
237  "<link name=\"link_a\">"
238  " <inertial>"
239  " <mass value=\"1.0\"/>"
240  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
241  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
242  " </inertial>"
243  " <collision>"
244  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
245  " <geometry>"
246  " <box size=\"1 2 1\" />"
247  " </geometry>"
248  " </collision>"
249  " <visual>"
250  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
251  " <geometry>"
252  " <box size=\"1 2 1\" />"
253  " </geometry>"
254  " </visual>"
255  "</link>"
256  "<joint name=\"joint_b\" type=\"fixed\">"
257  " <parent link=\"link_a\"/>"
258  " <child link=\"link_b\"/>"
259  " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
260  "</joint>"
261  "<link name=\"link_b\">"
262  " <inertial>"
263  " <mass value=\"1.0\"/>"
264  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
265  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
266  " </inertial>"
267  " <collision>"
268  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
269  " <geometry>"
270  " <box size=\"1 2 1\" />"
271  " </geometry>"
272  " </collision>"
273  " <visual>"
274  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
275  " <geometry>"
276  " <box size=\"1 2 1\" />"
277  " </geometry>"
278  " </visual>"
279  "</link>"
280  " <joint name=\"joint_c\" type=\"prismatic\">"
281  " <axis xyz=\"1 0 0\"/>"
282  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
283  " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
284  "soft_upper_limit=\"0.089\"/>"
285  " <parent link=\"link_b\"/>"
286  " <child link=\"link_c\"/>"
287  " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
288  " </joint>"
289  "<link name=\"link_c\">"
290  " <inertial>"
291  " <mass value=\"1.0\"/>"
292  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
293  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
294  " </inertial>"
295  " <collision>"
296  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
297  " <geometry>"
298  " <box size=\"1 2 1\" />"
299  " </geometry>"
300  " </collision>"
301  " <visual>"
302  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
303  " <geometry>"
304  " <box size=\"1 2 1\" />"
305  " </geometry>"
306  " </visual>"
307  "</link>"
308  " <joint name=\"mim_f\" type=\"prismatic\">"
309  " <axis xyz=\"1 0 0\"/>"
310  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
311  " <parent link=\"link_c\"/>"
312  " <child link=\"link_d\"/>"
313  " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
314  " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
315  " </joint>"
316  " <joint name=\"joint_f\" type=\"prismatic\">"
317  " <axis xyz=\"1 0 0\"/>"
318  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
319  " <parent link=\"link_d\"/>"
320  " <child link=\"link_e\"/>"
321  " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
322  " </joint>"
323  "<link name=\"link_d\">"
324  " <collision>"
325  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
326  " <geometry>"
327  " <box size=\"1 2 1\" />"
328  " </geometry>"
329  " </collision>"
330  " <visual>"
331  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
332  " <geometry>"
333  " <box size=\"1 2 1\" />"
334  " </geometry>"
335  " </visual>"
336  "</link>"
337  "<link name=\"link_e\">"
338  " <collision>"
339  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
340  " <geometry>"
341  " <box size=\"1 2 1\" />"
342  " </geometry>"
343  " </collision>"
344  " <visual>"
345  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
346  " <geometry>"
347  " <box size=\"1 2 1\" />"
348  " </geometry>"
349  " </visual>"
350  "</link>"
351  "</robot>";
352 
353  static const std::string SMODEL2 =
354  "<?xml version=\"1.0\" ?>"
355  "<robot name=\"one_robot\">"
356  "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
357  "<group name=\"base_from_joints\">"
358  "<joint name=\"base_joint\"/>"
359  "<joint name=\"joint_a\"/>"
360  "<joint name=\"joint_c\"/>"
361  "</group>"
362  "<group name=\"mim_joints\">"
363  "<joint name=\"joint_f\"/>"
364  "<joint name=\"mim_f\"/>"
365  "</group>"
366  "<group name=\"base_with_subgroups\">"
367  "<group name=\"base_from_base_to_tip\"/>"
368  "<joint name=\"joint_c\"/>"
369  "</group>"
370  "<group name=\"base_from_base_to_tip\">"
371  "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
372  "<joint name=\"base_joint\"/>"
373  "</group>"
374  "<group name=\"base_with_bad_subgroups\">"
375  "<group name=\"error\"/>"
376  "</group>"
377  "</robot>";
378 
379  urdf::ModelInterfaceSharedPtr urdfModel = urdf::parseURDF(MODEL2);
380 
381  srdf::ModelSharedPtr srdfModel(new srdf::Model());
382  srdfModel->initString(*urdfModel, SMODEL2);
383 
384  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
385 
386  // testing that the two planning groups are the same
387  const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
388  const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
389  const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
390  const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
391  const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
392 
393  ASSERT_TRUE(g_one != NULL);
394  ASSERT_TRUE(g_two != NULL);
395  ASSERT_TRUE(g_three != NULL);
396  ASSERT_TRUE(g_four == NULL);
397 
398  // joint_b is a fixed joint, so no one should have it
399  ASSERT_EQ(g_one->getJointModelNames().size(), 3);
400  ASSERT_EQ(g_two->getJointModelNames().size(), 3);
401  ASSERT_EQ(g_three->getJointModelNames().size(), 4);
402  ASSERT_EQ(g_mim->getJointModelNames().size(), 2);
403 
404  // only the links in between the joints, and the children of the leafs
405  ASSERT_EQ(g_one->getLinkModelNames().size(), 3);
406  // g_two only has three links
407  ASSERT_EQ(g_two->getLinkModelNames().size(), 3);
408  ASSERT_EQ(g_three->getLinkModelNames().size(), 4);
409 
410  std::vector<std::string> jmn = g_one->getJointModelNames();
411  std::sort(jmn.begin(), jmn.end());
412  EXPECT_EQ(jmn[0], "base_joint");
413  EXPECT_EQ(jmn[1], "joint_a");
414  EXPECT_EQ(jmn[2], "joint_c");
415  jmn = g_two->getJointModelNames();
416  std::sort(jmn.begin(), jmn.end());
417  EXPECT_EQ(jmn[0], "base_joint");
418  EXPECT_EQ(jmn[1], "joint_a");
419  EXPECT_EQ(jmn[2], "joint_b");
420  jmn = g_three->getJointModelNames();
421  std::sort(jmn.begin(), jmn.end());
422  EXPECT_EQ(jmn[0], "base_joint");
423  EXPECT_EQ(jmn[1], "joint_a");
424  EXPECT_EQ(jmn[2], "joint_b");
425  EXPECT_EQ(jmn[3], "joint_c");
426 
427  // but they should have the same links to be updated
428  ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6);
429  ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6);
430  ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6);
431 
432  EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(), "base_link");
433  EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(), "link_a");
434  EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(), "link_b");
435  EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(), "link_c");
436 
437  EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(), "base_link");
438  EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(), "link_a");
439  EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(), "link_b");
440  EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(), "link_c");
441 
442  EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(), "base_link");
443  EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(), "link_a");
444  EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(), "link_b");
445  EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(), "link_c");
446 
447  // bracketing so the state gets destroyed before we bring down the model
448 
449  moveit::core::RobotState state(model);
450 
451  EXPECT_EQ((unsigned int)7, state.getVariableCount());
452 
453  state.setToDefaultValues();
454 
455  std::map<std::string, double> joint_values;
456  joint_values["base_joint/x"] = 1.0;
457  joint_values["base_joint/y"] = 1.0;
458  joint_values["base_joint/theta"] = 0.5;
459  joint_values["joint_a"] = -0.5;
460  joint_values["joint_c"] = 0.08;
461  state.setVariablePositions(joint_values);
462 
463  EXPECT_NEAR(1.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
464  EXPECT_NEAR(1.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
465  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
466  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5);
467  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5);
468  EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5);
469  EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5);
470 
471  EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_a").translation().x(), 1e-5);
472  EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_a").translation().y(), 1e-5);
473  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_a").translation().z(), 1e-5);
474  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5);
475  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5);
476  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5);
477  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5);
478 
479  EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_b").translation().x(), 1e-5);
480  EXPECT_NEAR(1.5, state.getGlobalLinkTransform("link_b").translation().y(), 1e-5);
481  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_b").translation().z(), 1e-5);
482  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5);
483  EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5);
484  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5);
485  EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5);
486 
487  EXPECT_NEAR(1.08, state.getGlobalLinkTransform("link_c").translation().x(), 1e-5);
488  EXPECT_NEAR(1.4, state.getGlobalLinkTransform("link_c").translation().y(), 1e-5);
489  EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_c").translation().z(), 1e-5);
490  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5);
491  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5);
492  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5);
493  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5);
494 
495  EXPECT_TRUE(state.satisfiesBounds());
496 
497  std::map<std::string, double> upd_a;
498  upd_a["joint_a"] = 0.2;
499  state.setVariablePositions(upd_a);
500  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
501  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
502  state.enforceBounds();
503  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
504 
505  upd_a["joint_a"] = 3.2;
506  state.setVariablePositions(upd_a);
507  EXPECT_FALSE(state.satisfiesBounds(model->getJointModel("joint_a")));
508  EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
509  state.enforceBounds();
510  EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
511  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
512 }
513 
514 int main(int argc, char** argv)
515 {
516  testing::InitGoogleTest(&argc, argv);
517  return RUN_ALL_TESTS();
518 }
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.
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1308
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
#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...
int main(int argc, char **argv)
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:68
double y
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
#define EXPECT_FALSE(args)
#define EXPECT_EQ(a, b)
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 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:97
TEST(Loading, SimpleRobot)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
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:365
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:200
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:103
double x
#define EXPECT_TRUE(args)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44