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/MimicMotorConstraint.hpp"
34
35 #include <iostream>
36
37 #include "dart/external/odelcpsolver/lcp.h"
38
39 #include "dart/common/Console.hpp"
40 #include "dart/dynamics/BodyNode.hpp"
41 #include "dart/dynamics/Joint.hpp"
42 #include "dart/dynamics/Skeleton.hpp"
43
44 #define DART_CFM 1e-9
45
46 namespace dart {
47 namespace constraint {
48
49 double MimicMotorConstraint::mConstraintForceMixing = DART_CFM;
50
51 //==============================================================================
MimicMotorConstraint(dynamics::Joint * joint,const dynamics::Joint * mimicJoint,double multiplier,double offset)52 MimicMotorConstraint::MimicMotorConstraint(
53 dynamics::Joint* joint,
54 const dynamics::Joint* mimicJoint,
55 double multiplier,
56 double offset)
57 : ConstraintBase(),
58 mJoint(joint),
59 mMimicJoint(mimicJoint),
60 mMultiplier(multiplier),
61 mOffset(offset),
62 mBodyNode(joint->getChildBodyNode()),
63 mAppliedImpulseIndex(0)
64 {
65 assert(joint);
66 assert(mimicJoint);
67 assert(mBodyNode);
68 assert(joint->getNumDofs() == mimicJoint->getNumDofs());
69
70 mLifeTime[0] = 0;
71 mLifeTime[1] = 0;
72 mLifeTime[2] = 0;
73 mLifeTime[3] = 0;
74 mLifeTime[4] = 0;
75 mLifeTime[5] = 0;
76
77 mActive[0] = false;
78 mActive[1] = false;
79 mActive[2] = false;
80 mActive[3] = false;
81 mActive[4] = false;
82 mActive[5] = false;
83 }
84
85 //==============================================================================
~MimicMotorConstraint()86 MimicMotorConstraint::~MimicMotorConstraint()
87 {
88 // Do nothing
89 }
90
91 //==============================================================================
getType() const92 const std::string& MimicMotorConstraint::getType() const
93 {
94 return getStaticType();
95 }
96
97 //==============================================================================
getStaticType()98 const std::string& MimicMotorConstraint::getStaticType()
99 {
100 static const std::string name = "MimicMotorConstraint";
101 return name;
102 }
103
104 //==============================================================================
setConstraintForceMixing(double cfm)105 void MimicMotorConstraint::setConstraintForceMixing(double cfm)
106 {
107 // Clamp constraint force mixing parameter if it is out of the range
108 if (cfm < 1e-9)
109 {
110 dtwarn << "[MimicMotorConstraint::setConstraintForceMixing] "
111 << "Constraint force mixing parameter[" << cfm
112 << "] is lower than 1e-9. "
113 << "It is set to 1e-9.\n";
114 mConstraintForceMixing = 1e-9;
115 }
116
117 mConstraintForceMixing = cfm;
118 }
119
120 //==============================================================================
getConstraintForceMixing()121 double MimicMotorConstraint::getConstraintForceMixing()
122 {
123 return mConstraintForceMixing;
124 }
125
126 //==============================================================================
update()127 void MimicMotorConstraint::update()
128 {
129 // Reset dimention
130 mDim = 0;
131
132 std::size_t dof = mJoint->getNumDofs();
133 for (std::size_t i = 0; i < dof; ++i)
134 {
135 double timeStep = mJoint->getSkeleton()->getTimeStep();
136 double qError = mMimicJoint->getPosition(i) * mMultiplier + mOffset
137 - mJoint->getPosition(i);
138 double desiredVelocity = math::clip(
139 qError / timeStep,
140 mJoint->getVelocityLowerLimit(i),
141 mJoint->getVelocityUpperLimit(i));
142
143 mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i);
144
145 if (mNegativeVelocityError[i] != 0.0)
146 {
147 // Note that we are computing impulse not force
148 mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep;
149 mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep;
150
151 if (mActive[i])
152 {
153 ++(mLifeTime[i]);
154 }
155 else
156 {
157 mActive[i] = true;
158 mLifeTime[i] = 0;
159 }
160
161 ++mDim;
162 }
163 else
164 {
165 mActive[i] = false;
166 }
167 }
168 }
169
170 //==============================================================================
getInformation(ConstraintInfo * lcp)171 void MimicMotorConstraint::getInformation(ConstraintInfo* lcp)
172 {
173 std::size_t index = 0;
174 std::size_t dof = mJoint->getNumDofs();
175 for (std::size_t i = 0; i < dof; ++i)
176 {
177 if (mActive[i] == false)
178 continue;
179
180 assert(lcp->w[index] == 0.0);
181
182 lcp->b[index] = mNegativeVelocityError[i];
183 lcp->lo[index] = mLowerBound[i];
184 lcp->hi[index] = mUpperBound[i];
185
186 assert(lcp->findex[index] == -1);
187
188 if (mLifeTime[i])
189 lcp->x[index] = mOldX[i];
190 else
191 lcp->x[index] = 0.0;
192
193 index++;
194 }
195 }
196
197 //==============================================================================
applyUnitImpulse(std::size_t index)198 void MimicMotorConstraint::applyUnitImpulse(std::size_t index)
199 {
200 assert(index < mDim && "Invalid Index.");
201
202 std::size_t localIndex = 0;
203 const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton();
204
205 std::size_t dof = mJoint->getNumDofs();
206 for (std::size_t i = 0; i < dof; ++i)
207 {
208 if (mActive[i] == false)
209 continue;
210
211 if (localIndex == index)
212 {
213 skeleton->clearConstraintImpulses();
214 mJoint->setConstraintImpulse(i, 1.0);
215 skeleton->updateBiasImpulse(mBodyNode);
216 skeleton->updateVelocityChange();
217 mJoint->setConstraintImpulse(i, 0.0);
218 }
219
220 ++localIndex;
221 }
222
223 mAppliedImpulseIndex = index;
224 }
225
226 //==============================================================================
getVelocityChange(double * delVel,bool withCfm)227 void MimicMotorConstraint::getVelocityChange(double* delVel, bool withCfm)
228 {
229 assert(delVel != nullptr && "Null pointer is not allowed.");
230
231 std::size_t localIndex = 0;
232 std::size_t dof = mJoint->getNumDofs();
233 for (std::size_t i = 0; i < dof; ++i)
234 {
235 if (mActive[i] == false)
236 continue;
237
238 if (mJoint->getSkeleton()->isImpulseApplied())
239 delVel[localIndex] = mJoint->getVelocityChange(i);
240 else
241 delVel[localIndex] = 0.0;
242
243 ++localIndex;
244 }
245
246 // Add small values to diagnal to keep it away from singular, similar to cfm
247 // varaible in ODE
248 if (withCfm)
249 {
250 delVel[mAppliedImpulseIndex]
251 += delVel[mAppliedImpulseIndex] * mConstraintForceMixing;
252 }
253
254 assert(localIndex == mDim);
255 }
256
257 //==============================================================================
excite()258 void MimicMotorConstraint::excite()
259 {
260 mJoint->getSkeleton()->setImpulseApplied(true);
261 }
262
263 //==============================================================================
unexcite()264 void MimicMotorConstraint::unexcite()
265 {
266 mJoint->getSkeleton()->setImpulseApplied(false);
267 }
268
269 //==============================================================================
applyImpulse(double * lambda)270 void MimicMotorConstraint::applyImpulse(double* lambda)
271 {
272 std::size_t localIndex = 0;
273 std::size_t dof = mJoint->getNumDofs();
274 for (std::size_t i = 0; i < dof; ++i)
275 {
276 if (mActive[i] == false)
277 continue;
278
279 mJoint->setConstraintImpulse(
280 i, mJoint->getConstraintImpulse(i) + lambda[localIndex]);
281 // TODO(JS): consider to add Joint::addConstraintImpulse()
282
283 mOldX[i] = lambda[localIndex];
284
285 ++localIndex;
286 }
287 }
288
289 //==============================================================================
getRootSkeleton() const290 dynamics::SkeletonPtr MimicMotorConstraint::getRootSkeleton() const
291 {
292 return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
293 }
294
295 //==============================================================================
isActive() const296 bool MimicMotorConstraint::isActive() const
297 {
298 // Since we are not allowed to set the joint actuator type per each
299 // DegreeOfFreedom, we just check if the whole joint is SERVO actuator.
300 if (mJoint->getActuatorType() == dynamics::Joint::MIMIC)
301 return true;
302
303 return false;
304 }
305
306 } // namespace constraint
307 } // namespace dart
308