Debian Codegraph @

.deprecated/articulated_model/model.h

raw
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Dalibor Matura, Jia Pan */
37
38 #ifndef FCL_ARTICULATED_MODEL_MODEL_H
39 #define FCL_ARTICULATED_MODEL_MODEL_H
40
41 #include "fcl/articulated_model/joint.h"
42 #include "fcl/articulated_model/link.h"
43
44 #include "fcl/common/data_types.h"
45 #include <memory>
46
47 #include <map>
48 #include <stdexcept>
49
50 namespace fcl
51 {
52
53 class ModelParseError : public std::runtime_error
54 {
55 public:
56 ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {}
57 };
58
59 template <typename S>
60 class Model
61 {
62 public:
63 Model() {}
64
65 virtual ~Model() {}
66
67 const std::string& getName() const;
68
69 void addLink(const std::shared_ptr<Link>& link);
70
71 void addJoint(const std::shared_ptr<Joint>& joint);
72
73 void initRoot(const std::map<std::string, std::string>& link_parent_tree);
74
75 void initTree(std::map<std::string, std::string>& link_parent_tree);
76
77 std::size_t getNumDofs() const;
78
79 std::size_t getNumLinks() const;
80
81 std::size_t getNumJoints() const;
82
83 std::shared_ptr<Link> getRoot() const;
84 std::shared_ptr<Link> getLink(const std::string& name) const;
85 std::shared_ptr<Joint> getJoint(const std::string& name) const;
86
87 std::vector<std::shared_ptr<Link> > getLinks() const;
88 std::vector<std::shared_ptr<Joint> > getJoints() const;
89 protected:
90 std::shared_ptr<Link> root_link_;
91 std::map<std::string, std::shared_ptr<Link> > links_;
92 std::map<std::string, std::shared_ptr<Joint> > joints_;
93
94 std::string name_;
95
96 };
97
98 //==============================================================================
99 template <typename S>
100 std::shared_ptr<Link> Model::getRoot() const
101 {
102 return root_link_;
103 }
104
105 //==============================================================================
106 template <typename S>
107 std::shared_ptr<Link> Model::getLink(const std::string& name) const
108 {
109 std::shared_ptr<Link> ptr;
110 std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.find(name);
111 if(it == links_.end())
112 ptr.reset();
113 else
114 ptr = it->second;
115 return ptr;
116 }
117
118 //==============================================================================
119 template <typename S>
120 std::shared_ptr<Joint> Model::getJoint(const std::string& name) const
121 {
122 std::shared_ptr<Joint> ptr;
123 std::map<std::string, std::shared_ptr<Joint> >::const_iterator it = joints_.find(name);
124 if(it == joints_.end())
125 ptr.reset();
126 else
127 ptr = it->second;
128 return ptr;
129 }
130
131 //==============================================================================
132 template <typename S>
133 const std::string& Model::getName() const
134 {
135 return name_;
136 }
137
138 //==============================================================================
139 template <typename S>
140 std::vector<std::shared_ptr<Link> > Model::getLinks() const
141 {
142 std::vector<std::shared_ptr<Link> > links;
143 for(std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
144 {
145 links.push_back(it->second);
146 }
147
148 return links;
149 }
150
151 //==============================================================================
152 template <typename S>
153 std::size_t Model::getNumLinks() const
154 {
155 return links_.size();
156 }
157
158 //==============================================================================
159 template <typename S>
160 std::size_t Model::getNumJoints() const
161 {
162 return joints_.size();
163 }
164
165 //==============================================================================
166 template <typename S>
167 std::size_t Model::getNumDofs() const
168 {
169 std::size_t dof = 0;
170 for(std::map<std::string, std::shared_ptr<Joint> >::const_iterator it = joints_.begin(); it != joints_.end(); ++it)
171 {
172 dof += it->second->getNumDofs();
173 }
174
175 return dof;
176 }
177
178 //==============================================================================
179 template <typename S>
180 void Model::addLink(const std::shared_ptr<Link>& link)
181 {
182 links_[link->getName()] = link;
183 }
184
185 //==============================================================================
186 template <typename S>
187 void Model::addJoint(const std::shared_ptr<Joint>& joint)
188 {
189 joints_[joint->getName()] = joint;
190 }
191
192 //==============================================================================
193 template <typename S>
194 void Model::initRoot(const std::map<std::string, std::string>& link_parent_tree)
195 {
196 root_link_.reset();
197
198 /// find the links that have no parent in the tree
199 for(std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
200 {
201 std::map<std::string, std::string>::const_iterator parent = link_parent_tree.find(it->first);
202 if(parent == link_parent_tree.end())
203 {
204 if(!root_link_)
205 {
206 root_link_ = getLink(it->first);
207 }
208 else
209 {
210 throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]");
211 }
212 }
213 }
214
215 if(!root_link_)
216 throw ModelParseError("No root link found.");
217 }
218
219 //==============================================================================
220 template <typename S>
221 void Model::initTree(std::map<std::string, std::string>& link_parent_tree)
222 {
223 for(std::map<std::string, std::shared_ptr<Joint> >::iterator it = joints_.begin(); it != joints_.end(); ++it)
224 {
225 std::string parent_link_name = it->second->getParentLink()->getName();
226 std::string child_link_name = it->second->getChildLink()->getName();
227
228 it->second->getParentLink()->addChildJoint(it->second);
229 it->second->getChildLink()->setParentJoint(it->second);
230
231 link_parent_tree[child_link_name] = parent_link_name;
232 }
233 }
234
235 } // namespace fcl
236
237 #endif