test_aabb.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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: Martin Pecka */
36 
37 #include <moveit/robot_model/aabb.h>
38 #include <moveit/robot_model/robot_model.h>
39 #include <moveit/robot_state/robot_state.h>
40 #include <moveit_resources/config.h>
41 #include <urdf_parser/urdf_parser.h>
42 #include <fstream>
43 #include <boost/filesystem.hpp>
44 #include <gtest/gtest.h>
45 
46 // To visualize bbox of the PR2, set this to 1.
47 #ifndef VISUALIZE_PR2_RVIZ
48 #define VISUALIZE_PR2_RVIZ 0
49 #endif
50 
51 #if VISUALIZE_PR2_RVIZ
52 #include <ros/ros.h>
53 #include <visualization_msgs/Marker.h>
54 #include <geometric_shapes/shape_operations.h>
55 #endif
56 
57 class TestAABB : public testing::Test
58 {
59 protected:
60  std::string readFileToString(boost::filesystem::path path) const
61  {
62  std::string file_string;
63  std::fstream file(path.string().c_str(), std::fstream::in);
64  if (file.is_open())
65  {
66  std::string line;
67  while (file.good())
68  {
69  std::getline(file, line);
70  file_string += (line + "\n");
71  }
72  file.close();
73  }
74  return file_string;
75  }
76 
77  void SetUp() override{};
78 
79  robot_state::RobotState loadModel(const std::string urdf, const std::string srdf)
80  {
81  urdf::ModelInterfaceSharedPtr parsed_urdf(urdf::parseURDF(urdf));
82  if (!parsed_urdf)
83  throw std::runtime_error("Cannot parse URDF.");
84 
85  srdf::ModelSharedPtr parsed_srdf(new srdf::Model());
86  bool srdf_ok = parsed_srdf->initString(*parsed_urdf, srdf);
87  if (!srdf_ok)
88  throw std::runtime_error("Cannot parse URDF.");
89 
90  robot_model::RobotModelPtr model(new robot_model::RobotModel(parsed_urdf, parsed_srdf));
91  robot_state::RobotState robot_state = robot_state::RobotState(model);
92  robot_state.setToDefaultValues();
93  robot_state.update(true);
94 
95  return robot_state;
96  }
97 
98  void TearDown() override
99  {
100  }
101 };
102 
103 TEST_F(TestAABB, TestPR2)
104 {
105  // Contains a link with mesh geometry that is not centered
106 
107  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
108 
109  const std::string PR2_URDF = this->readFileToString(res_path / "pr2_description/urdf/robot.xml");
110  const std::string PR2_SRDF = this->readFileToString(res_path / "pr2_description/srdf/robot.xml");
111 
112  robot_state::RobotState pr2_state = this->loadModel(PR2_URDF, PR2_SRDF);
113 
114  const Eigen::Vector3d& extentsBaseFootprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin();
115  // values taken from moveit_resources/pr2_description/urdf/robot.xml
116  EXPECT_NEAR(extentsBaseFootprint[0], 0.001, 1e-4);
117  EXPECT_NEAR(extentsBaseFootprint[1], 0.001, 1e-4);
118  EXPECT_NEAR(extentsBaseFootprint[2], 0.001, 1e-4);
119 
120  const Eigen::Vector3d& offsetBaseFootprint = pr2_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset();
121  EXPECT_NEAR(offsetBaseFootprint[0], 0.0, 1e-4);
122  EXPECT_NEAR(offsetBaseFootprint[1], 0.0, 1e-4);
123  EXPECT_NEAR(offsetBaseFootprint[2], 0.071, 1e-4);
124 
125  const Eigen::Vector3d& extentsBaseLink = pr2_state.getLinkModel("base_link")->getShapeExtentsAtOrigin();
126  // values computed from moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl in e.g. Meshlab
127  EXPECT_NEAR(extentsBaseLink[0], 0.668242, 1e-4);
128  EXPECT_NEAR(extentsBaseLink[1], 0.668242, 1e-4);
129  EXPECT_NEAR(extentsBaseLink[2], 0.656175, 1e-4);
130 
131  const Eigen::Vector3d& offsetBaseLink = pr2_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset();
132  EXPECT_NEAR(offsetBaseLink[0], 0.0, 1e-4);
133  EXPECT_NEAR(offsetBaseLink[1], 0.0, 1e-4);
134  EXPECT_NEAR(offsetBaseLink[2], 0.656175 / 2, 1e-4); // The 3D mesh isn't centered, but is whole above z axis
135 
136  std::vector<double> pr2_aabb;
137  pr2_state.computeAABB(pr2_aabb);
138  ASSERT_EQ(pr2_aabb.size(), 6);
139 
140  EXPECT_NEAR(pr2_aabb[0], -0.3376, 1e-4);
141  EXPECT_NEAR(pr2_aabb[1], 0.6499, 1e-4);
142  EXPECT_NEAR(pr2_aabb[2], -0.6682 / 2, 1e-4);
143  EXPECT_NEAR(pr2_aabb[3], 0.6682 / 2, 1e-4);
144  EXPECT_NEAR(pr2_aabb[4], 0.0044, 1e-4);
145  EXPECT_NEAR(pr2_aabb[5], 1.6328, 1e-4);
146 
147  // Test a specific link known to have some global rotation in the default pose
148 
149  const moveit::core::LinkModel* link = pr2_state.getLinkModel("l_forearm_link");
150  Eigen::Affine3d transform = pr2_state.getGlobalLinkTransform(link); // intentional copy, we will translate
151  const Eigen::Vector3d& extents = link->getShapeExtentsAtOrigin();
152  transform.translate(link->getCenteredBoundingBoxOffset());
153  moveit::core::AABB aabb;
154  aabb.extendWithTransformedBox(transform, extents);
155 
156  EXPECT_NEAR(aabb.center()[0], 0.5394, 1e-4);
157  EXPECT_NEAR(aabb.center()[1], 0.1880, 1e-4);
158  EXPECT_NEAR(aabb.center()[2], 1.1665, 1e-4);
159  EXPECT_NEAR(aabb.sizes()[0], 0.2209, 1e-4);
160  EXPECT_NEAR(aabb.sizes()[1], 0.1201, 1e-4);
161  EXPECT_NEAR(aabb.sizes()[2], 0.2901, 1e-4);
162 
163 #if VISUALIZE_PR2_RVIZ
164  std::cout << "Overall bounding box of PR2:" << std::endl;
165  std::string dims[] = { "x", "y", "z" };
166  for (std::size_t i = 0; i < 3; ++i)
167  {
168  double dim = pr2_aabb[2 * i + 1] - pr2_aabb[2 * i];
169  double center = dim / 2;
170  std::cout << dims[i] << ": size=" << dim << ", offset=" << (pr2_aabb[2 * i + 1] - center) << std::endl;
171  }
172 
173  // Initialize a ROS publisher
174  char* argv[0];
175  int argc = 0;
176  ros::init(argc, argv, "visualize_pr2");
177  ros::NodeHandle nh;
178  ros::Publisher pub_aabb = nh.advertise<visualization_msgs::Marker>("/visualization_aabb", 10);
179  ros::Publisher pub_obb = nh.advertise<visualization_msgs::Marker>("/visualization_obb", 10);
180 
181  // Wait for the publishers to establish connections
182  sleep(5);
183 
184  // Prepare the ROS message we will reuse throughout the rest of the function.
185  visualization_msgs::Marker msg;
186  msg.header.frame_id = pr2_state.getRobotModel()->getRootLinkName();
187  msg.type = visualization_msgs::Marker::CUBE;
188  msg.color.a = 0.5;
189  msg.lifetime.sec = 3000;
190 
191  // Publish the AABB of the whole model
192  msg.ns = "pr2";
193  msg.pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2;
194  msg.pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2;
195  msg.pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2;
196  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
197  msg.pose.orientation.w = 1;
198  msg.scale.x = pr2_aabb[1] - pr2_aabb[0];
199  msg.scale.y = pr2_aabb[3] - pr2_aabb[2];
200  msg.scale.z = pr2_aabb[5] - pr2_aabb[4];
201  pub_aabb.publish(msg);
202 
203  // Publish BBs for all links
204  std::vector<const moveit::core::LinkModel*> links = pr2_state.getRobotModel()->getLinkModelsWithCollisionGeometry();
205  for (std::size_t i = 0; i < links.size(); ++i)
206  {
207  Eigen::Affine3d transform = pr2_state.getGlobalLinkTransform(links[i]); // intentional copy, we will translate
208  const Eigen::Vector3d& extents = links[i]->getShapeExtentsAtOrigin();
209  transform.translate(links[i]->getCenteredBoundingBoxOffset());
210  moveit::core::AABB aabb;
211  aabb.extendWithTransformedBox(transform, extents);
212 
213  // Publish AABB
214  msg.ns = links[i]->getName();
215  msg.pose.position.x = transform.translation()[0];
216  msg.pose.position.y = transform.translation()[1];
217  msg.pose.position.z = transform.translation()[2];
218  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
219  msg.pose.orientation.w = 1;
220  msg.color.r = 1;
221  msg.color.b = 0;
222  msg.scale.x = aabb.sizes()[0];
223  msg.scale.y = aabb.sizes()[1];
224  msg.scale.z = aabb.sizes()[2];
225  pub_aabb.publish(msg);
226 
227  // Publish OBB (oriented BB)
228  msg.ns += "-obb";
229  msg.pose.position.x = transform.translation()[0];
230  msg.pose.position.y = transform.translation()[1];
231  msg.pose.position.z = transform.translation()[2];
232  msg.scale.x = extents[0];
233  msg.scale.y = extents[1];
234  msg.scale.z = extents[2];
235  msg.color.r = 0;
236  msg.color.b = 1;
237  Eigen::Quaterniond q(transform.linear());
238  msg.pose.orientation.x = q.x();
239  msg.pose.orientation.y = q.y();
240  msg.pose.orientation.z = q.z();
241  msg.pose.orientation.w = q.w();
242  pub_obb.publish(msg);
243  }
244 
245  // Publish BBs for all attached bodies
246  std::vector<const moveit::core::AttachedBody*> attached_bodies;
247  pr2_state.getAttachedBodies(attached_bodies);
248  for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_bodies.begin();
249  it != attached_bodies.end(); ++it)
250  {
251  const EigenSTL::vector_Affine3d& transforms = (*it)->getGlobalCollisionBodyTransforms();
252  const std::vector<shapes::ShapeConstPtr>& shapes = (*it)->getShapes();
253  for (std::size_t i = 0; i < transforms.size(); ++i)
254  {
255  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
256  moveit::core::AABB aabb;
257  aabb.extendWithTransformedBox(transforms[i], extents);
258 
259  // Publish AABB
260  msg.ns = (*it)->getName() + boost::lexical_cast<std::string>(i);
261  msg.pose.position.x = transforms[i].translation()[0];
262  msg.pose.position.y = transforms[i].translation()[1];
263  msg.pose.position.z = transforms[i].translation()[2];
264  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
265  msg.pose.orientation.w = 1;
266  msg.color.r = 1;
267  msg.color.b = 0;
268  msg.scale.x = aabb.sizes()[0];
269  msg.scale.y = aabb.sizes()[1];
270  msg.scale.z = aabb.sizes()[2];
271  pub_aabb.publish(msg);
272 
273  // Publish OBB (oriented BB)
274  msg.ns += "-obb";
275  msg.pose.position.x = transforms[i].translation()[0];
276  msg.pose.position.y = transforms[i].translation()[1];
277  msg.pose.position.z = transforms[i].translation()[2];
278  msg.scale.x = extents[0];
279  msg.scale.y = extents[1];
280  msg.scale.z = extents[2];
281  msg.color.r = 0;
282  msg.color.b = 1;
283  Eigen::Quaterniond q(transforms[i].linear());
284  msg.pose.orientation.x = q.x();
285  msg.pose.orientation.y = q.y();
286  msg.pose.orientation.z = q.z();
287  msg.pose.orientation.w = q.w();
288  pub_obb.publish(msg);
289  }
290  }
291 #endif
292 }
293 
294 TEST_F(TestAABB, TestSimple)
295 {
296  // Contains a link with simple geometry and an offset in the collision link
297 
298  const std::string SIMPLE_URDF =
299  "<?xml version='1.0' ?>"
300  "<robot name='simple'>"
301  " <link name='base_link'>"
302  " <collision>"
303  " <origin rpy='0 0 0' xyz='0 0 0'/>"
304  " <geometry>"
305  " <mesh filename='package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl'/>"
306  " </geometry>"
307  " </collision>"
308  " </link>"
309  " <link name='base_footprint'>"
310  " <collision>"
311  " <origin rpy='0 0 0' xyz='0 0 0.071'/>"
312  " <geometry>"
313  " <box size='0.001 0.001 0.001'/>"
314  " </geometry>"
315  " </collision>"
316  " </link>"
317  " <joint name='base_footprint_joint' type='fixed'>"
318  " <origin rpy='0 0 0' xyz='0 0 0.051'/>"
319  " <child link='base_link'/>"
320  " <parent link='base_footprint'/>"
321  " </joint>"
322  "</robot>";
323 
324  const std::string SIMPLE_SRDF =
325  "<?xml version='1.0'?>"
326  "<robot name='simple'> "
327  " <virtual_joint name='world_joint' type='planar' parent_frame='odom_combined' child_link='base_footprint'/> "
328  " <group name='base'>"
329  " <joint name='world_joint'/>"
330  " </group> "
331  "</robot>";
332 
333  robot_state::RobotState simple_state = this->loadModel(SIMPLE_URDF, SIMPLE_SRDF);
334 
335  std::vector<double> simple_aabb;
336  simple_state.computeAABB(simple_aabb);
337 
338  ASSERT_EQ(simple_aabb.size(), 6);
339  EXPECT_NEAR(simple_aabb[0], -0.6682 / 2, 1e-4);
340  EXPECT_NEAR(simple_aabb[1], 0.6682 / 2, 1e-4);
341  EXPECT_NEAR(simple_aabb[2], -0.6682 / 2, 1e-4);
342  EXPECT_NEAR(simple_aabb[3], 0.6682 / 2, 1e-4);
343  EXPECT_NEAR(simple_aabb[4], 0.0510, 1e-4);
344  EXPECT_NEAR(simple_aabb[5], 0.7071, 1e-4);
345 }
346 
347 TEST_F(TestAABB, TestComplex)
348 {
349  // Contains a link with simple geometry and an offset and rotation in the collision link
350 
351  const std::string COMPLEX_URDF = "<?xml version='1.0' ?>"
352  "<robot name='complex'>"
353  " <link name='base_link'>"
354  " <collision>"
355  " <origin rpy='0 0 1.5708' xyz='5.0 0 1.0'/>"
356  " <geometry>"
357  " <box size='1.0 0.1 0.1' />"
358  " </geometry>"
359  " </collision>"
360  " <collision>"
361  " <origin rpy='0 0 1.5708' xyz='4.0 0 1.0'/>"
362  " <geometry>"
363  " <box size='1.0 0.1 0.1' />"
364  " </geometry>"
365  " </collision>"
366  " </link>"
367  " <link name='base_footprint'>"
368  " <collision>"
369  " <origin rpy='0 1.5708 0' xyz='-5.0 0 -1.0'/>"
370  " <geometry>"
371  " <box size='0.1 1.0 0.1' />"
372  " </geometry>"
373  " </collision>"
374  " </link>"
375  " <joint name='base_footprint_joint' type='fixed'>"
376  " <origin rpy='0 0 1.5708' xyz='0 0 1'/>"
377  " <child link='base_link'/>"
378  " <parent link='base_footprint'/>"
379  " </joint>"
380  "</robot>";
381 
382  const std::string COMPLEX_SRDF =
383  "<?xml version='1.0'?>"
384  "<robot name='complex'> "
385  " <virtual_joint name='world_joint' type='planar' parent_frame='odom_combined' child_link='base_footprint'/> "
386  " <group name='base'>"
387  " <joint name='world_joint'/>"
388  " </group> "
389  "</robot>";
390 
391  robot_state::RobotState complex_state = this->loadModel(COMPLEX_URDF, COMPLEX_SRDF);
392 
393  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[0], 0.1, 1e-4);
394  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
395  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
396  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[0], -5.0, 1e-4);
397  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
398  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[2], -1.0, 1e-4);
399 
400  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[0], 1.1, 1e-4);
401  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
402  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
403  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[0], 4.5, 1e-4);
404  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
405  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[2], 1.0, 1e-4);
406 
407  std::vector<double> complex_aabb;
408  complex_state.computeAABB(complex_aabb);
409 
410  ASSERT_EQ(complex_aabb.size(), 6);
411  EXPECT_NEAR(complex_aabb[0], -5.05, 1e-4);
412  EXPECT_NEAR(complex_aabb[1], 0.5, 1e-4);
413  EXPECT_NEAR(complex_aabb[2], -0.5, 1e-4);
414  EXPECT_NEAR(complex_aabb[3], 5.05, 1e-4);
415  EXPECT_NEAR(complex_aabb[4], -1.05, 1e-4);
416  EXPECT_NEAR(complex_aabb[5], 2.05, 1e-4);
417 }
418 
419 int main(int argc, char** argv)
420 {
421  testing::InitGoogleTest(&argc, argv);
422  return RUN_ALL_TESTS();
423 }
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:878
TestAABB::readFileToString
std::string readFileToString(boost::filesystem::path path) const
Definition: test_aabb.cpp:60
moveit::core
Core components of MoveIt!
Definition: kinematics_base.h:50
moveit::core::LinkModel::getCenteredBoundingBoxOffset
const Eigen::Vector3d & getCenteredBoundingBoxOffset() const
Get the offset of the center of the bounding box of this link when the link is positioned at origin...
Definition: link_model.h:190
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
EigenSTL::vector_Affine3d
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
robot_model.h
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
moveit::core::LinkModel::getShapeExtentsAtOrigin
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link&#39;s geometry (dimensions of axis-aligned bounding box around all shapes tha...
Definition: link_model.h:184
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
boost::shared_ptr< Model >
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:288
TestAABB::TearDown
void TearDown() override
Definition: test_aabb.cpp:98
moveit::core::RobotState::computeAABB
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
Definition: robot_state.cpp:2117
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:563
main
int main(int argc, char **argv)
Definition: test_aabb.cpp:419
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
moveit::core::AABB::extendWithTransformedBox
void extendWithTransformedBox(const Eigen::Affine3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Definition: aabb.cpp:39
TestAABB::loadModel
robot_state::RobotState loadModel(const std::string urdf, const std::string srdf)
Definition: test_aabb.cpp:79
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:138
robot_state.h
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
shape_operations.h
moveit::core::RobotState
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
shapes::computeShapeExtents
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
TestAABB::SetUp
void SetUp() override
Definition: test_aabb.cpp:77
TEST_F
TEST_F(TestAABB, TestPR2)
Definition: test_aabb.cpp:103
moveit::core::AABB
Represents an axis-aligned bounding box.
Definition: aabb.h:47

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