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 <gmock/gmock-matchers.h>
43 #include <sstream>
44 #include <algorithm>
45 #include <limits>
46 #include <ctype.h>
47 
48 namespace
49 {
50 constexpr double EPSILON{ 1.e-9 };
51 }
52 
53 #if 0 // unused function
54 static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2)
55 {
56  unsigned int i1 = 0;
57  unsigned int i2 = 0;
58  while (i1 < s1.size() && isspace(s1[i1]))
59  i1++;
60  while (i2 < s2.size() && isspace(s2[i2]))
61  i2++;
62  while (i1 < s1.size() && i2 < s2.size())
63  {
64  if (i1 < s1.size() && i2 < s2.size())
65  {
66  if (s1[i1] != s2[i2])
67  return false;
68  i1++;
69  i2++;
70  }
71  while (i1 < s1.size() && isspace(s1[i1]))
72  i1++;
73  while (i2 < s2.size() && isspace(s2[i2]))
74  i2++;
75  }
76  return i1 == s1.size() && i2 == s2.size();
77 }
78 #endif
79 
80 static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
81  double eps = std::numeric_limits<double>::epsilon())
82 {
83  ASSERT_EQ(x.rows(), y.rows());
84  ASSERT_EQ(x.cols(), y.cols());
85  for (int r = 0; r < x.rows(); ++r)
86  for (int c = 0; c < x.cols(); ++c)
87  EXPECT_NEAR(x(r, c), y(r, c), eps) << "(r,c) = (" << r << "," << c << ")";
88 }
89 
90 // clang-format off
91 #define EXPECT_NEAR_TRACED(...) { \
92  SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
93  expect_near(__VA_ARGS__); \
94 }
95 // clang-format on
96 
97 TEST(Loading, SimpleRobot)
98 {
99  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
100  builder.addVirtualJoint("odom_combined", "base_link", "floating", "base_joint");
101  ASSERT_TRUE(builder.isValid());
102  moveit::core::RobotModelPtr model = builder.build();
103  moveit::core::RobotState state(model);
104 
105  state.setToDefaultValues();
106 
107  // make sure that this copy constructor works
108  moveit::core::RobotState new_state(state);
109 
110  EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
111 
112  EXPECT_EQ(std::string("myrobot"), model->getName());
113  EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
114 
115  const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
116  EXPECT_EQ((unsigned int)1, links.size());
117 
118  const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
119  EXPECT_EQ((unsigned int)1, joints.size());
120 
121  const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
122  EXPECT_EQ((unsigned int)0, pgroups.size());
123 }
124 
125 TEST(LoadingAndFK, SimpleRobot)
126 {
127  moveit::core::RobotModelBuilder builder("myrobot", "base_link");
128  geometry_msgs::Pose pose;
129  tf2::toMsg(tf2::Vector3(-0.1, 0, 0), pose.position);
130  tf2::Quaternion q;
131  q.setRPY(0, 0, -1);
132  pose.orientation = tf2::toMsg(q);
133  builder.addCollisionBox("base_link", { 1, 2, 1 }, pose);
134  tf2::toMsg(tf2::Vector3(0, 0, 0), pose.position);
135  q.setRPY(0, 0, 0);
136  pose.orientation = tf2::toMsg(q);
137  builder.addVisualBox("base_link", { 1, 2, 1 }, pose);
138  tf2::toMsg(tf2::Vector3(0, 0.099, 0), pose.position);
139  q.setRPY(0, 0, 0);
140  pose.orientation = tf2::toMsg(q);
141  builder.addInertial("base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
142  builder.addVirtualJoint("odom_combined", "base_link", "planar", "base_joint");
143  builder.addGroup({}, { "base_joint" }, "base");
144 
145  ASSERT_TRUE(builder.isValid());
146  moveit::core::RobotModelPtr model = builder.build();
147  moveit::core::RobotState state(model);
148 
149  EXPECT_EQ((unsigned int)3, state.getVariableCount());
150 
151  state.setToDefaultValues();
152 
153  EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
154  EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
155 
156  std::map<std::string, double> joint_values;
157  joint_values["base_joint/x"] = 10.0;
158  joint_values["base_joint/y"] = 8.0;
159 
160  // testing incomplete state
161  std::vector<std::string> missing_states;
162  state.setVariablePositions(joint_values, missing_states);
163  ASSERT_EQ(missing_states.size(), 1u);
164  EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
165  joint_values["base_joint/theta"] = 0.1;
166 
167  state.setVariablePositions(joint_values, missing_states);
168  ASSERT_EQ(missing_states.size(), 0u);
169 
170  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
171 
172  state.setVariableAcceleration("base_joint/x", 0.0);
173 
174  const auto new_state = std::make_unique<moveit::core::RobotState>(state); // making sure that values get copied
175  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
176 
177  std::vector<double> jv(state.getVariableCount(), 0.0);
178  jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0;
179  jv[state.getRobotModel()->getVariableIndex("base_joint/y")] = 4.0;
180  jv[state.getRobotModel()->getVariableIndex("base_joint/theta")] = 0.0;
181 
182  state.setVariablePositions(jv);
183  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(5, 4, 0));
184 }
185 
186 TEST(Init, FixedJoints)
187 {
188  char const* const urdf_description = R"(
189 <robot name="minibot">
190  <link name="root"/>
191  <link name="link1"/>
192  <link name="link2"/>
193 
194  <joint name="link1_joint" type="prismatic">
195  <parent link="root"/>
196  <child link="link1"/>
197  <limit effort="1" velocity="1" lower="0" upper="1"/>
198  </joint>
199 
200  <joint name="link2_joint" type="fixed">
201  <parent link="link1"/>
202  <child link="link2"/>
203  </joint>
204 </robot>
205 )";
206 
207  char const* const srdf_description = R"(
208 <robot name="minibot">
209  <virtual_joint name="world_to_root" type="fixed" parent_frame="world" child_link="root"/>
210 </robot>
211 )";
212 
213  auto urdf = std::make_shared<urdf::Model>();
214  ASSERT_TRUE(urdf->initString(urdf_description));
215  auto srdf = std::make_shared<srdf::Model>();
216  ASSERT_TRUE(srdf->initString(*urdf, srdf_description));
217  moveit::core::RobotModelConstPtr model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
218  moveit::core::RobotState state{ model };
219  state.setJointPositions("link1_joint", { 4.2 });
220  state.update();
221 
222  const auto& cstate = state;
223  EXPECT_NEAR_TRACED(cstate.getGlobalLinkTransform("link1").translation(), Eigen::Vector3d(4.2, 0, 0));
224  EXPECT_NEAR_TRACED(cstate.getGlobalLinkTransform("link2").translation(), Eigen::Vector3d(4.2, 0, 0));
225  EXPECT_NEAR_TRACED(cstate.getJointTransform("link1_joint").translation(), Eigen::Vector3d(4.2, 0, 0));
226  EXPECT_FALSE(cstate.dirtyJointTransform(model->getJointModel("link1_joint")));
227  EXPECT_FALSE(cstate.dirtyJointTransform(model->getJointModel("link2_joint")));
228  EXPECT_NEAR_TRACED(cstate.getJointTransform("link2_joint").translation(), Eigen::Vector3d(0, 0, 0));
229  std::cout << cstate << std::endl;
230 }
231 
232 class OneRobot : public testing::Test
233 {
234 protected:
235  void SetUp() override
236  {
237  static const std::string MODEL2 = R"(
238 <?xml version="1.0" ?>
239 <robot name="one_robot">
240 <link name="base_link">
241  <inertial>
242  <mass value="2.81"/>
243  <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
244  <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
245  </inertial>
246  <collision name="my_collision">
247  <origin rpy="0 0 0" xyz="0 0 0"/>
248  <geometry>
249  <box size="1 2 1" />
250  </geometry>
251  </collision>
252  <visual>
253  <origin rpy="0 0 0" xyz="0.0 0 0"/>
254  <geometry>
255  <box size="1 2 1" />
256  </geometry>
257  </visual>
258 </link>
259 <joint name="joint_a" type="continuous">
260  <axis xyz="0 0 1"/>
261  <parent link="base_link"/>
262  <child link="link_a"/>
263  <origin rpy=" 0.0 0 0 " xyz="0.0 0 0 "/>
264 </joint>
265 <link name="link_a">
266  <inertial>
267  <mass value="1.0"/>
268  <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
269  <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
270  </inertial>
271  <collision>
272  <origin rpy="0 0 0" xyz="0 0 0"/>
273  <geometry>
274  <box size="1 2 1" />
275  </geometry>
276  </collision>
277  <visual>
278  <origin rpy="0 0 0" xyz="0.0 0 0"/>
279  <geometry>
280  <box size="1 2 1" />
281  </geometry>
282  </visual>
283 </link>
284 <joint name="joint_b" type="fixed">
285  <parent link="link_a"/>
286  <child link="link_b"/>
287  <origin rpy=" 0.0 -0.42 0 " xyz="0.0 0.5 0 "/>
288 </joint>
289 <link name="link_b">
290  <inertial>
291  <mass value="1.0"/>
292  <origin rpy="0 0 0" xyz="0.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="joint_c" type="prismatic">
309  <axis xyz="1 0 0"/>
310  <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
311  <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0"
312 soft_upper_limit="0.089"/>
313  <parent link="link_b"/>
314  <child link="link_c"/>
315  <origin rpy=" 0.0 0.42 0.0 " xyz="0.0 -0.1 0 "/>
316  </joint>
317 <link name="link_c">
318  <inertial>
319  <mass value="1.0"/>
320  <origin rpy="0 0 0" xyz="0.0 0 .0"/>
321  <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
322  </inertial>
323  <collision>
324  <origin rpy="0 0 0" xyz="0 0 0"/>
325  <geometry>
326  <box size="1 2 1" />
327  </geometry>
328  </collision>
329  <visual>
330  <origin rpy="0 0 0" xyz="0.0 0 0"/>
331  <geometry>
332  <box size="1 2 1" />
333  </geometry>
334  </visual>
335 </link>
336  <joint name="mim_f" type="prismatic">
337  <axis xyz="1 0 0"/>
338  <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
339  <parent link="link_c"/>
340  <child link="link_d"/>
341  <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
342  <mimic joint="joint_f" multiplier="1.5" offset="0.1"/>
343  </joint>
344  <joint name="joint_f" type="prismatic">
345  <axis xyz="1 0 0"/>
346  <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
347  <parent link="link_d"/>
348  <child link="link_e"/>
349  <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
350  </joint>
351 <link name="link_d">
352  <collision>
353  <origin rpy="0 0 0" xyz="0 0 0"/>
354  <geometry>
355  <box size="1 2 1" />
356  </geometry>
357  </collision>
358  <visual>
359  <origin rpy="0 1 0" xyz="0 0.1 0"/>
360  <geometry>
361  <box size="1 2 1" />
362  </geometry>
363  </visual>
364 </link>
365 <link name="link_e">
366  <collision>
367  <origin rpy="0 0 0" xyz="0 0 0"/>
368  <geometry>
369  <box size="1 2 1" />
370  </geometry>
371  </collision>
372  <visual>
373  <origin rpy="0 1 0" xyz="0 0.1 0"/>
374  <geometry>
375  <box size="1 2 1" />
376  </geometry>
377  </visual>
378 </link>
379 <link name="link/with/slash" />
380 <joint name="joint_link_with_slash" type="fixed">
381  <parent link="base_link"/>
382  <child link="link/with/slash"/>
383  <origin rpy="0 0 0" xyz="0 0 0"/>
384 </joint>
385 </robot>
386 )";
387  static const std::string SMODEL2 = R"(
388 <?xml version="1.0" ?>
389 <robot name="one_robot">
390 <virtual_joint name="base_joint" child_link="base_link" parent_frame="odom_combined" type="planar"/>
391 <group name="base_from_joints">
392 <joint name="base_joint"/>
393 <joint name="joint_a"/>
394 <joint name="joint_c"/>
395 </group>
396 <group name="mim_joints">
397 <joint name="joint_f"/>
398 <joint name="mim_f"/>
399 </group>
400 <group name="base_with_subgroups">
401 <group name="base_from_base_to_tip"/>
402 <joint name="joint_c"/>
403 </group>
404 <group name="base_from_base_to_tip">
405 <chain base_link="base_link" tip_link="link_b"/>
406 <joint name="base_joint"/>
407 </group>
408 <group name="base_from_base_to_e">
409 <chain base_link="base_link" tip_link="link_e"/>
410 <joint name="base_joint"/>
411 </group>
412 <group name="base_with_bad_subgroups">
413 <group name="error"/>
414 </group>
415 </robot>
416 )";
417 
418  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
419  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
420  srdf_model->initString(*urdf_model, SMODEL2);
421  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
422  }
423 
424  void TearDown() override
425  {
426  }
427 
428 protected:
429  moveit::core::RobotModelConstPtr robot_model_;
430 };
431 
433 {
434  moveit::core::RobotModelConstPtr model = robot_model_;
435 
436  // testing that the two planning groups are the same
437  const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
438  const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
439  const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
440  const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
441  const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
442 
443  ASSERT_TRUE(g_one != nullptr);
444  ASSERT_TRUE(g_two != nullptr);
445  ASSERT_TRUE(g_three != nullptr);
446  ASSERT_TRUE(g_four == nullptr);
447 
448  EXPECT_THAT(g_one->getJointModelNames(), ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_c" }));
449  EXPECT_THAT(g_two->getJointModelNames(), ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_b" }));
450  EXPECT_THAT(g_three->getJointModelNames(),
451  ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_b", "joint_c" }));
452  EXPECT_THAT(g_mim->getJointModelNames(), ::testing::ElementsAreArray({ "mim_f", "joint_f" }));
453 
454  EXPECT_THAT(g_one->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_c" }));
455  EXPECT_THAT(g_two->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_b" }));
456  EXPECT_THAT(g_three->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_b", "link_c" }));
457 
458  // but they should have the same links to be updated
459  auto updated_link_model_names = { "base_link", "link_a", "link_b", "link_c", "link_d", "link_e", "link/with/slash" };
460  EXPECT_THAT(g_one->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
461  EXPECT_THAT(g_two->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
462  EXPECT_THAT(g_three->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names));
463 
464  moveit::core::RobotState state(model);
465 
466  EXPECT_EQ(state.getVariableCount(), 7u);
467 
468  state.setToDefaultValues();
469 
470  std::map<std::string, double> joint_values;
471  joint_values["base_joint/x"] = 1.0;
472  joint_values["base_joint/y"] = 1.0;
473  joint_values["base_joint/theta"] = 0.5;
474  joint_values["joint_a"] = -0.5;
475  joint_values["joint_c"] = 0.08;
476  state.setVariablePositions(joint_values);
477 
478  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0));
479  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5);
480  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5);
481  EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5);
482  EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5);
483 
484  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0));
485  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5);
486  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5);
487  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5);
488  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5);
489 
490  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
491  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5);
492  EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5);
493  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5);
494  EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5);
495 
496  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
497  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5);
498  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5);
499  EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5);
500  EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5);
501 
502  EXPECT_TRUE(state.satisfiesBounds());
503 
504  std::map<std::string, double> upd_a;
505  upd_a["joint_a"] = 0.2;
506  state.setVariablePositions(upd_a);
507  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
508  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
509  state.enforceBounds();
510  EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
511 
512  upd_a["joint_a"] = 3.2;
513  state.setVariablePositions(upd_a);
514  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
515  EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
516  state.enforceBounds();
517  EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
518  EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
519 
520  // mimic joints
521  state.setToDefaultValues();
523  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
524  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.2, 0.5, 0));
525  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(0.3, 0.6, 0));
526 
527  // setVariablePosition should update corresponding mimic joints too
528  state.setVariablePosition("joint_f", 1.0);
529  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
530  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
532  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
533  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
534  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
535 
536  ASSERT_EQ(g_mim->getVariableCount(), 2u);
537  double gstate[2];
538  state.copyJointGroupPositions(g_mim, gstate);
539 
540  // setToRandomPositions() uses a different mechanism to update mimic joints
541  state.setToRandomPositions(g_mim);
542  double joint_f = state.getVariablePosition("joint_f");
543  double mim_f = state.getVariablePosition("mim_f");
544  EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
546  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
547  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.1 + mim_f, 0.5, 0));
548  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(),
549  Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
550 
551  // setJointGroupPositions() uses a different mechanism to update mimic joints
552  state.setJointGroupPositions(g_mim, gstate);
553  EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
554  EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
556  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
557  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
558  EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
559 }
560 
561 TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits)
562 {
563  moveit::core::RobotState state(robot_model_);
564  const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
565  ASSERT_TRUE(joint_model_group);
566 
567  state.setToDefaultValues();
568 
569  std::cout << "\nVisual inspection should show NO joints out of bounds:" << std::endl;
570  state.printStatePositionsWithJointLimits(joint_model_group);
571 
572  std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl;
573  std::vector<double> single_joint(1);
574  single_joint[0] = -1.0;
575  state.setJointPositions("joint_c", single_joint);
576  state.printStatePositionsWithJointLimits(joint_model_group);
577 
578  std::cout << "\nVisual inspection should show TWO joint out of bounds:" << std::endl;
579  single_joint[0] = 1.0;
580  state.setJointPositions("joint_f", single_joint);
581  state.printStatePositionsWithJointLimits(joint_model_group);
582 
583  std::cout << "\nVisual inspection should show ONE joint out of bounds:" << std::endl;
584  single_joint[0] = 0.19;
585  state.setJointPositions("joint_f", single_joint);
586  state.printStatePositionsWithJointLimits(joint_model_group);
587 }
588 
589 TEST_F(OneRobot, testInterpolation)
590 {
591  moveit::core::RobotState state_a(robot_model_);
592 
593  // Interpolate with itself
594  state_a.setToDefaultValues();
595  moveit::core::RobotState state_b(state_a);
596  moveit::core::RobotState interpolated_state(state_a);
597  for (size_t i = 0; i <= 10; ++i)
598  {
599  state_a.interpolate(state_b, static_cast<double>(i) / 10., interpolated_state,
600  robot_model_->getJointModelGroup("base_from_base_to_e"));
601  EXPECT_NEAR(state_a.distance(state_b), 0, EPSILON)
602  << "Interpolation between identical states yielded a different state.";
603 
604  for (const auto& link_name : robot_model_->getLinkModelNames())
605  {
606  EXPECT_FALSE(interpolated_state.getCollisionBodyTransform(link_name, 0).matrix().hasNaN())
607  << "Interpolation between identical states yielded NaN value.";
608  }
609  }
610 
611  // Some simple interpolation
612  std::map<std::string, double> joint_values;
613  joint_values["base_joint/x"] = 1.0;
614  joint_values["base_joint/y"] = 1.0;
615  state_a.setVariablePositions(joint_values);
616  joint_values["base_joint/x"] = 0.0;
617  joint_values["base_joint/y"] = 2.0;
618  state_b.setVariablePositions(joint_values);
619  EXPECT_NEAR(3 * std::sqrt(2), state_a.distance(state_b), EPSILON) << "Simple interpolation of base_joint failed.";
620 
621  state_a.interpolate(state_b, 0.5, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
622  EXPECT_NEAR(0., state_a.distance(interpolated_state) - state_b.distance(interpolated_state), EPSILON)
623  << "Simple interpolation of base_joint failed.";
624  EXPECT_NEAR(0.5, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
625  << "Simple interpolation of base_joint failed.";
626  EXPECT_NEAR(1.5, interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
627  << "Simple interpolation of base_joint failed.";
628  state_a.interpolate(state_b, 0.1, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
629  EXPECT_NEAR(0.9, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
630  << "Simple interpolation of base_joint failed.";
631  EXPECT_NEAR(1.1, interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
632  << "Simple interpolation of base_joint failed.";
633 
634  // Interpolate all the joints
635  joint_values["base_joint/x"] = 0.0;
636  joint_values["base_joint/y"] = 20.0;
637  joint_values["base_joint/theta"] = 3 * M_PI / 4;
638  joint_values["joint_a"] = -4 * M_PI / 5;
639  joint_values["joint_c"] = 0.0;
640  joint_values["joint_f"] = 1.0;
641  state_a.setVariablePositions(joint_values);
642 
643  joint_values["base_joint/x"] = 10.0;
644  joint_values["base_joint/y"] = 0.0;
645  joint_values["base_joint/theta"] = -3 * M_PI / 4;
646  joint_values["joint_a"] = 4 * M_PI / 5;
647  joint_values["joint_c"] = 0.07;
648  joint_values["joint_f"] = 0.0;
649  state_b.setVariablePositions(joint_values);
650 
651  for (size_t i = 0; i <= 5; ++i)
652  {
653  double t = static_cast<double>(i) / 5.;
654  state_a.interpolate(state_b, t, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
655  EXPECT_NEAR(10.0 * t, interpolated_state.getVariablePosition("base_joint/x"), EPSILON)
656  << "Base joint interpolation failed.";
657  EXPECT_NEAR(20.0 * (1 - t), interpolated_state.getVariablePosition("base_joint/y"), EPSILON)
658  << "Base joint interpolation failed.";
659  if (t < 0.5)
660  {
661  EXPECT_NEAR(3 * M_PI / 4 + (M_PI / 2) * t, interpolated_state.getVariablePosition("base_joint/theta"), EPSILON)
662  << "Base joint theta interpolation failed.";
663  EXPECT_NEAR(-4 * M_PI / 5 - (2 * M_PI / 5) * t, interpolated_state.getVariablePosition("joint_a"), EPSILON)
664  << "Continuous joint interpolation failed.";
665  }
666  else
667  {
668  EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.getVariablePosition("base_joint/theta"),
669  EPSILON)
670  << "Base joint theta interpolation failed.";
671  EXPECT_NEAR(4 * M_PI / 5 + (2 * M_PI / 5) * (1 - t), interpolated_state.getVariablePosition("joint_a"), EPSILON)
672  << "Continuous joint interpolation failed.";
673  }
674  EXPECT_NEAR(0.07 * t, interpolated_state.getVariablePosition("joint_c"), EPSILON)
675  << "Interpolation of joint_c failed.";
676  EXPECT_NEAR(1 - t, interpolated_state.getVariablePosition("joint_f"), EPSILON)
677  << "Interpolation of joint_f failed.";
678  EXPECT_NEAR(1.5 * (1 - t) + 0.1, interpolated_state.getVariablePosition("mim_f"), EPSILON)
679  << "Interpolation of mimic joint mim_f failed.";
680  }
681 
682  bool nan_exception = false;
683  try
684  {
685  const double infty = std::numeric_limits<double>::infinity();
686  state_a.interpolate(state_b, infty, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
687  }
688  catch (std::exception& e)
689  {
690  std::cout << "Caught expected exception: " << e.what() << std::endl;
691  nan_exception = true;
692  }
693  EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception.";
694 }
695 
696 TEST_F(OneRobot, rigidlyConnectedParent)
697 {
698  // link_e is its own rigidly-connected parent
699  const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") };
700  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
701 
702  // link_b is rigidly connected to its parent link_a
703  const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") };
704  const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") };
705  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
706 
707  moveit::core::RobotState state(robot_model_);
708  state.setToDefaultValues();
709 
710  Eigen::Isometry3d a_to_b;
711  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b", &a_to_b), link_a);
712  // translation from link_a to link_b is (0 0.5 0)
713  EXPECT_NEAR_TRACED(a_to_b.translation(), Eigen::Translation3d(0, 0.5, 0).translation());
714 
715  // attach "object" with "subframe" to link_b
716  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
717  link_b, "object", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)), std::vector<shapes::ShapeConstPtr>{},
718  EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::JointTrajectory{},
719  moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } }));
720 
721  // RobotState's version should resolve these too
722  Eigen::Isometry3d transform;
723  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object"));
724  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe", &transform));
725  // transform from link_b to object/subframe is (1 0 1)
726  EXPECT_NEAR_TRACED((a_to_b.inverse() * transform).matrix(), Eigen::Isometry3d(Eigen::Translation3d(1, 0, 1)).matrix());
727 
728  // test failure cases
729  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));
730  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe"));
731  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel(""));
732  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/"));
733  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/"));
734 
735  // link names with '/' should still work as before
736  const moveit::core::LinkModel* link_with_slash{ robot_model_->getLinkModel("link/with/slash") };
737  EXPECT_TRUE(link_with_slash);
738  const moveit::core::LinkModel* rigid_parent_of_link_with_slash =
739  state.getRigidlyConnectedParentLinkModel("link/with/slash");
740  ASSERT_TRUE(rigid_parent_of_link_with_slash);
741  EXPECT_EQ("base_link", rigid_parent_of_link_with_slash->getName());
742 
743  // the last /-separated component of an object might be a subframe
744  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
745  link_with_slash, "object/with/slash", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)),
746  std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{}, std::set<std::string>{},
747  trajectory_msgs::JointTrajectory{},
748  moveit::core::FixedTransformsMap{ { "sub/frame", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } }));
749  const moveit::core::LinkModel* rigid_parent_of_object =
750  state.getRigidlyConnectedParentLinkModel("object/with/slash/sub/frame");
751  ASSERT_TRUE(rigid_parent_of_object);
752  EXPECT_EQ(rigid_parent_of_link_with_slash, rigid_parent_of_object);
753 }
754 
755 int main(int argc, char** argv)
756 {
757  testing::InitGoogleTest(&argc, argv);
758  return RUN_ALL_TESTS();
759 }
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:581
OneRobot
Definition: robot_state_test.cpp:232
moveit::core::RobotState::distance
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. Only considers active joints.
Definition: robot_state.h:1576
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition: robot_state.cpp:443
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
EPSILON
constexpr double EPSILON
Definition: test_kinematic_complex.cpp:49
TEST_F
TEST_F(OneRobot, FK)
Definition: robot_state_test.cpp:432
moveit::core::RobotModelBuilder::addVisualBox
RobotModelBuilder & addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::Pose origin)
Adds a visual box to a specific link.
Definition: robot_model_test_utils.cpp:322
expect_near
static void expect_near(const Eigen::MatrixXd &x, const Eigen::MatrixXd &y, double eps=std::numeric_limits< double >::epsilon())
Definition: robot_state_test.cpp:80
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
main
int main(int argc, char **argv)
Definition: robot_state_test.cpp:755
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
srdf::ModelSharedPtr
std::shared_ptr< Model > ModelSharedPtr
EXPECT_NEAR_TRACED
#define EXPECT_NEAR_TRACED(...)
Definition: robot_state_test.cpp:91
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:757
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1466
moveit::core::RobotModelBuilder::addInertial
RobotModelBuilder & addInertial(const std::string &link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
Definition: robot_model_test_utils.cpp:292
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:353
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
robot_model.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
OneRobot::TearDown
void TearDown() override
Definition: robot_state_test.cpp:424
moveit::core::RobotState::getRigidlyConnectedParentLinkModel
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame, Eigen::Isometry3d *transform=nullptr, const moveit::core::JointModelGroup *jmg=nullptr) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
Definition: robot_state.cpp:875
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition: robot_state.h:1553
moveit::core::RobotState::setVariablePosition
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known.
Definition: robot_state.h:254
moveit::core::RobotModelBuilder::addCollisionBox
RobotModelBuilder & addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::Pose origin)
Adds a collision box to a specific link.
Definition: robot_model_test_utils.cpp:344
y
double y
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
moveit::core::JointModelGroup::getJointModelNames
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: joint_model_group.h:216
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:170
moveit::core::RobotState::attachBody
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
Definition: robot_state.cpp:1087
moveit::core::RobotState::getVariablePosition
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:273
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:434
moveit::core::JointModelGroup::getUpdatedLinkModelNames
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
Definition: joint_model_group.h:311
moveit::core::RobotModelBuilder::isValid
bool isValid()
Returns true if the building process so far has been valid.
Definition: robot_model_test_utils.cpp:479
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition: robot_model_test_utils.cpp:484
moveit::core::RobotModelBuilder::addVirtualJoint
RobotModelBuilder & 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.
Definition: robot_model_test_utils.cpp:421
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
SimpleRobot
Definition: test_cartesian_interpolator.cpp:45
moveit::core::RobotState::printStatePositionsWithJointLimits
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
Definition: robot_state.cpp:2171
OneRobot::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: robot_state_test.cpp:429
r
S r
TEST
TEST(Loading, SimpleRobot)
Definition: robot_state_test.cpp:97
OneRobot::SetUp
void SetUp() override
Definition: robot_state_test.cpp:235
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
Definition: joint_model_group.h:284
robot_model_test_utils.h
urdf
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition: robot_state.cpp:921
moveit::core::RobotState::getCollisionBodyTransform
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1498
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:158
M_PI
#define M_PI
moveit::core::RobotState::interpolate
void interpolate(const RobotState &to, double t, RobotState &state) const
Definition: robot_state.cpp:1044
srdf
tf2::Quaternion
tf2_geometry_msgs.h
tf2::toMsg
B toMsg(const A &a)
x
double x
moveit::core::RobotModelBuilder::addGroup
RobotModelBuilder & 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.
Definition: robot_model_test_utils.cpp:449
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:671
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
moveit::core::RobotState::setVariableAcceleration
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:447
moveit::core::RobotState::enforceBounds
void enforceBounds()
Definition: robot_state.cpp:939
robot_state.h
t
geometry_msgs::TransformStamped t
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jul 21 2024 02:24:29