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_JOINT_CONFIG_H
39 #define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
40
41 #include "fcl/common/data_types.h"
42 #include <memory>
43 #include <vector>
44
45 namespace fcl
46 {
47
48 template <typename S>
49 class Joint;
50
51 template <typename S>
52 class JointConfig
53 {
54 public:
55 JointConfig();
56
57 JointConfig(const JointConfig& joint_cfg);
58
59 JointConfig(const std::shared_ptr<Joint>& joint,
60 S default_value = 0,
61 S default_value_min = 0,
62 S default_value_max = 0);
63
64 std::size_t getDim() const;
65
66 inline S operator [] (std::size_t i) const
67 {
68 return values_[i];
69 }
70
71 inline S& operator [] (std::size_t i)
72 {
73 return values_[i];
74 }
75
76 S getValue(std::size_t i) const;
77
78 S& getValue(std::size_t i);
79
80 S getLimitMin(std::size_t i) const;
81
82 S& getLimitMin(std::size_t i);
83
84 S getLimitMax(std::size_t i) const;
85
86 S& getLimitMax(std::size_t i);
87
88 std::shared_ptr<Joint> getJoint() const;
89
90 private:
91 std::weak_ptr<Joint> joint_;
92
93 std::vector<S> values_;
94 std::vector<S> limits_min_;
95 std::vector<S> limits_max_;
96 };
97
98 //============================================================================//
99 // //
100 // Implementations //
101 // //
102 //============================================================================//
103
104 //==============================================================================
105 template <typename S>
JointConfig()106 JointConfig<S>::JointConfig() {}
107
108 //==============================================================================
109 template <typename S>
JointConfig(const JointConfig & joint_cfg)110 JointConfig<S>::JointConfig(const JointConfig& joint_cfg) :
111 joint_(joint_cfg.joint_),
112 values_(joint_cfg.values_),
113 limits_min_(joint_cfg.limits_min_),
114 limits_max_(joint_cfg.limits_max_)
115 {
116 }
117
118 //==============================================================================
119 template <typename S>
JointConfig(const std::shared_ptr<Joint> & joint,S default_value,S default_value_min,S default_value_max)120 JointConfig<S>::JointConfig(const std::shared_ptr<Joint>& joint,
121 S default_value,
122 S default_value_min,
123 S default_value_max) :
124 joint_(joint)
125 {
126 values_.resize(joint->getNumDofs(), default_value);
127 limits_min_.resize(joint->getNumDofs(), default_value_min);
128 limits_max_.resize(joint->getNumDofs(), default_value_max);
129 }
130
131 //==============================================================================
132 template <typename S>
getDim()133 std::size_t JointConfig<S>::getDim() const
134 {
135 return values_.size();
136 }
137
138 //==============================================================================
139 template <typename S>
getValue(std::size_t i)140 S JointConfig<S>::getValue(std::size_t i) const
141 {
142 return values_[i];
143 }
144
145 //==============================================================================
146 template <typename S>
getValue(std::size_t i)147 S& JointConfig<S>::getValue(std::size_t i)
148 {
149 return values_[i];
150 }
151
152 //==============================================================================
153 template <typename S>
getLimitMin(std::size_t i)154 S JointConfig<S>::getLimitMin(std::size_t i) const
155 {
156 return limits_min_[i];
157 }
158
159 //==============================================================================
160 template <typename S>
getLimitMin(std::size_t i)161 S& JointConfig<S>::getLimitMin(std::size_t i)
162 {
163 return limits_min_[i];
164 }
165
166 //==============================================================================
167 template <typename S>
getLimitMax(std::size_t i)168 S JointConfig<S>::getLimitMax(std::size_t i) const
169 {
170 return limits_max_[i];
171 }
172
173 //==============================================================================
174 template <typename S>
getLimitMax(std::size_t i)175 S& JointConfig<S>::getLimitMax(std::size_t i)
176 {
177 return limits_max_[i];
178 }
179
180 //==============================================================================
181 template <typename S>
getJoint()182 std::shared_ptr<Joint> JointConfig<S>::getJoint() const
183 {
184 return joint_.lock();
185 }
186
187 } // namespace fcl
188
189 #endif
190