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: Wim Meeussen */
36 
37 #ifndef URDF_INTERFACE_LINK_H
38 #define URDF_INTERFACE_LINK_H
39 
40 #include <string>
41 #include <vector>
42 #include <map>
43 
44 #include "joint.h"
45 #include "color.h"
46 #include "types.h"
47 
48 namespace urdf{
49 
50 class Geometry
51 {
52 public:
53   enum {SPHERE, BOX, CYLINDER, MESH} type;
54 
~Geometry(void)55   virtual ~Geometry(void)
56   {
57   }
58 };
59 
60 class Sphere : public Geometry
61 {
62 public:
Sphere()63   Sphere() { this->clear(); type = SPHERE; };
64   double radius;
65 
clear()66   void clear()
67   {
68     radius = 0;
69   };
70 };
71 
72 class Box : public Geometry
73 {
74 public:
Box()75   Box() { this->clear(); type = BOX; };
76   Vector3 dim;
77 
clear()78   void clear()
79   {
80     this->dim.clear();
81   };
82 };
83 
84 class Cylinder : public Geometry
85 {
86 public:
Cylinder()87   Cylinder() { this->clear(); type = CYLINDER; };
88   double length;
89   double radius;
90 
clear()91   void clear()
92   {
93     length = 0;
94     radius = 0;
95   };
96 };
97 
98 class Mesh : public Geometry
99 {
100 public:
Mesh()101   Mesh() { this->clear(); type = MESH; };
102   std::string filename;
103   Vector3 scale;
104 
clear()105   void clear()
106   {
107     filename.clear();
108     // default scale
109     scale.x = 1;
110     scale.y = 1;
111     scale.z = 1;
112   };
113 };
114 
115 class Material
116 {
117 public:
Material()118   Material() { this->clear(); };
119   std::string name;
120   std::string texture_filename;
121   Color color;
122 
clear()123   void clear()
124   {
125     color.clear();
126     texture_filename.clear();
127     name.clear();
128   };
129 };
130 
131 class Inertial
132 {
133 public:
Inertial()134   Inertial() { this->clear(); };
135   Pose origin;
136   double mass;
137   double ixx,ixy,ixz,iyy,iyz,izz;
138 
clear()139   void clear()
140   {
141     origin.clear();
142     mass = 0;
143     ixx = ixy = ixz = iyy = iyz = izz = 0;
144   };
145 };
146 
147 class Visual
148 {
149 public:
Visual()150   Visual() { this->clear(); };
151   Pose origin;
152   GeometrySharedPtr geometry;
153 
154   std::string material_name;
155   MaterialSharedPtr material;
156 
clear()157   void clear()
158   {
159     origin.clear();
160     material_name.clear();
161     material.reset();
162     geometry.reset();
163     name.clear();
164   };
165 
166   std::string name;
167 };
168 
169 class Collision
170 {
171 public:
Collision()172   Collision() { this->clear(); };
173   Pose origin;
174   GeometrySharedPtr geometry;
175 
clear()176   void clear()
177   {
178     origin.clear();
179     geometry.reset();
180     name.clear();
181   };
182 
183   std::string name;
184 
185 };
186 
187 
188 class Link
189 {
190 public:
Link()191   Link() { this->clear(); };
192 
193   std::string name;
194 
195   /// inertial element
196   InertialSharedPtr inertial;
197 
198   /// visual element
199   VisualSharedPtr visual;
200 
201   /// collision element
202   CollisionSharedPtr collision;
203 
204   /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
205   std::vector<CollisionSharedPtr> collision_array;
206 
207   /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
208   std::vector<VisualSharedPtr> visual_array;
209 
210   /// Parent Joint element
211   ///   explicitly stating "parent" because we want directional-ness for tree structure
212   ///   every link can have one parent
213   JointSharedPtr parent_joint;
214 
215   std::vector<JointSharedPtr> child_joints;
216   std::vector<LinkSharedPtr> child_links;
217 
getParent()218   LinkSharedPtr getParent() const
219   {return parent_link_.lock();};
220 
setParent(const LinkSharedPtr & parent)221   void setParent(const LinkSharedPtr &parent)
222   { parent_link_ = parent; }
223 
clear()224   void clear()
225   {
226     this->name.clear();
227     this->inertial.reset();
228     this->visual.reset();
229     this->collision.reset();
230     this->parent_joint.reset();
231     this->child_joints.clear();
232     this->child_links.clear();
233     this->collision_array.clear();
234     this->visual_array.clear();
235   };
236 
237 private:
238   LinkWeakPtr parent_link_;
239 
240 };
241 
242 
243 
244 
245 }
246 
247 #endif
248