1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
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 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifndef DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_
34 #define DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_
35
36 #include "dart/common/AspectWithVersion.hpp"
37 #include "dart/dynamics/Joint.hpp"
38 #include "dart/math/Helpers.hpp"
39
40 namespace dart {
41 namespace dynamics {
42
43 // Forward declare the GenericJoint class
44 template <class ConfigSpaceT>
45 class GenericJoint;
46
47 namespace detail {
48
49 //==============================================================================
50 template <class ConfigSpaceT>
51 struct GenericJointState
52 {
53 constexpr static std::size_t NumDofs = ConfigSpaceT::NumDofs;
54 using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
55 using Vector = typename ConfigSpaceT::Vector;
56
57 /// Position
58 EuclideanPoint mPositions;
59
60 /// Generalized velocity
61 Vector mVelocities;
62
63 /// Generalized acceleration
64 Vector mAccelerations;
65
66 /// Generalized force
67 Vector mForces;
68
69 /// Command
70 Vector mCommands;
71
72 GenericJointState(
73 const EuclideanPoint& positions = EuclideanPoint::Zero(),
74 const Vector& velocities = Vector::Zero(),
75 const Vector& accelerations = Vector::Zero(),
76 const Vector& forces = Vector::Zero(),
77 const Vector& commands = Vector::Zero());
78
79 virtual ~GenericJointState() = default;
80
81 // To get byte-aligned Eigen vectors
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 };
84
85 //==============================================================================
86 template <class ConfigSpaceT>
87 struct GenericJointUniqueProperties
88 {
89 constexpr static std::size_t NumDofs = ConfigSpaceT::NumDofs;
90 using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
91 using Vector = typename ConfigSpaceT::Vector;
92 using BoolArray = std::array<bool, NumDofs>;
93 using StringArray = std::array<std::string, NumDofs>;
94
95 /// Lower limit of position
96 EuclideanPoint mPositionLowerLimits;
97
98 /// Upper limit of position
99 EuclideanPoint mPositionUpperLimits;
100
101 /// Initial positions
102 EuclideanPoint mInitialPositions;
103
104 /// Min value allowed.
105 Vector mVelocityLowerLimits;
106
107 /// Max value allowed.
108 Vector mVelocityUpperLimits;
109
110 /// Initial velocities
111 Vector mInitialVelocities;
112
113 /// Min value allowed.
114 Vector mAccelerationLowerLimits;
115
116 /// upper limit of generalized acceleration
117 Vector mAccelerationUpperLimits;
118
119 /// Min value allowed.
120 Vector mForceLowerLimits;
121
122 /// Max value allowed.
123 Vector mForceUpperLimits;
124
125 /// Joint spring stiffness
126 Vector mSpringStiffnesses;
127
128 /// Rest joint position for joint spring
129 EuclideanPoint mRestPositions;
130
131 /// Joint damping coefficient
132 Vector mDampingCoefficients;
133
134 /// Joint Coulomb friction
135 Vector mFrictions;
136
137 /// True if the name of the corresponding DOF is not allowed to be
138 /// overwritten
139 BoolArray mPreserveDofNames;
140
141 /// The name of the DegreesOfFreedom for this Joint
142 StringArray mDofNames;
143
144 /// Default constructor
145 GenericJointUniqueProperties(
146 const EuclideanPoint& positionLowerLimits
147 = EuclideanPoint::Constant(-math::constantsd::inf()),
148 const EuclideanPoint& positionUpperLimits
149 = EuclideanPoint::Constant(math::constantsd::inf()),
150 const EuclideanPoint& initialPositions = EuclideanPoint::Zero(),
151 const Vector& velocityLowerLimits
152 = Vector::Constant(-math::constantsd::inf()),
153 const Vector& velocityUpperLimits
154 = Vector::Constant(math::constantsd::inf()),
155 const Vector& initialVelocities = Vector::Zero(),
156 const Vector& accelerationLowerLimits
157 = Vector::Constant(-math::constantsd::inf()),
158 const Vector& accelerationUpperLimits
159 = Vector::Constant(math::constantsd::inf()),
160 const Vector& forceLowerLimits
161 = Vector::Constant(-math::constantsd::inf()),
162 const Vector& forceUpperLimits
163 = Vector::Constant(math::constantsd::inf()),
164 const Vector& springStiffness = Vector::Zero(),
165 const EuclideanPoint& restPosition = EuclideanPoint::Zero(),
166 const Vector& dampingCoefficient = Vector::Zero(),
167 const Vector& coulombFrictions = Vector::Zero());
168
169 /// Copy constructor
170 // Note: we only need this because VS2013 lacks full support for std::array
171 // Once std::array is properly supported, this should be removed.
172 GenericJointUniqueProperties(const GenericJointUniqueProperties& other);
173
174 virtual ~GenericJointUniqueProperties() = default;
175
176 /// Copy assignment operator
177 // Note: we only need this because VS2013 lacks full support for std::array
178 // Once std::array is properly supported, this should be removed.
179 GenericJointUniqueProperties& operator=(
180 const GenericJointUniqueProperties& other);
181
182 public:
183 // To get byte-aligned Eigen vectors
184 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185 };
186
187 //==============================================================================
188 template <class ConfigSpaceT>
189 struct GenericJointProperties : Joint::Properties,
190 GenericJointUniqueProperties<ConfigSpaceT>
191 {
192 GenericJointProperties(
193 const Joint::Properties& jointProperties = Joint::Properties(),
194 const GenericJointUniqueProperties<ConfigSpaceT>& genericProperties
195 = GenericJointUniqueProperties<ConfigSpaceT>());
196
197 virtual ~GenericJointProperties() = default;
198
199 public:
200 // To get byte-aligned Eigen vectors
201 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
202 };
203
204 //==============================================================================
205 //
206 // These namespace-level definitions are required to enable ODR-use of static
207 // constexpr member variables.
208 //
209 // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426
210 //
211 template <class ConfigSpaceT>
212 constexpr std::size_t GenericJointState<ConfigSpaceT>::NumDofs;
213
214 template <class ConfigSpaceT>
215 constexpr std::size_t GenericJointUniqueProperties<ConfigSpaceT>::NumDofs;
216
217 //==============================================================================
218 template <class ConfigSpaceT>
GenericJointState(const EuclideanPoint & positions,const Vector & velocities,const Vector & accelerations,const Vector & forces,const Vector & commands)219 GenericJointState<ConfigSpaceT>::GenericJointState(
220 const EuclideanPoint& positions,
221 const Vector& velocities,
222 const Vector& accelerations,
223 const Vector& forces,
224 const Vector& commands)
225 : mPositions(positions),
226 mVelocities(velocities),
227 mAccelerations(accelerations),
228 mForces(forces),
229 mCommands(commands)
230 {
231 // Do nothing
232 }
233
234 //==============================================================================
235 template <class ConfigSpaceT>
GenericJointUniqueProperties(const EuclideanPoint & positionLowerLimits,const EuclideanPoint & positionUpperLimits,const EuclideanPoint & initialPositions,const Vector & velocityLowerLimits,const Vector & velocityUpperLimits,const Vector & initialVelocities,const Vector & accelerationLowerLimits,const Vector & accelerationUpperLimits,const Vector & forceLowerLimits,const Vector & forceUpperLimits,const Vector & springStiffness,const EuclideanPoint & restPosition,const Vector & dampingCoefficient,const Vector & coulombFrictions)236 GenericJointUniqueProperties<ConfigSpaceT>::GenericJointUniqueProperties(
237 const EuclideanPoint& positionLowerLimits,
238 const EuclideanPoint& positionUpperLimits,
239 const EuclideanPoint& initialPositions,
240 const Vector& velocityLowerLimits,
241 const Vector& velocityUpperLimits,
242 const Vector& initialVelocities,
243 const Vector& accelerationLowerLimits,
244 const Vector& accelerationUpperLimits,
245 const Vector& forceLowerLimits,
246 const Vector& forceUpperLimits,
247 const Vector& springStiffness,
248 const EuclideanPoint& restPosition,
249 const Vector& dampingCoefficient,
250 const Vector& coulombFrictions)
251 : mPositionLowerLimits(positionLowerLimits),
252 mPositionUpperLimits(positionUpperLimits),
253 mInitialPositions(initialPositions),
254 mVelocityLowerLimits(velocityLowerLimits),
255 mVelocityUpperLimits(velocityUpperLimits),
256 mInitialVelocities(initialVelocities),
257 mAccelerationLowerLimits(accelerationLowerLimits),
258 mAccelerationUpperLimits(accelerationUpperLimits),
259 mForceLowerLimits(forceLowerLimits),
260 mForceUpperLimits(forceUpperLimits),
261 mSpringStiffnesses(springStiffness),
262 mRestPositions(restPosition),
263 mDampingCoefficients(dampingCoefficient),
264 mFrictions(coulombFrictions)
265 {
266 for (auto i = 0u; i < NumDofs; ++i)
267 {
268 mPreserveDofNames[i] = false;
269 mDofNames[i] = std::string();
270 }
271 }
272
273 //==============================================================================
274 template <class ConfigSpaceT>
GenericJointUniqueProperties(const GenericJointUniqueProperties & _other)275 GenericJointUniqueProperties<ConfigSpaceT>::GenericJointUniqueProperties(
276 const GenericJointUniqueProperties& _other)
277 : mPositionLowerLimits(_other.mPositionLowerLimits),
278 mPositionUpperLimits(_other.mPositionUpperLimits),
279 mInitialPositions(_other.mInitialPositions),
280 mVelocityLowerLimits(_other.mVelocityLowerLimits),
281 mVelocityUpperLimits(_other.mVelocityUpperLimits),
282 mInitialVelocities(_other.mInitialVelocities),
283 mAccelerationLowerLimits(_other.mAccelerationLowerLimits),
284 mAccelerationUpperLimits(_other.mAccelerationUpperLimits),
285 mForceLowerLimits(_other.mForceLowerLimits),
286 mForceUpperLimits(_other.mForceUpperLimits),
287 mSpringStiffnesses(_other.mSpringStiffnesses),
288 mRestPositions(_other.mRestPositions),
289 mDampingCoefficients(_other.mDampingCoefficients),
290 mFrictions(_other.mFrictions)
291 {
292 for (auto i = 0u; i < NumDofs; ++i)
293 {
294 mPreserveDofNames[i] = _other.mPreserveDofNames[i];
295 mDofNames[i] = _other.mDofNames[i];
296 }
297 }
298
299 //==============================================================================
300 template <class ConfigSpaceT>
301 GenericJointUniqueProperties<ConfigSpaceT>&
operator =(const GenericJointUniqueProperties & other)302 GenericJointUniqueProperties<ConfigSpaceT>::operator=(
303 const GenericJointUniqueProperties& other)
304 {
305 if (this != &other)
306 {
307 mPositionLowerLimits = other.mPositionLowerLimits;
308 mPositionUpperLimits = other.mPositionUpperLimits;
309 mInitialPositions = other.mInitialPositions;
310 mVelocityLowerLimits = other.mVelocityLowerLimits;
311 mVelocityUpperLimits = other.mVelocityUpperLimits;
312 mInitialVelocities = other.mInitialVelocities;
313 mAccelerationLowerLimits = other.mAccelerationLowerLimits;
314 mAccelerationUpperLimits = other.mAccelerationUpperLimits;
315 mForceLowerLimits = other.mForceLowerLimits;
316 mForceUpperLimits = other.mForceUpperLimits;
317 mSpringStiffnesses = other.mSpringStiffnesses;
318 mRestPositions = other.mRestPositions;
319 mDampingCoefficients = other.mDampingCoefficients;
320 mFrictions = other.mFrictions;
321
322 for (auto i = 0u; i < NumDofs; ++i)
323 {
324 mPreserveDofNames[i] = other.mPreserveDofNames[i];
325 mDofNames[i] = other.mDofNames[i];
326 }
327 }
328 return *this;
329 }
330
331 //==============================================================================
332 template <class ConfigSpaceT>
GenericJointProperties(const Joint::Properties & jointProperties,const GenericJointUniqueProperties<ConfigSpaceT> & genericProperties)333 GenericJointProperties<ConfigSpaceT>::GenericJointProperties(
334 const Joint::Properties& jointProperties,
335 const GenericJointUniqueProperties<ConfigSpaceT>& genericProperties)
336 : Joint::Properties(jointProperties),
337 GenericJointUniqueProperties<ConfigSpaceT>(genericProperties)
338 {
339 // Do nothing
340 }
341
342 //==============================================================================
343 template <class Derived, class ConfigSpaceT>
344 using GenericJointBase = common::EmbedStateAndPropertiesOnTopOf<
345 Derived,
346 GenericJointState<ConfigSpaceT>,
347 GenericJointUniqueProperties<ConfigSpaceT>,
348 Joint>;
349
350 } // namespace detail
351
352 } // namespace dynamics
353 } // namespace dart
354
355 #endif // DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_
356