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 <urdf_parser/urdf_parser.h>
41 #include <fstream>
42 #include <boost/filesystem.hpp>
43 #include <gtest/gtest.h>
44 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
45 #include <tf2/LinearMath/Vector3.h>
46 #include <moveit/utils/robot_model_test_utils.h>
47 
48 // To visualize bbox of the PR2, set this to 1.
49 #ifndef VISUALIZE_PR2_RVIZ
50 #define VISUALIZE_PR2_RVIZ 0
51 #endif
52 
53 #if VISUALIZE_PR2_RVIZ
54 #include <ros/ros.h>
55 #include <visualization_msgs/Marker.h>
56 #include <geometric_shapes/shape_operations.h>
57 #endif
58 
59 class TestAABB : public testing::Test
60 {
61 protected:
62  void SetUp() override{};
63 
64  moveit::core::RobotState loadModel(const std::string& robot_name)
65  {
66  moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name);
67  return loadModel(model);
68  }
69 
70  moveit::core::RobotState loadModel(const moveit::core::RobotModelPtr& model)
71  {
72  moveit::core::RobotState robot_state = moveit::core::RobotState(model);
73  robot_state.setToDefaultValues();
74  robot_state.update(true);
75 
76  return robot_state;
77  }
78 
79  void TearDown() override
80  {
81  }
82 };
83 
84 TEST_F(TestAABB, TestPR2)
85 {
86  // Contains a link with mesh geometry that is not centered
87 
88  moveit::core::RobotState pr2_state = this->loadModel("pr2");
89 
90  const Eigen::Vector3d& extents_base_footprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin();
91  // values taken from moveit_resources/pr2_description/urdf/robot.xml
92  EXPECT_NEAR(extents_base_footprint[0], 0.001, 1e-4);
93  EXPECT_NEAR(extents_base_footprint[1], 0.001, 1e-4);
94  EXPECT_NEAR(extents_base_footprint[2], 0.001, 1e-4);
95 
96  const Eigen::Vector3d& offset_base_footprint =
97  pr2_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset();
98  EXPECT_NEAR(offset_base_footprint[0], 0.0, 1e-4);
99  EXPECT_NEAR(offset_base_footprint[1], 0.0, 1e-4);
100  EXPECT_NEAR(offset_base_footprint[2], 0.071, 1e-4);
101 
102  const Eigen::Vector3d& extents_base_link = pr2_state.getLinkModel("base_link")->getShapeExtentsAtOrigin();
103  // values computed from moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl in e.g. Meshlab
104  EXPECT_NEAR(extents_base_link[0], 0.668242, 1e-4);
105  EXPECT_NEAR(extents_base_link[1], 0.668242, 1e-4);
106  EXPECT_NEAR(extents_base_link[2], 0.656175, 1e-4);
107 
108  const Eigen::Vector3d& offset_base_link = pr2_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset();
109  EXPECT_NEAR(offset_base_link[0], 0.0, 1e-4);
110  EXPECT_NEAR(offset_base_link[1], 0.0, 1e-4);
111  EXPECT_NEAR(offset_base_link[2], 0.656175 / 2, 1e-4); // The 3D mesh isn't centered, but is whole above z axis
112 
113  std::vector<double> pr2_aabb;
114  pr2_state.computeAABB(pr2_aabb);
115  ASSERT_EQ(pr2_aabb.size(), 6u);
116 
117  EXPECT_NEAR(pr2_aabb[0], -0.3376, 1e-4);
118  EXPECT_NEAR(pr2_aabb[1], 0.6499, 1e-4);
119  EXPECT_NEAR(pr2_aabb[2], -0.6682 / 2, 1e-4);
120  EXPECT_NEAR(pr2_aabb[3], 0.6682 / 2, 1e-4);
121  EXPECT_NEAR(pr2_aabb[4], 0.0044, 1e-4);
122  EXPECT_NEAR(pr2_aabb[5], 1.6328, 1e-4);
123 
124  // Test a specific link known to have some global rotation in the default pose
125 
126  const moveit::core::LinkModel* link = pr2_state.getLinkModel("l_forearm_link");
127  Eigen::Isometry3d transform = pr2_state.getGlobalLinkTransform(link); // intentional copy, we will translate
128  const Eigen::Vector3d& extents = link->getShapeExtentsAtOrigin();
129  transform.translate(link->getCenteredBoundingBoxOffset());
130  moveit::core::AABB aabb;
131  aabb.extendWithTransformedBox(transform, extents);
132 
133  EXPECT_NEAR(aabb.center()[0], 0.5394, 1e-4);
134  EXPECT_NEAR(aabb.center()[1], 0.1880, 1e-4);
135  EXPECT_NEAR(aabb.center()[2], 1.1665, 1e-4);
136  EXPECT_NEAR(aabb.sizes()[0], 0.2209, 1e-4);
137  EXPECT_NEAR(aabb.sizes()[1], 0.1201, 1e-4);
138  EXPECT_NEAR(aabb.sizes()[2], 0.2901, 1e-4);
139 
140 #if VISUALIZE_PR2_RVIZ
141  std::cout << "Overall bounding box of PR2:" << std::endl;
142  std::string dims[] = { "x", "y", "z" };
143  for (std::size_t i = 0; i < 3; ++i)
144  {
145  double dim = pr2_aabb[2 * i + 1] - pr2_aabb[2 * i];
146  double center = dim / 2;
147  std::cout << dims[i] << ": size=" << dim << ", offset=" << (pr2_aabb[2 * i + 1] - center) << std::endl;
148  }
149 
150  // Initialize a ROS publisher
151  char* argv[0];
152  int argc = 0;
153  ros::init(argc, argv, "visualize_pr2");
154  ros::NodeHandle nh;
155  ros::Publisher pub_aabb = nh.advertise<visualization_msgs::Marker>("/visualization_aabb", 10);
156  ros::Publisher pub_obb = nh.advertise<visualization_msgs::Marker>("/visualization_obb", 10);
157 
158  // Wait for the publishers to establish connections
159  sleep(5);
160 
161  // Prepare the ROS message we will reuse throughout the rest of the function.
162  visualization_msgs::Marker msg;
163  msg.header.frame_id = pr2_state.getRobotModel()->getRootLinkName();
164  msg.type = visualization_msgs::Marker::CUBE;
165  msg.color.a = 0.5;
166  msg.lifetime.sec = 3000;
167 
168  // Publish the AABB of the whole model
169  msg.ns = "pr2";
170  msg.pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2;
171  msg.pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2;
172  msg.pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2;
173  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
174  msg.pose.orientation.w = 1;
175  msg.scale.x = pr2_aabb[1] - pr2_aabb[0];
176  msg.scale.y = pr2_aabb[3] - pr2_aabb[2];
177  msg.scale.z = pr2_aabb[5] - pr2_aabb[4];
178  pub_aabb.publish(msg);
179 
180  // Publish BBs for all links
181  std::vector<const moveit::core::LinkModel*> links = pr2_state.getRobotModel()->getLinkModelsWithCollisionGeometry();
182  for (std::size_t i = 0; i < links.size(); ++i)
183  {
184  Eigen::Isometry3d transform = pr2_state.getGlobalLinkTransform(links[i]); // intentional copy, we will translate
185  const Eigen::Vector3d& extents = links[i]->getShapeExtentsAtOrigin();
186  transform.translate(links[i]->getCenteredBoundingBoxOffset());
187  moveit::core::AABB aabb;
188  aabb.extendWithTransformedBox(transform, extents);
189 
190  // Publish AABB
191  msg.ns = links[i]->getName();
192  msg.pose.position.x = transform.translation()[0];
193  msg.pose.position.y = transform.translation()[1];
194  msg.pose.position.z = transform.translation()[2];
195  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
196  msg.pose.orientation.w = 1;
197  msg.color.r = 1;
198  msg.color.b = 0;
199  msg.scale.x = aabb.sizes()[0];
200  msg.scale.y = aabb.sizes()[1];
201  msg.scale.z = aabb.sizes()[2];
202  pub_aabb.publish(msg);
203 
204  // Publish OBB (oriented BB)
205  msg.ns += "-obb";
206  msg.pose.position.x = transform.translation()[0];
207  msg.pose.position.y = transform.translation()[1];
208  msg.pose.position.z = transform.translation()[2];
209  msg.scale.x = extents[0];
210  msg.scale.y = extents[1];
211  msg.scale.z = extents[2];
212  msg.color.r = 0;
213  msg.color.b = 1;
214  Eigen::Quaterniond q(transform.linear());
215  msg.pose.orientation.x = q.x();
216  msg.pose.orientation.y = q.y();
217  msg.pose.orientation.z = q.z();
218  msg.pose.orientation.w = q.w();
219  pub_obb.publish(msg);
220  }
221 
222  // Publish BBs for all attached bodies
223  std::vector<const moveit::core::AttachedBody*> attached_bodies;
224  pr2_state.getAttachedBodies(attached_bodies);
225  for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_bodies.begin();
226  it != attached_bodies.end(); ++it)
227  {
228  const EigenSTL::vector_Isometry3d& transforms = (*it)->getGlobalCollisionBodyTransforms();
229  const std::vector<shapes::ShapeConstPtr>& shapes = (*it)->getShapes();
230  for (std::size_t i = 0; i < transforms.size(); ++i)
231  {
232  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
233  moveit::core::AABB aabb;
234  aabb.extendWithTransformedBox(transforms[i], extents);
235 
236  // Publish AABB
237  msg.ns = (*it)->getName() + boost::lexical_cast<std::string>(i);
238  msg.pose.position.x = transforms[i].translation()[0];
239  msg.pose.position.y = transforms[i].translation()[1];
240  msg.pose.position.z = transforms[i].translation()[2];
241  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
242  msg.pose.orientation.w = 1;
243  msg.color.r = 1;
244  msg.color.b = 0;
245  msg.scale.x = aabb.sizes()[0];
246  msg.scale.y = aabb.sizes()[1];
247  msg.scale.z = aabb.sizes()[2];
248  pub_aabb.publish(msg);
249 
250  // Publish OBB (oriented BB)
251  msg.ns += "-obb";
252  msg.pose.position.x = transforms[i].translation()[0];
253  msg.pose.position.y = transforms[i].translation()[1];
254  msg.pose.position.z = transforms[i].translation()[2];
255  msg.scale.x = extents[0];
256  msg.scale.y = extents[1];
257  msg.scale.z = extents[2];
258  msg.color.r = 0;
259  msg.color.b = 1;
260  Eigen::Quaterniond q(transforms[i].linear());
261  msg.pose.orientation.x = q.x();
262  msg.pose.orientation.y = q.y();
263  msg.pose.orientation.z = q.z();
264  msg.pose.orientation.w = q.w();
265  pub_obb.publish(msg);
266  }
267  }
268 #endif
269 }
270 
271 TEST_F(TestAABB, TestSimple)
272 {
273  // Contains a link with simple geometry and an offset in the collision link
274  moveit::core::RobotModelBuilder builder("simple", "base_footprint");
275  geometry_msgs::Pose origin;
276  tf2::toMsg(tf2::Vector3(0, 0, 0.051), origin.position);
277  origin.orientation.w = 1.0;
278  builder.addChain("base_footprint->base_link", "fixed", { origin });
279 
280  tf2::toMsg(tf2::Vector3(0, 0, 0), origin.position);
281  builder.addCollisionMesh("base_link", "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl",
282  origin);
283 
284  tf2::toMsg(tf2::Vector3(0, 0, 0.071), origin.position);
285  builder.addCollisionBox("base_footprint", { 0.001, 0.001, 0.001 }, origin);
286 
287  builder.addVirtualJoint("odom_combined", "base_footprint", "planar", "world_joint");
288  builder.addGroup({}, { "world_joint" }, "base");
289 
290  ASSERT_TRUE(builder.isValid());
291  moveit::core::RobotState simple_state = loadModel(builder.build());
292  std::vector<double> simple_aabb;
293  simple_state.computeAABB(simple_aabb);
294 
295  ASSERT_EQ(simple_aabb.size(), 6u);
296  EXPECT_NEAR(simple_aabb[0], -0.6682 / 2, 1e-4);
297  EXPECT_NEAR(simple_aabb[1], 0.6682 / 2, 1e-4);
298  EXPECT_NEAR(simple_aabb[2], -0.6682 / 2, 1e-4);
299  EXPECT_NEAR(simple_aabb[3], 0.6682 / 2, 1e-4);
300  EXPECT_NEAR(simple_aabb[4], 0.0510, 1e-4);
301  EXPECT_NEAR(simple_aabb[5], 0.7071, 1e-4);
302 }
303 
304 TEST_F(TestAABB, TestComplex)
305 {
306  // Contains a link with simple geometry and an offset and rotation in the collision link
307  moveit::core::RobotModelBuilder builder("complex", "base_footprint");
308  geometry_msgs::Pose origin;
309  tf2::toMsg(tf2::Vector3(0, 0, 1.0), origin.position);
310  tf2::Quaternion q;
311  q.setRPY(0, 0, 1.5708);
312  origin.orientation = tf2::toMsg(q);
313  builder.addChain("base_footprint->base_link", "fixed", { origin });
314  tf2::toMsg(tf2::Vector3(5.0, 0, 1.0), origin.position);
315  builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin);
316  tf2::toMsg(tf2::Vector3(4.0, 0, 1.0), origin.position);
317  builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin);
318  tf2::toMsg(tf2::Vector3(-5.0, 0.0, -1.0), origin.position);
319  q.setRPY(0, 1.5708, 0);
320  origin.orientation = tf2::toMsg(q);
321  builder.addCollisionBox("base_footprint", { 0.1, 1.0, 0.1 }, origin);
322  builder.addVirtualJoint("odom_combined", "base_footprint", "planar", "world_joint");
323  builder.addGroup({}, { "world_joint" }, "base");
324 
325  ASSERT_TRUE(builder.isValid());
326  moveit::core::RobotState complex_state = this->loadModel(builder.build());
327 
328  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[0], 0.1, 1e-4);
329  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
330  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
331  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[0], -5.0, 1e-4);
332  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
333  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[2], -1.0, 1e-4);
334 
335  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[0], 1.1, 1e-4);
336  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
337  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
338  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[0], 4.5, 1e-4);
339  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
340  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[2], 1.0, 1e-4);
341 
342  std::vector<double> complex_aabb;
343  complex_state.computeAABB(complex_aabb);
344 
345  ASSERT_EQ(complex_aabb.size(), 6u);
346  EXPECT_NEAR(complex_aabb[0], -5.05, 1e-4);
347  EXPECT_NEAR(complex_aabb[1], 0.5, 1e-4);
348  EXPECT_NEAR(complex_aabb[2], -0.5, 1e-4);
349  EXPECT_NEAR(complex_aabb[3], 5.05, 1e-4);
350  EXPECT_NEAR(complex_aabb[4], -1.05, 1e-4);
351  EXPECT_NEAR(complex_aabb[5], 2.05, 1e-4);
352 }
353 
354 int main(int argc, char** argv)
355 {
356  testing::InitGoogleTest(&argc, argv);
357  return RUN_ALL_TESTS();
358 }
TestAABB::loadModel
moveit::core::RobotState loadModel(const moveit::core::RobotModelPtr &model)
Definition: test_aabb.cpp:70
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
Core components of MoveIt.
Definition: kinematics_base.h:83
TestAABB
Definition: test_aabb.cpp:59
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
ros::Publisher
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
moveit::core::RobotModelBuilder::addCollisionMesh
RobotModelBuilder & addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::Pose origin)
Adds a collision mesh to a specific link.
Definition: robot_model_test_utils.cpp:361
ros.h
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:188
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
moveit::core::LinkModel::getShapeExtentsAtOrigin
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes tha...
Definition: link_model.h:185
shape_operations.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
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::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:191
Vector3.h
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
get
def get(url)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
robot_model.h
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
TestAABB::SetUp
void SetUp() override
Definition: test_aabb.cpp:62
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:170
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
main
int main(int argc, char **argv)
Definition: test_aabb.cpp:354
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:2129
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:1113
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
aabb.h
moveit::core::AABB
Represents an axis-aligned bounding box.
Definition: aabb.h:110
robot_model_test_utils.h
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:158
TestAABB::TearDown
void TearDown() override
Definition: test_aabb.cpp:79
moveit::core::RobotModelBuilder::addChain
RobotModelBuilder & addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
Definition: robot_model_test_utils.cpp:204
tf2::Quaternion
tf2_geometry_msgs.h
tf2::toMsg
B toMsg(const A &a)
shapes::computeShapeExtents
Eigen::Vector3d computeShapeExtents(const Shape *shape)
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
aabb
SaPCollisionManager< S >::SaPAABB * aabb
extents
std::array< S, 6 > & extents()
TestAABB::loadModel
moveit::core::RobotState loadModel(const std::string &robot_name)
Definition: test_aabb.cpp:64
robot_state.h
TEST_F
TEST_F(TestAABB, TestPR2)
Definition: test_aabb.cpp:84
ros::NodeHandle
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:117

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Apr 18 2024 02:23:40