1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2009-12 Stanford University and the Authors. *
10 * Authors: Michael Sherman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 #include "SimTKcommon.h"
25
26 #include "simbody/internal/common.h"
27 #include "simbody/internal/MobilizedBody.h"
28 #include "simbody/internal/SimbodyMatterSubsystem.h"
29 #include "simbody/internal/Force_LinearBushing.h"
30
31 #include "ForceImpl.h"
32
33 namespace SimTK {
34
35 class Force::LinearBushingImpl : public ForceImpl {
36 struct InstanceVars {
InstanceVarsSimTK::Force::LinearBushingImpl::InstanceVars37 InstanceVars(const Transform& defX_B1F, const Transform& defX_B2M,
38 const Vec6& defStiffness, const Vec6& defDamping)
39 : X_B1F(defX_B1F), X_B2M(defX_B2M), k(defStiffness), c(defDamping) {}
40
41 InstanceVars() = default;
42
43 Transform X_B1F, X_B2M;
44 Vec6 k{NaN}, c{NaN};
45 };
46 struct PositionCache {
47 Transform X_GF, X_GM, X_FM;
48 Vec3 p_B1F_G, p_B2M_G, p_FM_G;
49 Vec6 q;
50 };
51 struct VelocityCache {
52 SpatialVec V_GF, V_GM, V_FM;
53 Vec6 qdot;
54 };
55 struct ForceCache {
56 SpatialVec F_GF, F_GM; // at Bushing frames
57 SpatialVec F_GB1, F_GB2; // at Body frames
58 Vec6 f; // scalar generalized forces
59 Real power;
60 };
61 public:
62 LinearBushingImpl(const MobilizedBody& body1, const Transform& frameOnB1,
63 const MobilizedBody& body2, const Transform& frameOnB2,
64 const Vec6& stiffness, const Vec6& damping);
clone() const65 LinearBushingImpl* clone() const override {
66 return new LinearBushingImpl(*this);
67 }
dependsOnlyOnPositions() const68 bool dependsOnlyOnPositions() const override {
69 return false;
70 }
71 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
72 Vector_<Vec3>& particleForces, Vector& mobilityForces) const override;
73 Real calcPotentialEnergy(const State& state) const override;
74
75 // Allocate the position and velocity cache entries. These are all
76 // lazy-evaluation entries - be sure to check whether they have already
77 // been calculated; calculate them if not; and then mark them done.
78 // They will be invalidated when the indicated stage has changed and
79 // can be recalculated any time after that stage is realized.
realizeTopology(State & s) const80 void realizeTopology(State& s) const override {
81 LinearBushingImpl* mThis = const_cast<LinearBushingImpl*>(this);
82
83 const InstanceVars iv(defX_B1F,defX_B2M,defK,defC);
84 mThis->instanceVarsIx = getForceSubsystem()
85 .allocateDiscreteVariable(s, Stage::Instance,
86 new Value<InstanceVars>(iv));
87
88 Vector einit(1, Real(0));
89 mThis->dissipatedEnergyIx = getForceSubsystem().allocateZ(s,einit);
90
91 mThis->positionCacheIx = getForceSubsystem().allocateCacheEntry(s,
92 Stage::Position, Stage::Infinity, new Value<PositionCache>());
93 mThis->potEnergyCacheIx = getForceSubsystem().allocateCacheEntry(s,
94 Stage::Position, Stage::Infinity, new Value<Real>(NaN));
95 mThis->velocityCacheIx = getForceSubsystem().allocateCacheEntry(s,
96 Stage::Velocity, Stage::Infinity, new Value<VelocityCache>());
97 mThis->forceCacheIx = getForceSubsystem().allocateCacheEntry(s,
98 Stage::Velocity, Stage::Infinity, new Value<ForceCache>());
99 }
100
realizeAcceleration(const State & s) const101 void realizeAcceleration(const State& s) const override {
102 ensureForceCacheValid(s);
103 updDissipatedEnergyDeriv(s) = getForceCache(s).power;
104 }
105
106 private:
getInstanceVars(const State & s) const107 const InstanceVars& getInstanceVars(const State& s) const
108 { return Value<InstanceVars>::downcast
109 (getForceSubsystem().getDiscreteVariable(s,instanceVarsIx)); }
updInstanceVars(State & s) const110 InstanceVars& updInstanceVars(State& s) const
111 { return Value<InstanceVars>::updDowncast
112 (getForceSubsystem().updDiscreteVariable(s,instanceVarsIx)); }
113
getDissipatedEnergyVar(const State & s) const114 const Real& getDissipatedEnergyVar(const State& s) const
115 { return getForceSubsystem().getZ(s)[dissipatedEnergyIx]; }
updDissipatedEnergyVar(State & s) const116 Real& updDissipatedEnergyVar(State& s) const
117 { return getForceSubsystem().updZ(s)[dissipatedEnergyIx]; }
updDissipatedEnergyDeriv(const State & s) const118 Real& updDissipatedEnergyDeriv(const State& s) const
119 { return getForceSubsystem().updZDot(s)[dissipatedEnergyIx]; }
120
getPositionCache(const State & s) const121 const PositionCache& getPositionCache(const State& s) const
122 { return Value<PositionCache>::downcast
123 (getForceSubsystem().getCacheEntry(s,positionCacheIx)); }
getPotentialEnergyCache(const State & s) const124 const Real& getPotentialEnergyCache(const State& s) const
125 { return Value<Real>::downcast
126 (getForceSubsystem().getCacheEntry(s,potEnergyCacheIx)); }
getVelocityCache(const State & s) const127 const VelocityCache& getVelocityCache(const State& s) const
128 { return Value<VelocityCache>::downcast
129 (getForceSubsystem().getCacheEntry(s,velocityCacheIx)); }
getForceCache(const State & s) const130 const ForceCache& getForceCache(const State& s) const
131 { return Value<ForceCache>::downcast
132 (getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
133
updPositionCache(const State & s) const134 PositionCache& updPositionCache(const State& s) const
135 { return Value<PositionCache>::updDowncast
136 (getForceSubsystem().updCacheEntry(s,positionCacheIx)); }
updPotentialEnergyCache(const State & s) const137 Real& updPotentialEnergyCache(const State& s) const
138 { return Value<Real>::updDowncast
139 (getForceSubsystem().updCacheEntry(s,potEnergyCacheIx)); }
updVelocityCache(const State & s) const140 VelocityCache& updVelocityCache(const State& s) const
141 { return Value<VelocityCache>::updDowncast
142 (getForceSubsystem().updCacheEntry(s,velocityCacheIx)); }
updForceCache(const State & s) const143 ForceCache& updForceCache(const State& s) const
144 { return Value<ForceCache>::updDowncast
145 (getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
146
isPositionCacheValid(const State & s) const147 bool isPositionCacheValid(const State& s) const
148 { return getForceSubsystem().isCacheValueRealized(s,positionCacheIx); }
isPotentialEnergyValid(const State & s) const149 bool isPotentialEnergyValid(const State& s) const
150 { return getForceSubsystem().isCacheValueRealized(s,potEnergyCacheIx); }
isVelocityCacheValid(const State & s) const151 bool isVelocityCacheValid(const State& s) const
152 { return getForceSubsystem().isCacheValueRealized(s,velocityCacheIx); }
isForceCacheValid(const State & s) const153 bool isForceCacheValid(const State& s) const
154 { return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
155
markPositionCacheValid(const State & s) const156 void markPositionCacheValid(const State& s) const
157 { getForceSubsystem().markCacheValueRealized(s,positionCacheIx); }
markPotentialEnergyValid(const State & s) const158 void markPotentialEnergyValid(const State& s) const
159 { getForceSubsystem().markCacheValueRealized(s,potEnergyCacheIx); }
markVelocityCacheValid(const State & s) const160 void markVelocityCacheValid(const State& s) const
161 { getForceSubsystem().markCacheValueRealized(s,velocityCacheIx); }
markForceCacheValid(const State & s) const162 void markForceCacheValid(const State& s) const
163 { getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
164
165 void ensurePositionCacheValid(const State&) const;
166 void ensurePotentialEnergyValid(const State&) const;
167 void ensureVelocityCacheValid(const State&) const;
168 void ensureForceCacheValid(const State&) const;
169
170 // TOPOLOGY STATE
171 const SimbodyMatterSubsystem& matter;
172 MobilizedBodyIndex body1x;
173 MobilizedBodyIndex body2x;
174 Transform defX_B1F, defX_B2M;
175 Vec6 defK, defC;
176
177 // TOPOLOGY CACHE
178 DiscreteVariableIndex instanceVarsIx;
179 ZIndex dissipatedEnergyIx;
180 CacheEntryIndex positionCacheIx;
181 CacheEntryIndex potEnergyCacheIx;
182 CacheEntryIndex velocityCacheIx;
183 CacheEntryIndex forceCacheIx;
184
185 friend class Force::LinearBushing;
186 friend std::ostream& operator<<(std::ostream&,const InstanceVars&);
187 friend std::ostream& operator<<(std::ostream&,const PositionCache&);
188 friend std::ostream& operator<<(std::ostream&,const VelocityCache&);
189 friend std::ostream& operator<<(std::ostream&,const ForceCache&);
190 };
191
192 // These are required by Value<T>.
operator <<(std::ostream & o,const Force::LinearBushingImpl::InstanceVars & iv)193 inline std::ostream& operator<<
194 (std::ostream& o, const Force::LinearBushingImpl::InstanceVars& iv)
195 { assert(!"implemented"); return o; }
operator <<(std::ostream & o,const Force::LinearBushingImpl::PositionCache & pc)196 inline std::ostream& operator<<
197 (std::ostream& o, const Force::LinearBushingImpl::PositionCache& pc)
198 { assert(!"implemented"); return o; }
operator <<(std::ostream & o,const Force::LinearBushingImpl::VelocityCache & vc)199 inline std::ostream& operator<<
200 (std::ostream& o, const Force::LinearBushingImpl::VelocityCache& vc)
201 { assert(!"implemented"); return o; }
operator <<(std::ostream & o,const Force::LinearBushingImpl::ForceCache & fc)202 inline std::ostream& operator<<
203 (std::ostream& o, const Force::LinearBushingImpl::ForceCache& fc)
204 { assert(!"implemented"); return o; }
205
206
207 //------------------------------ LinearBushing ---------------------------------
208 //------------------------------------------------------------------------------
209
210 SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::LinearBushing,
211 Force::LinearBushingImpl, Force);
212
LinearBushing(GeneralForceSubsystem & forces,const MobilizedBody & body1,const Transform & frameOnB1,const MobilizedBody & body2,const Transform & frameOnB2,const Vec6 & stiffness,const Vec6 & damping)213 Force::LinearBushing::LinearBushing
214 (GeneralForceSubsystem& forces,
215 const MobilizedBody& body1, const Transform& frameOnB1,
216 const MobilizedBody& body2, const Transform& frameOnB2,
217 const Vec6& stiffness, const Vec6& damping)
218 : Force(new LinearBushingImpl(body1,frameOnB1,body2,frameOnB2,
219 stiffness,damping))
220 {
221 SimTK_ERRCHK_ALWAYS(stiffness >= 0,
222 "Force::LinearBushing::ctor()",
223 "Bushing spring constants must be nonnegative.");
224 SimTK_ERRCHK_ALWAYS(damping >= 0,
225 "Force::LinearBushing::ctor()",
226 "Bushing damping coefficients must be nonnegative.");
227 updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
228 }
229
230
LinearBushing(GeneralForceSubsystem & forces,const MobilizedBody & body1,const MobilizedBody & body2,const Vec6 & stiffness,const Vec6 & damping)231 Force::LinearBushing::LinearBushing
232 (GeneralForceSubsystem& forces,
233 const MobilizedBody& body1, // assume body frames
234 const MobilizedBody& body2,
235 const Vec6& stiffness, const Vec6& damping)
236 : Force(new LinearBushingImpl(body1,Transform(),body2,Transform(),
237 stiffness,damping))
238 {
239 SimTK_ERRCHK_ALWAYS(stiffness >= 0,
240 "Force::LinearBushing::ctor()",
241 "Bushing spring constants must be nonnegative.");
242 SimTK_ERRCHK_ALWAYS(damping >= 0,
243 "Force::LinearBushing::ctor()",
244 "Bushing damping coefficients must be nonnegative.");
245 updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
246 }
247
248 Force::LinearBushing& Force::LinearBushing::
setDefaultFrameOnBody1(const Transform & X_B1F)249 setDefaultFrameOnBody1(const Transform& X_B1F) {
250 getImpl().invalidateTopologyCache();
251 updImpl().defX_B1F = X_B1F;
252 return *this;
253 }
254 Force::LinearBushing& Force::LinearBushing::
setDefaultFrameOnBody2(const Transform & X_B2M)255 setDefaultFrameOnBody2(const Transform& X_B2M) {
256 getImpl().invalidateTopologyCache();
257 updImpl().defX_B2M = X_B2M;
258 return *this;
259 }
260 Force::LinearBushing& Force::LinearBushing::
setDefaultStiffness(const Vec6 & stiffness)261 setDefaultStiffness(const Vec6& stiffness) {
262 SimTK_ERRCHK_ALWAYS(stiffness >= 0,
263 "Force::LinearBushing::setDefaultStiffness()",
264 "Bushing spring constants must be nonnegative.");
265 getImpl().invalidateTopologyCache();
266 updImpl().defK = stiffness;
267 return *this;
268 }
269 Force::LinearBushing& Force::LinearBushing::
setDefaultDamping(const Vec6 & damping)270 setDefaultDamping(const Vec6& damping) {
271 SimTK_ERRCHK_ALWAYS(damping >= 0,
272 "Force::LinearBushing::setDefaultDamping()",
273 "Bushing damping coefficients must be nonnegative.");
274 getImpl().invalidateTopologyCache();
275 updImpl().defC = damping;
276 return *this;
277 }
278 const Transform& Force::LinearBushing::
getDefaultFrameOnBody1() const279 getDefaultFrameOnBody1() const {return getImpl().defX_B1F;}
280 const Transform& Force::LinearBushing::
getDefaultFrameOnBody2() const281 getDefaultFrameOnBody2() const {return getImpl().defX_B2M;}
282 const Vec6& Force::LinearBushing::
getDefaultStiffness() const283 getDefaultStiffness() const {return getImpl().defK;}
284 const Vec6& Force::LinearBushing::
getDefaultDamping() const285 getDefaultDamping() const {return getImpl().defC;}
286
287 const Force::LinearBushing& Force::LinearBushing::
setFrameOnBody1(State & state,const Transform & X_B1F) const288 setFrameOnBody1(State& state, const Transform& X_B1F) const {
289 getImpl().updInstanceVars(state).X_B1F = X_B1F;
290 return *this;
291 }
292 const Force::LinearBushing& Force::LinearBushing::
setFrameOnBody2(State & state,const Transform & X_B2M) const293 setFrameOnBody2(State& state, const Transform& X_B2M) const {
294 getImpl().updInstanceVars(state).X_B2M = X_B2M;
295 return *this;
296 }
297 const Force::LinearBushing& Force::LinearBushing::
setStiffness(State & state,const Vec6 & stiffness) const298 setStiffness(State& state, const Vec6& stiffness) const {
299 SimTK_ERRCHK_ALWAYS(stiffness >= 0,
300 "Force::LinearBushing::setStiffness()",
301 "Bushing spring constants must be nonnegative.");
302 getImpl().updInstanceVars(state).k = stiffness;
303 return *this;
304 }
305 const Force::LinearBushing& Force::LinearBushing::
setDamping(State & state,const Vec6 & damping) const306 setDamping(State& state, const Vec6& damping) const {
307 SimTK_ERRCHK_ALWAYS(damping >= 0,
308 "Force::LinearBushing::setDamping()",
309 "Bushing damping coefficients must be nonnegative.");
310 getImpl().updInstanceVars(state).c = damping;
311 return *this;
312 }
313 const Transform& Force::LinearBushing::
getFrameOnBody1(const State & state) const314 getFrameOnBody1(const State& state) const
315 { return getImpl().getInstanceVars(state).X_B1F; }
316 const Transform& Force::LinearBushing::
getFrameOnBody2(const State & state) const317 getFrameOnBody2(const State& state) const
318 { return getImpl().getInstanceVars(state).X_B2M; }
319 const Vec6& Force::LinearBushing::
getStiffness(const State & state) const320 getStiffness(const State& state) const
321 { return getImpl().getInstanceVars(state).k; }
322 const Vec6& Force::LinearBushing::
getDamping(const State & state) const323 getDamping(const State& state) const
324 { return getImpl().getInstanceVars(state).c; }
325
326 const Vec6& Force::LinearBushing::
getQ(const State & s) const327 getQ(const State& s) const
328 { getImpl().ensurePositionCacheValid(s);
329 return getImpl().getPositionCache(s).q; }
330
331 const Transform& Force::LinearBushing::
getX_GF(const State & s) const332 getX_GF(const State& s) const
333 { getImpl().ensurePositionCacheValid(s);
334 return getImpl().getPositionCache(s).X_GF; }
335 const Transform& Force::LinearBushing::
getX_GM(const State & s) const336 getX_GM(const State& s) const
337 { getImpl().ensurePositionCacheValid(s);
338 return getImpl().getPositionCache(s).X_GM; }
339 const Transform& Force::LinearBushing::
getX_FM(const State & s) const340 getX_FM(const State& s) const
341 { getImpl().ensurePositionCacheValid(s);
342 return getImpl().getPositionCache(s).X_FM; }
343
344 const Vec6& Force::LinearBushing::
getQDot(const State & s) const345 getQDot(const State& s) const
346 { getImpl().ensureVelocityCacheValid(s);
347 return getImpl().getVelocityCache(s).qdot; }
348 const SpatialVec& Force::LinearBushing::
getV_GF(const State & s) const349 getV_GF(const State& s) const
350 { getImpl().ensureVelocityCacheValid(s);
351 return getImpl().getVelocityCache(s).V_GF; }
352 const SpatialVec& Force::LinearBushing::
getV_GM(const State & s) const353 getV_GM(const State& s) const
354 { getImpl().ensureVelocityCacheValid(s);
355 return getImpl().getVelocityCache(s).V_GM; }
356 const SpatialVec& Force::LinearBushing::
getV_FM(const State & s) const357 getV_FM(const State& s) const
358 { getImpl().ensureVelocityCacheValid(s);
359 return getImpl().getVelocityCache(s).V_FM; }
360
361 const Vec6& Force::LinearBushing::
getF(const State & s) const362 getF(const State& s) const
363 { getImpl().ensureForceCacheValid(s);
364 return getImpl().getForceCache(s).f; }
365 const SpatialVec& Force::LinearBushing::
getF_GF(const State & s) const366 getF_GF(const State& s) const
367 { getImpl().ensureForceCacheValid(s);
368 return getImpl().getForceCache(s).F_GF; }
369 const SpatialVec& Force::LinearBushing::
getF_GM(const State & s) const370 getF_GM(const State& s) const
371 { getImpl().ensureForceCacheValid(s);
372 return getImpl().getForceCache(s).F_GM; }
373
374 Real Force::LinearBushing::
getPotentialEnergy(const State & s) const375 getPotentialEnergy(const State& s) const
376 { getImpl().ensurePotentialEnergyValid(s);
377 return getImpl().getPotentialEnergyCache(s); }
378
379 Real Force::LinearBushing::
getPowerDissipation(const State & s) const380 getPowerDissipation(const State& s) const
381 { getImpl().ensureForceCacheValid(s);
382 return getImpl().getForceCache(s).power; }
383
384 Real Force::LinearBushing::
getDissipatedEnergy(const State & s) const385 getDissipatedEnergy(const State& s) const
386 { return getImpl().getDissipatedEnergyVar(s); }
387
388 void Force::LinearBushing::
setDissipatedEnergy(State & s,Real energy) const389 setDissipatedEnergy(State& s, Real energy) const {
390 SimTK_ERRCHK1_ALWAYS(energy >= 0,
391 "Force::LinearBushing::setDissipatedEnergy()",
392 "The initial value for the dissipated energy must be nonnegative"
393 " but an attempt was made to set it to %g.", energy);
394 getImpl().updDissipatedEnergyVar(s) = energy;
395 }
396
397
398
399 //---------------------------- LinearBushingImpl -------------------------------
400 //------------------------------------------------------------------------------
401
402
LinearBushingImpl(const MobilizedBody & body1,const Transform & frameOnB1,const MobilizedBody & body2,const Transform & frameOnB2,const Vec6 & stiffness,const Vec6 & damping)403 Force::LinearBushingImpl::LinearBushingImpl
404 (const MobilizedBody& body1, const Transform& frameOnB1,
405 const MobilizedBody& body2, const Transform& frameOnB2,
406 const Vec6& stiffness, const Vec6& damping)
407 : matter(body1.getMatterSubsystem()),
408 body1x(body1.getMobilizedBodyIndex()),
409 body2x(body2.getMobilizedBodyIndex()),
410 defX_B1F(frameOnB1), defX_B2M(frameOnB2),
411 defK(stiffness), defC(damping)
412 {
413 }
414
415 void Force::LinearBushingImpl::
ensurePositionCacheValid(const State & state) const416 ensurePositionCacheValid(const State& state) const {
417 if (isPositionCacheValid(state)) return;
418
419 const InstanceVars& iv = getInstanceVars(state);
420 const Transform& X_B1F = iv.X_B1F;
421 const Transform& X_B2M = iv.X_B2M;
422
423 PositionCache& pc = updPositionCache(state);
424
425 const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
426 const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
427
428 const Transform& X_GB1 = body1.getBodyTransform(state);
429 const Transform& X_GB2 = body2.getBodyTransform(state);
430 pc.X_GF = X_GB1* X_B1F; // 63 flops
431 pc.X_GM = X_GB2* X_B2M; // 63 flops
432 pc.X_FM = ~pc.X_GF *pc.X_GM; // 63 flops
433
434 // Re-express local vectors in the Ground frame.
435 pc.p_B1F_G = X_GB1.R() * X_B1F.p(); // 15 flops
436 pc.p_B2M_G = X_GB2.R() * X_B2M.p(); // 15 flops
437 pc.p_FM_G = pc.X_GF.R() * pc.X_FM.p(); // 15 flops
438
439 // Calculate the 1-2-3 body B2-fixed Euler angles; these are the
440 // rotational coordinates.
441 pc.q.updSubVec<3>(0) = pc.X_FM.R().convertRotationToBodyFixedXYZ();
442
443 // The translation vector in X_FM contains our translational coordinates.
444 pc.q.updSubVec<3>(3) = pc.X_FM.p();
445
446 markPositionCacheValid(state);
447 }
448
449 void Force::LinearBushingImpl::
ensureVelocityCacheValid(const State & state) const450 ensureVelocityCacheValid(const State& state) const {
451 if (isVelocityCacheValid(state)) return;
452
453 // We'll be needing this.
454 ensurePositionCacheValid(state);
455 const PositionCache& pc = getPositionCache(state);
456 const Rotation& R_GF = pc.X_GF.R();
457 const Rotation& R_FM = pc.X_FM.R();
458 const Vec3& q = pc.q.getSubVec<3>(0);
459 const Vec3& p = pc.q.getSubVec<3>(3);
460
461 VelocityCache& vc = updVelocityCache(state);
462
463 const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
464 const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
465
466 // Now do velocities.
467 const SpatialVec& V_GB1 = body1.getBodyVelocity(state);
468 const SpatialVec& V_GB2 = body2.getBodyVelocity(state);
469
470 vc.V_GF = SpatialVec(V_GB1[0], V_GB1[1] + V_GB1[0] % pc.p_B1F_G);
471 vc.V_GM = SpatialVec(V_GB2[0], V_GB2[1] + V_GB2[0] % pc.p_B2M_G);
472
473 // This is the velocity of M in F, but with the time derivative
474 // taken in the Ground frame.
475 const SpatialVec V_FM_G = vc.V_GM - vc.V_GF;
476
477 // To get derivative in F, we must remove the part due to the
478 // angular velocity w_GF of F in G.
479 vc.V_FM = ~R_GF * SpatialVec(V_FM_G[0],
480 V_FM_G[1] - vc.V_GF[0] % pc.p_FM_G);
481
482 // Need angular velocity in M frame for conversion to qdot.
483 const Vec3 w_FM_M = ~R_FM*vc.V_FM[0];
484 const Mat33 N_FM = Rotation::calcNForBodyXYZInBodyFrame(q);
485 vc.qdot.updSubVec<3>(0) = N_FM * w_FM_M;
486 vc.qdot.updSubVec<3>(3) = vc.V_FM[1];
487
488 markVelocityCacheValid(state);
489 }
490
491 // This will also calculate potential energy since we can do it on the
492 // cheap simultaneously with the force.
493 void Force::LinearBushingImpl::
ensureForceCacheValid(const State & state) const494 ensureForceCacheValid(const State& state) const {
495 if (isForceCacheValid(state)) return;
496
497 const InstanceVars& iv = getInstanceVars(state);
498 const Transform& X_B1F = iv.X_B1F;
499 const Transform& X_B2M = iv.X_B2M;
500 const Vec6& k = iv.k;
501 const Vec6& c = iv.c;
502
503 ForceCache& fc = updForceCache(state);
504
505 ensurePositionCacheValid(state);
506 const PositionCache& pc = getPositionCache(state);
507
508 const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
509 const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
510
511 const Transform& X_GB1 = body1.getBodyTransform(state);
512 const Rotation& R_GB1 = X_GB1.R();
513
514 const Transform& X_GB2 = body2.getBodyTransform(state);
515 const Rotation& R_GB2 = X_GB2.R();
516
517 const Vec3& p_B1F = X_B1F.p();
518 const Vec3& p_B2M = X_B2M.p();
519
520 const Rotation& R_GF = pc.X_GF.R();
521 const Vec3& p_GF = pc.X_GF.p();
522
523 const Rotation& R_GM = pc.X_GM.R();
524 const Vec3& p_GM = pc.X_GM.p();
525
526 const Rotation& R_FM = pc.X_FM.R();
527 const Vec3& p_FM = pc.X_FM.p();
528
529 // Calculate stiffness generalized forces and potential
530 // energy (cheap to do here).
531 const Vec6& q = pc.q;
532 Vec6 fk; Real pe2=0;
533 for (int i=0; i<6; ++i)
534 pe2 += (fk[i]=k[i]*q[i])*q[i];
535 updPotentialEnergyCache(state) = pe2/2;
536 markPotentialEnergyValid(state);
537
538 ensureVelocityCacheValid(state);
539 const VelocityCache& vc = getVelocityCache(state);
540
541 const Vec6& qd = vc.qdot;
542 Vec6 fv; fc.power=0;
543 for (int i=0; i<6; ++i)
544 fc.power += (fv[i]=c[i]*qd[i])*qd[i];
545
546 fc.f = -(fk+fv); // generalized forces on body 2
547 const Vec3& fB2_q = fc.f.getSubVec<3>(0); // in q basis
548 const Vec3& fM_F = fc.f.getSubVec<3>(3); // acts at M, but exp. in F frame
549
550 // Calculate the matrix relating q-space generalized forces to a real-space
551 // moment vector. We know qforce = ~H * moment (where H is the
552 // the hinge matrix for a mobilizer using qdots as generalized speeds).
553 // In that case H would be N^-1, qforce = ~(N^-1)*moment so
554 // moment = ~N*qforce. Caution: our N wants the moment in the outboard
555 // body frame, in this case M.
556 const Mat33 N_FM = Rotation::calcNForBodyXYZInBodyFrame(q.getSubVec<3>(0));
557 const Vec3 mB2_M = ~N_FM * fB2_q; // moment acting on body 2, exp. in M
558 const Vec3 mB2_G = R_GM * mB2_M; // moment on body 2, now exp. in G
559
560 // Transform force from F frame to ground. This is the force to
561 // apply to body 2 at point OM; -f goes on body 1 at the same
562 // spatial location. Here we actually apply it at OF so we have to
563 // account for the moment produced by the shift from OM.
564 const Vec3 fM_G = R_GF*fM_F;
565
566 fc.F_GM = SpatialVec( mB2_G, fM_G);
567 fc.F_GF = SpatialVec(-(mB2_G + pc.p_FM_G % fM_G) , -fM_G);
568
569 // Shift forces to body origins.
570 fc.F_GB2 = SpatialVec(fc.F_GM[0] + pc.p_B2M_G % fc.F_GM[1], fc.F_GM[1]);
571 fc.F_GB1 = SpatialVec(fc.F_GF[0] + pc.p_B1F_G % fc.F_GF[1], fc.F_GF[1]);
572
573 markForceCacheValid(state);
574 }
575
576 // This calculate is only performed if the PE is requested without
577 // already having calculated the force.
578 void Force::LinearBushingImpl::
ensurePotentialEnergyValid(const State & state) const579 ensurePotentialEnergyValid(const State& state) const {
580 if (isPotentialEnergyValid(state)) return;
581
582 const InstanceVars& iv = getInstanceVars(state);
583 const Vec6& k = iv.k;
584
585 ensurePositionCacheValid(state);
586 const PositionCache& pc = getPositionCache(state);
587 const Vec6& q = pc.q;
588
589 Real pe2=0;
590 for (int i=0; i<6; ++i)
591 pe2 += k[i]*q[i]*q[i];
592
593 updPotentialEnergyCache(state) = pe2 / 2;
594 markPotentialEnergyValid(state);
595 }
596
597 void Force::LinearBushingImpl::
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const598 calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
599 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
600 {
601 ensureForceCacheValid(state);
602 const ForceCache& fc = getForceCache(state);
603 bodyForces[body2x] += fc.F_GB2;
604 bodyForces[body1x] += fc.F_GB1; // apply forces
605 }
606
607 // If the force was calculated, then the potential energy will already
608 // be valid. Otherwise we'll have to calculate it.
609 Real Force::LinearBushingImpl::
calcPotentialEnergy(const State & state) const610 calcPotentialEnergy(const State& state) const {
611 ensurePotentialEnergyValid(state);
612 return getPotentialEnergyCache(state);
613 }
614
615
616 } // namespace SimTK
617
618