ros_bullet_utils.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2018, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Licensed under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  *********************************************************************/
18 
19 /* Author: Levi Armstrong */
20 
21 #pragma once
22 
27 #include <ros/console.h>
28 
30 
32 {
38 static inline void getActiveLinkNamesRecursive(std::vector<std::string>& active_links,
39  const urdf::LinkConstSharedPtr& urdf_link, bool active)
40 {
41  if (active)
42  {
43  active_links.push_back(urdf_link->name);
44  for (const auto& child_link : urdf_link->child_links)
45  {
46  getActiveLinkNamesRecursive(active_links, child_link, active);
47  }
48  }
49  else
50  {
51  for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i)
52  {
53  const urdf::LinkConstSharedPtr child_link = urdf_link->child_links[i];
54  if ((child_link->parent_joint) && (child_link->parent_joint->type != urdf::Joint::FIXED))
55  getActiveLinkNamesRecursive(active_links, child_link, true);
56  else
57  getActiveLinkNamesRecursive(active_links, child_link, active);
58  }
59  }
60 }
61 
62 inline shapes::ShapePtr constructShape(const urdf::Geometry* geom)
63 {
64  shapes::Shape* result = nullptr;
65  switch (geom->type)
66  {
67  case urdf::Geometry::SPHERE:
68  result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
69  break;
70  case urdf::Geometry::BOX:
71  {
72  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
73  result = new shapes::Box(dim.x, dim.y, dim.z);
74  }
75  break;
76  case urdf::Geometry::CYLINDER:
77  result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
78  static_cast<const urdf::Cylinder*>(geom)->length);
79  break;
80  case urdf::Geometry::MESH:
81  {
82  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
83  if (!mesh->filename.empty())
84  {
85  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
86  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
87  result = m;
88  }
89  }
90  break;
91  default:
92  ROS_ERROR("Unknown geometry type: %d", static_cast<int>(geom->type));
93  break;
94  }
95 
96  return shapes::ShapePtr(result);
97 }
98 
99 inline Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose)
100 {
101  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
102  Eigen::Isometry3d result;
103  result.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
104  result.linear() = q.toRotationMatrix();
105  return result;
106 }
107 } // namespace collision_detection_bullet
collision_detection_bullet
Definition: basic_types.h:34
basic_types.h
collision_detection_bullet::constructShape
shapes::ShapePtr constructShape(const urdf::Geometry *geom)
Definition: ros_bullet_utils.h:78
shapes::Cylinder
collision_detection_bullet::getActiveLinkNamesRecursive
static void getActiveLinkNamesRecursive(std::vector< std::string > &active_links, const urdf::LinkConstSharedPtr &urdf_link, bool active)
Recursively traverses robot from root to get all active links.
Definition: ros_bullet_utils.h:54
shape_operations.h
shapes::Shape
shapes::Mesh
console.h
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
shapes::Box
shapes::Sphere
ROS_ERROR
#define ROS_ERROR(...)
shapes.h
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
conversions.h
shape_messages.h
collision_detection_bullet::urdfPose2Eigen
Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose &pose)
Definition: ros_bullet_utils.h:115
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 Nov 21 2021 03:25:56