1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) Example: Cable Path *
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) 2012 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 /* Simbody ExampleCablePath
25 This example shows how to use a CableTrackerSubsystem to follow the motion of
26 a cable that connects two bodies and passes around obstacles. We'll then
27 create a force element that generates spring forces that result from the
28 stretching and stretching rate of the cable. The custom force element defined
29 here duplicates the Simbody built-in CableSpring force element, and you can
30 choose to use either one by changing a #define. */
31
32 #include "Simbody.h"
33
34 #include <cassert>
35 #include <iostream>
36 using std::cout; using std::endl;
37
38 using namespace SimTK;
39
40 //#define USE_MY_CABLE_SPRING
41 #ifdef USE_MY_CABLE_SPRING
42 #define CABLE_SPRING MyCableSpring // defined below
43 #else
44 #define CABLE_SPRING CableSpring // Simbody built-in
45 #endif
46
47 #ifdef USE_MY_CABLE_SPRING
48 // This force element implements an elastic cable of a given nominal length,
49 // and a stiffness k that generates a k*x force opposing stretch beyond
50 // the slack length. There is also a dissipation term (k*x)*c*xdot. We keep
51 // track of dissipated power here so we can use conservation of energy to check
52 // that the cable and force element aren't obviously broken.
53 class MyCableSpringImpl : public Force::Custom::Implementation {
54 public:
MyCableSpringImpl(const GeneralForceSubsystem & forces,const CablePath & path,Real stiffness,Real nominal,Real damping)55 MyCableSpringImpl(const GeneralForceSubsystem& forces,
56 const CablePath& path,
57 Real stiffness, Real nominal, Real damping)
58 : forces(forces), path(path), k(stiffness), x0(nominal), c(damping)
59 { assert(stiffness >= 0 && nominal >= 0 && damping >= 0); }
60
getCablePath() const61 const CablePath& getCablePath() const {return path;}
62
63 // Must be at stage Velocity. Evalutes tension if necessary.
getTension(const State & state) const64 Real getTension(const State& state) const {
65 ensureTensionCalculated(state);
66 return Value<Real>::downcast(forces.getCacheEntry(state, tensionx));
67 }
68
69 // Must be at stage Velocity.
getPowerDissipation(const State & state) const70 Real getPowerDissipation(const State& state) const {
71 const Real stretch = calcStretch(state);
72 if (stretch == 0) return 0;
73 const Real rate = path.getCableLengthDot(state);
74 return k*stretch*std::max(c*rate, -1.)*rate;
75 }
76
77 // This integral is always available.
getDissipatedEnergy(const State & state) const78 Real getDissipatedEnergy(const State& state) const {
79 return forces.getZ(state)[workx];
80 }
81
82 //--------------------------------------------------------------------------
83 // Custom force virtuals
84
85 // Ask the cable to apply body forces given the tension calculated here.
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const86 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
87 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
88 override
89 { path.applyBodyForces(state, getTension(state), bodyForces); }
90
91 // Return the potential energy currently stored by the stretch of the cable.
calcPotentialEnergy(const State & state) const92 Real calcPotentialEnergy(const State& state) const override {
93 const Real stretch = calcStretch(state);
94 if (stretch == 0) return 0;
95 return k*square(stretch)/2;
96 }
97
98 // Allocate the state variable for tracking dissipated energy, and a
99 // cache entry to hold the calculated tension.
realizeTopology(State & state) const100 void realizeTopology(State& state) const override {
101 Vector initWork(1, 0.);
102 workx = forces.allocateZ(state, initWork);
103 tensionx = forces.allocateLazyCacheEntry(state, Stage::Velocity,
104 new Value<Real>(NaN));
105 }
106
107 // Report power dissipation as the derivative for the work variable.
realizeAcceleration(const State & state) const108 void realizeAcceleration(const State& state) const override {
109 Real& workDot = forces.updZDot(state)[workx];
110 workDot = getPowerDissipation(state);
111 }
112 //--------------------------------------------------------------------------
113
114 private:
115 // Return the amount by which the cable is stretched beyond its nominal
116 // length or zero if the cable is slack. Must be at stage Position.
calcStretch(const State & state) const117 Real calcStretch(const State& state) const {
118 const Real stretch = path.getCableLength(state) - x0;
119 return std::max(stretch, 0.);
120 }
121
122 // Must be at stage Velocity to calculate tension.
calcTension(const State & state) const123 Real calcTension(const State& state) const {
124 const Real stretch = calcStretch(state);
125 if (stretch == 0) return 0;
126 const Real rate = path.getCableLengthDot(state);
127 if (c*rate < -1)
128 cout << "c*rate=" << c*rate << "; limited to -1\n";
129 const Real tension = k*stretch*(1+std::max(c*rate,-1.));
130 return tension;
131 }
132
133 // If state is at stage Velocity, we can calculate and store tension
134 // in the cache if it hasn't already been calculated.
ensureTensionCalculated(const State & state) const135 void ensureTensionCalculated(const State& state) const {
136 if (forces.isCacheValueRealized(state, tensionx))
137 return;
138 Value<Real>::updDowncast(forces.updCacheEntry(state, tensionx))
139 = calcTension(state);
140 forces.markCacheValueRealized(state, tensionx);
141 }
142
143 const GeneralForceSubsystem& forces;
144 CablePath path;
145 Real k, x0, c;
146 mutable ZIndex workx;
147 mutable CacheEntryIndex tensionx;
148 };
149
150 // A nice handle to hide most of the cable spring implementation. This defines
151 // a user's API.
152 class MyCableSpring : public Force::Custom {
153 public:
MyCableSpring(GeneralForceSubsystem & forces,const CablePath & path,Real stiffness,Real nominal,Real damping)154 MyCableSpring(GeneralForceSubsystem& forces, const CablePath& path,
155 Real stiffness, Real nominal, Real damping)
156 : Force::Custom(forces, new MyCableSpringImpl(forces,path,
157 stiffness,nominal,damping))
158 {}
159
160 // Expose some useful methods.
getCablePath() const161 const CablePath& getCablePath() const
162 { return getImpl().getCablePath(); }
getTension(const State & state) const163 Real getTension(const State& state) const
164 { return getImpl().getTension(state); }
getPowerDissipation(const State & state) const165 Real getPowerDissipation(const State& state) const
166 { return getImpl().getPowerDissipation(state); }
getDissipatedEnergy(const State & state) const167 Real getDissipatedEnergy(const State& state) const
168 { return getImpl().getDissipatedEnergy(state); }
169
170 private:
getImpl() const171 const MyCableSpringImpl& getImpl() const
172 { return dynamic_cast<const MyCableSpringImpl&>(getImplementation()); }
173 };
174 #endif
175
176 // This gets called periodically to dump out interesting things about
177 // the cables and the system as a whole. It also saves states so that we
178 // can play back at the end.
179 static Array_<State> saveStates;
180 class ShowStuff : public PeriodicEventReporter {
181 public:
ShowStuff(const MultibodySystem & mbs,const CABLE_SPRING & cable1,const CABLE_SPRING & cable2,Real interval)182 ShowStuff(const MultibodySystem& mbs,
183 const CABLE_SPRING& cable1,
184 const CABLE_SPRING& cable2, Real interval)
185 : PeriodicEventReporter(interval),
186 mbs(mbs), cable1(cable1), cable2(cable2) {}
187
showHeading(std::ostream & o)188 static void showHeading(std::ostream& o) {
189 printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
190 "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
191 "KE", "PE", "KE+PE-W");
192 }
193
194 /** This is the implementation of the EventReporter virtual. **/
handleEvent(const State & state) const195 void handleEvent(const State& state) const override {
196 const CablePath& path1 = cable1.getCablePath();
197 const CablePath& path2 = cable2.getCablePath();
198 printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g CPU=%g\n",
199 state.getTime(),
200 path1.getCableLength(state),
201 path1.getCableLengthDot(state),
202 path1.getIntegratedCableLengthDot(state),
203 path1.calcCablePower(state, 1), // unit power
204 cable1.getTension(state),
205 cable1.getDissipatedEnergy(state),
206 cpuTime());
207 printf("%8s %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g\n",
208 "",
209 path2.getCableLength(state),
210 path2.getCableLengthDot(state),
211 path2.getIntegratedCableLengthDot(state),
212 path2.calcCablePower(state, 1), // unit power
213 cable2.getTension(state),
214 cable2.getDissipatedEnergy(state),
215 mbs.calcKineticEnergy(state),
216 mbs.calcPotentialEnergy(state),
217 mbs.calcEnergy(state)
218 + cable1.getDissipatedEnergy(state)
219 + cable2.getDissipatedEnergy(state));
220 saveStates.push_back(state);
221 }
222 private:
223 const MultibodySystem& mbs;
224 CABLE_SPRING cable1, cable2;
225 };
226
main()227 int main() {
228 try {
229 // Create the system.
230 MultibodySystem system;
231 SimbodyMatterSubsystem matter(system);
232 CableTrackerSubsystem cables(system);
233 GeneralForceSubsystem forces(system);
234
235 system.setUseUniformBackground(true); // no ground plane in display
236
237 Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
238 //Force::GlobalDamper(forces, matter, 5);
239
240 Body::Rigid someBody(MassProperties(1.0, Vec3(0), Inertia(1)));
241 const Real Rad = .25;
242
243 Body::Rigid biggerBody(MassProperties(1.0, Vec3(0), Inertia(1)));
244 const Real BiggerRad = .5;
245
246 const Vec3 radii(.4, .25, .15);
247 Body::Rigid ellipsoidBody(MassProperties(1.0, Vec3(0),
248 1.*UnitInertia::ellipsoid(radii)));
249
250 const Real CylRad = .3, HalfLen = .5;
251 Body::Rigid cylinderBody(MassProperties(1.0, Vec3(0),
252 1.*UnitInertia::cylinderAlongX(Rad,HalfLen)));
253
254 Body::Rigid fancyBody = biggerBody; // NOT USING ELLIPSOID
255
256 MobilizedBody Ground = matter.Ground();
257
258 MobilizedBody::Ball body1(Ground, Transform(Vec3(0)),
259 someBody, Transform(Vec3(0, 1, 0)));
260 MobilizedBody::Ball body2(body1, Transform(Vec3(0)),
261 someBody, Transform(Vec3(0, 1, 0)));
262 MobilizedBody::Ball body3(body2, Transform(Vec3(0)),
263 someBody, Transform(Vec3(0, 1, 0)));
264 MobilizedBody::Ball body4(body3, Transform(Vec3(0)),
265 fancyBody, Transform(Vec3(0, 1, 0)));
266 MobilizedBody::Ball body5(body4, Transform(Vec3(0)),
267 someBody, Transform(Vec3(0, 1, 0)));
268
269 CablePath path1(cables, body1, Vec3(Rad,0,0), // origin
270 body5, Vec3(0,0,Rad)); // termination
271
272 CableObstacle::ViaPoint p1(path1, body2, Rad*UnitVec3(1,1,0));
273 //CableObstacle::ViaPoint p2(path1, body3, Rad*UnitVec3(0,1,1));
274 //CableObstacle::ViaPoint p3(path1, body3, Rad*UnitVec3(1,0,1));
275 CableObstacle::Surface obs4(path1, body3, Transform(),
276 ContactGeometry::Sphere(Rad));
277 //obs4.setContactPointHints(Rad*UnitVec3(-1,1,0),Rad*UnitVec3(-1,0,1));
278 obs4.setContactPointHints(Rad*UnitVec3(-.25,.04,0.08),
279 Rad*UnitVec3(-.05,-.25,-.04));
280
281 //CableObstacle::ViaPoint p4(path1, body4, Rad*UnitVec3(0,1,1));
282 //CableObstacle::ViaPoint p5(path1, body4, Rad*UnitVec3(1,0,1));
283 CableObstacle::Surface obs5(path1, body4,
284 // Transform(), ContactGeometry::Ellipsoid(radii));
285 //Rotation(Pi/2, YAxis), ContactGeometry::Cylinder(CylRad)); // along y
286 //Transform(), ContactGeometry::Sphere(Rad));
287 Transform(), ContactGeometry::Sphere(BiggerRad));
288 //obs5.setContactPointHints(Rad*UnitVec3(0,-1,-1),Rad*UnitVec3(0.1,-1,-1));
289 obs5.setContactPointHints(Rad*UnitVec3(.1,.125,-.2),
290 Rad*UnitVec3(0.1,-.1,-.2));
291
292 CABLE_SPRING cable1(forces, path1, 100., 3.5, 0.1);
293
294 CablePath path2(cables, Ground, Vec3(-3,0,0), // origin
295 Ground, Vec3(-2,1,0)); // termination
296 CableObstacle::ViaPoint(path2, body3, 2*Rad*UnitVec3(1,1,1));
297 CableObstacle::ViaPoint(path2, Ground, Vec3(-2.5,1,0));
298 CABLE_SPRING cable2(forces, path2, 100., 2, 0.1);
299
300
301 //obs1.setPathPreferencePoint(Vec3(2,3,4));
302 //obs1.setDecorativeGeometry(DecorativeSphere(0.25).setOpacity(.5));
303
304 Visualizer viz(system);
305 viz.setShowFrameNumber(true);
306 system.addEventReporter(new Visualizer::Reporter(viz, 0.1*1./30));
307 system.addEventReporter(new ShowStuff(system, cable1, cable2, 0.1*0.1));
308 // Initialize the system and state.
309
310 system.realizeTopology();
311 State state = system.getDefaultState();
312 Random::Gaussian random;
313 for (int i = 0; i < state.getNQ(); ++i)
314 state.updQ()[i] = random.getValue();
315 for (int i = 0; i < state.getNU(); ++i)
316 state.updU()[i] = 0.1*random.getValue();
317
318 system.realize(state, Stage::Position);
319 viz.report(state);
320 cout << "path1 init length=" << path1.getCableLength(state) << endl;
321 cout << "path2 init length=" << path2.getCableLength(state) << endl;
322 cout << "Hit ENTER ...";
323 getchar();
324
325 path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
326 path2.setIntegratedCableLengthDot(state, path2.getCableLength(state));
327
328
329 // Simulate it.
330 saveStates.clear(); saveStates.reserve(2000);
331
332 RungeKuttaMersonIntegrator integ(system);
333 //CPodesIntegrator integ(system);
334 integ.setAccuracy(1e-3);
335 //integ.setAccuracy(1e-6);
336 TimeStepper ts(system, integ);
337 ts.initialize(state);
338 ShowStuff::showHeading(cout);
339
340 const Real finalTime = 2;
341 const double startTime = realTime();
342 ts.stepTo(finalTime);
343 cout << "DONE with " << finalTime
344 << "s simulated in " << realTime()-startTime
345 << "s elapsed.\n";
346
347
348 while (true) {
349 cout << "Hit ENTER FOR REPLAY, Q to quit ...";
350 const char ch = getchar();
351 if (ch=='q' || ch=='Q') break;
352 for (unsigned i=0; i < saveStates.size(); ++i)
353 viz.report(saveStates[i]);
354 }
355
356 } catch (const std::exception& e) {
357 cout << "EXCEPTION: " << e.what() << "\n";
358 }
359 }
360