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 #include "dart/constraint/JointCoulombFrictionConstraint.hpp"
34
35 #include <iostream>
36
37 #include "dart/common/Console.hpp"
38 #include "dart/dynamics/BodyNode.hpp"
39 #include "dart/dynamics/Joint.hpp"
40 #include "dart/dynamics/Skeleton.hpp"
41
42 #define DART_CFM 1e-9
43
44 namespace dart {
45 namespace constraint {
46
47 double JointCoulombFrictionConstraint::mConstraintForceMixing = DART_CFM;
48
49 //==============================================================================
JointCoulombFrictionConstraint(dynamics::Joint * _joint)50 JointCoulombFrictionConstraint::JointCoulombFrictionConstraint(
51 dynamics::Joint* _joint)
52 : ConstraintBase(),
53 mJoint(_joint),
54 mBodyNode(_joint->getChildBodyNode()),
55 mAppliedImpulseIndex(0)
56 {
57 assert(_joint);
58 assert(mBodyNode);
59
60 mLifeTime[0] = 0;
61 mLifeTime[1] = 0;
62 mLifeTime[2] = 0;
63 mLifeTime[3] = 0;
64 mLifeTime[4] = 0;
65 mLifeTime[5] = 0;
66
67 mActive[0] = false;
68 mActive[1] = false;
69 mActive[2] = false;
70 mActive[3] = false;
71 mActive[4] = false;
72 mActive[5] = false;
73 }
74
75 //==============================================================================
~JointCoulombFrictionConstraint()76 JointCoulombFrictionConstraint::~JointCoulombFrictionConstraint()
77 {
78 }
79
80 //==============================================================================
getType() const81 const std::string& JointCoulombFrictionConstraint::getType() const
82 {
83 return getStaticType();
84 }
85
86 //==============================================================================
getStaticType()87 const std::string& JointCoulombFrictionConstraint::getStaticType()
88 {
89 static const std::string name = "JointCoulombFrictionConstraint";
90 return name;
91 }
92
93 //==============================================================================
setConstraintForceMixing(double _cfm)94 void JointCoulombFrictionConstraint::setConstraintForceMixing(double _cfm)
95 {
96 // Clamp constraint force mixing parameter if it is out of the range
97 if (_cfm < 1e-9)
98 {
99 dtwarn << "Constraint force mixing parameter[" << _cfm
100 << "] is lower than 1e-9. "
101 << "It is set to 1e-9." << std::endl;
102 mConstraintForceMixing = 1e-9;
103 }
104
105 mConstraintForceMixing = _cfm;
106 }
107
108 //==============================================================================
getConstraintForceMixing()109 double JointCoulombFrictionConstraint::getConstraintForceMixing()
110 {
111 return mConstraintForceMixing;
112 }
113
114 //==============================================================================
update()115 void JointCoulombFrictionConstraint::update()
116 {
117 // Reset dimention
118 mDim = 0;
119
120 std::size_t dof = mJoint->getNumDofs();
121 for (std::size_t i = 0; i < dof; ++i)
122 {
123 mNegativeVel[i] = -mJoint->getVelocity(i);
124
125 if (mNegativeVel[i] != 0.0)
126 {
127 double timeStep = mJoint->getSkeleton()->getTimeStep();
128 // TODO: There are multiple ways to get time step (or its inverse).
129 // - ContactConstraint get it from the constructor parameter
130 // - Skeleton has it itself.
131 // - ConstraintBase::getInformation() passes ConstraintInfo structure
132 // that contains reciprocal time step.
133 // We might need to pick one way and remove the others to get rid of
134 // redundancy.
135
136 // Note: Coulomb friction is force not impulse
137 mUpperBound[i] = mJoint->getCoulombFriction(i) * timeStep;
138 mLowerBound[i] = -mUpperBound[i];
139
140 if (mActive[i])
141 {
142 ++(mLifeTime[i]);
143 }
144 else
145 {
146 mActive[i] = true;
147 mLifeTime[i] = 0;
148 }
149
150 ++mDim;
151 }
152 else
153 {
154 mActive[i] = false;
155 }
156 }
157 }
158
159 //==============================================================================
getInformation(ConstraintInfo * _lcp)160 void JointCoulombFrictionConstraint::getInformation(ConstraintInfo* _lcp)
161 {
162 std::size_t index = 0;
163 std::size_t dof = mJoint->getNumDofs();
164 for (std::size_t i = 0; i < dof; ++i)
165 {
166 if (mActive[i] == false)
167 continue;
168
169 assert(_lcp->w[index] == 0.0);
170
171 _lcp->b[index] = mNegativeVel[i];
172 _lcp->lo[index] = mLowerBound[i];
173 _lcp->hi[index] = mUpperBound[i];
174
175 assert(_lcp->findex[index] == -1);
176
177 if (mLifeTime[i])
178 _lcp->x[index] = mOldX[i];
179 else
180 _lcp->x[index] = 0.0;
181
182 index++;
183 }
184 }
185
186 //==============================================================================
applyUnitImpulse(std::size_t _index)187 void JointCoulombFrictionConstraint::applyUnitImpulse(std::size_t _index)
188 {
189 assert(_index < mDim && "Invalid Index.");
190
191 std::size_t localIndex = 0;
192 const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton();
193
194 std::size_t dof = mJoint->getNumDofs();
195 for (std::size_t i = 0; i < dof; ++i)
196 {
197 if (mActive[i] == false)
198 continue;
199
200 if (localIndex == _index)
201 {
202 skeleton->clearConstraintImpulses();
203 mJoint->setConstraintImpulse(i, 1.0);
204 skeleton->updateBiasImpulse(mBodyNode);
205 skeleton->updateVelocityChange();
206 mJoint->setConstraintImpulse(i, 0.0);
207 }
208
209 ++localIndex;
210 }
211
212 mAppliedImpulseIndex = _index;
213 }
214
215 //==============================================================================
getVelocityChange(double * _delVel,bool _withCfm)216 void JointCoulombFrictionConstraint::getVelocityChange(
217 double* _delVel, bool _withCfm)
218 {
219 assert(_delVel != nullptr && "Null pointer is not allowed.");
220
221 std::size_t localIndex = 0;
222 std::size_t dof = mJoint->getNumDofs();
223 for (std::size_t i = 0; i < dof; ++i)
224 {
225 if (mActive[i] == false)
226 continue;
227
228 if (mJoint->getSkeleton()->isImpulseApplied())
229 _delVel[localIndex] = mJoint->getVelocityChange(i);
230 else
231 _delVel[localIndex] = 0.0;
232
233 ++localIndex;
234 }
235
236 // Add small values to diagnal to keep it away from singular, similar to cfm
237 // varaible in ODE
238 if (_withCfm)
239 {
240 _delVel[mAppliedImpulseIndex]
241 += _delVel[mAppliedImpulseIndex] * mConstraintForceMixing;
242 }
243
244 assert(localIndex == mDim);
245 }
246
247 //==============================================================================
excite()248 void JointCoulombFrictionConstraint::excite()
249 {
250 mJoint->getSkeleton()->setImpulseApplied(true);
251 }
252
253 //==============================================================================
unexcite()254 void JointCoulombFrictionConstraint::unexcite()
255 {
256 mJoint->getSkeleton()->setImpulseApplied(false);
257 }
258
259 //==============================================================================
applyImpulse(double * _lambda)260 void JointCoulombFrictionConstraint::applyImpulse(double* _lambda)
261 {
262 std::size_t localIndex = 0;
263 std::size_t dof = mJoint->getNumDofs();
264 for (std::size_t i = 0; i < dof; ++i)
265 {
266 if (mActive[i] == false)
267 continue;
268
269 mJoint->setConstraintImpulse(
270 i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]);
271
272 mOldX[i] = _lambda[localIndex];
273
274 ++localIndex;
275 }
276 }
277
278 //==============================================================================
getRootSkeleton() const279 dynamics::SkeletonPtr JointCoulombFrictionConstraint::getRootSkeleton() const
280 {
281 return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
282 }
283
284 //==============================================================================
isActive() const285 bool JointCoulombFrictionConstraint::isActive() const
286 {
287 for (std::size_t i = 0; i < 6; ++i)
288 {
289 if (mActive[i])
290 return true;
291 }
292
293 return false;
294 }
295
296 } // namespace constraint
297 } // namespace dart
298