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 /**@file
25 * Test the Force::LinearBushing force element.
26 */
27
28 #include "SimTKsimbody.h"
29 #include "SimTKcommon/Testing.h"
30
31 #include <cstdio>
32 #include <exception>
33 #include <iostream>
34 using std::cout; using std::endl;
35
36 //#define VISUALIZE
37 #ifdef VISUALIZE
38 #define WAIT_FOR_INPUT(str) \
39 do {printf(str); getchar();} while(false)
40 #define REPORT(state) \
41 do {viz.report(state);viz.zoomCameraToShowAllGeometry();} while (false)
42 #else
43 #define WAIT_FOR_INPUT(str)
44 #define REPORT(state)
45 #endif
46
47
48 using namespace SimTK;
49
testParameterSetting()50 void testParameterSetting() {
51 MultibodySystem system;
52 SimbodyMatterSubsystem matter(system);
53 GeneralForceSubsystem forces(system);
54
55 // This is the frame on body1.
56 const Transform X_B1F(Test::randRotation(), Test::randVec3());
57 // This is the frame on body 2.
58 const Transform X_B2M(Test::randRotation(), Test::randVec3());
59 // Material properties.
60 const Vec6 k = 10*Vec6(1,2,1,1,5,1);
61 const Vec6 c = 1.3*Vec6(1,.1,1,.11,1,11);
62
63 const Real Mass = 1.234;
64 Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
65 Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
66
67 MobilizedBody::Free body1(matter.Ground(), Transform(),
68 aBody, Transform());
69 MobilizedBody::Free body2(matter.Ground(), Transform(),
70 aBody, Transform());
71
72 Force::LinearBushing bushing
73 (forces, body1, Vec3(1,2,3),
74 body2, Vec3(-2,-3,-4),
75 Vec6(5), Vec6(7));
76 SimTK_TEST_EQ(bushing.getDefaultFrameOnBody1(),Transform(Vec3(1,2,3)));
77 SimTK_TEST_EQ(bushing.getDefaultFrameOnBody2(),Transform(Vec3(-2,-3,-4)));
78 SimTK_TEST_EQ(bushing.getDefaultStiffness(),Vec6(5,5,5,5,5,5));
79 SimTK_TEST_EQ(bushing.getDefaultDamping(),Vec6(7,7,7,7,7,7));
80
81 bushing.setDefaultFrameOnBody1(Vec3(-1,-2,-3))
82 .setDefaultFrameOnBody2(Vec3(2,3,4))
83 .setDefaultStiffness(Vec6(9))
84 .setDefaultDamping(Vec6(11));
85 SimTK_TEST_EQ(bushing.getDefaultFrameOnBody1(),Transform(Vec3(-1,-2,-3)));
86 SimTK_TEST_EQ(bushing.getDefaultFrameOnBody2(),Transform(Vec3(2,3,4)));
87 SimTK_TEST_EQ(bushing.getDefaultStiffness(),Vec6(9));
88 SimTK_TEST_EQ(bushing.getDefaultDamping(),Vec6(11));
89
90 system.realizeTopology();
91 State state = system.getDefaultState();
92 system.realizeModel(state);
93
94 // See if defaults transfer to default state properly.
95 SimTK_TEST_EQ(bushing.getFrameOnBody1(state),Transform(Vec3(-1,-2,-3)));
96 SimTK_TEST_EQ(bushing.getFrameOnBody2(state),Transform(Vec3(2,3,4)));
97 SimTK_TEST_EQ(bushing.getStiffness(state),Vec6(9));
98 SimTK_TEST_EQ(bushing.getDamping(state),Vec6(11));
99
100 bushing.setFrameOnBody1(state, X_B1F)
101 .setFrameOnBody2(state, X_B2M)
102 .setStiffness(state, k)
103 .setDamping(state, c);
104
105 SimTK_TEST_EQ(bushing.getFrameOnBody1(state),X_B1F);
106 SimTK_TEST_EQ(bushing.getFrameOnBody2(state),X_B2M);
107 SimTK_TEST_EQ(bushing.getStiffness(state),k);
108 SimTK_TEST_EQ(bushing.getDamping(state),c);
109
110
111 }
112
113 // Here we're going to build a chain like this:
114 //
115 // Ground --> body1 ==> body2
116 //
117 // where body1 is Free, and body2 is connected to body1 by
118 // a series of mobilizers designed to have the same kinematics
119 // as a LinearBushing element connected between them.
120 //
121 // We'll verify that the LinearBushing calculates kinematics
122 // that exactly matches the composite joint. Then we'll run
123 // for a while and see that energy+dissipation is conserved.
testKinematicsAndEnergyConservation()124 void testKinematicsAndEnergyConservation() {
125 // Build system.
126 MultibodySystem system;
127 SimbodyMatterSubsystem matter(system);
128 GeneralForceSubsystem forces(system);
129
130 // This is the frame on body1.
131 const Transform X_B1F(Test::randRotation(), Test::randVec3());
132 // This is the frame on body 2.
133 const Transform X_B2M(Test::randRotation(), Test::randVec3());
134 // Material properties.
135 const Vec6 k = 10*Vec6(1,1,1,1,1,1);
136 const Vec6 c = 1*Vec6(1,1,1,1,1,1);
137
138 // Use ugly mass properties to make sure we test all the terms.
139 const Real Mass = 1.234;
140 const Vec3 HalfShape = Vec3(1,.5,.25)/2;
141
142 Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
143 Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
144 aBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
145 .setOpacity(0.25)
146 .setColor(Blue));
147 aBody.addDecoration(X_B1F,
148 DecorativeFrame(0.5).setColor(Red));
149 aBody.addDecoration(X_B2M,
150 DecorativeFrame(0.5).setColor(Green));
151
152 // Ground attachement frame.
153 const Transform X_GA(
154 Test::randRotation(), Test::randVec3());
155
156 MobilizedBody::Free body1(matter.Ground(), X_GA,
157 aBody, X_B2M);
158
159 // This is to keep the system from flying away.
160 Force::TwoPointLinearSpring(forces,matter.Ground(),Vec3(0),body1,Vec3(0),
161 10,0);
162
163 Body::Rigid massless(MassProperties(0, Vec3(0), Inertia(0)));
164 const Rotation ZtoX(Pi/2, YAxis);
165 const Rotation ZtoY(-Pi/2, XAxis);
166 MobilizedBody::Cartesian dummy1(body1, X_B1F,
167 massless, Transform());
168 MobilizedBody::Pin dummy2(dummy1, ZtoX,
169 massless, ZtoX);
170 MobilizedBody::Pin dummy3(dummy2, ZtoY,
171 massless, ZtoY);
172 MobilizedBody::Pin body2 (dummy3, Transform(), // about Z
173 aBody, X_B2M);
174
175 // Set the actual parameters in the State below.
176 Force::LinearBushing bushing
177 (forces, body1, body2, Vec6(0), Vec6(0));
178
179 #ifdef VISUALIZE
180 Visualizer viz(system);
181 viz.setBackgroundType(Visualizer::SolidColor);
182 system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
183 #endif
184
185 // Initialize the system and state.
186
187 system.realizeTopology();
188 State state = system.getDefaultState();
189
190 bushing.setFrameOnBody1(state, X_B1F)
191 .setFrameOnBody2(state, X_B2M)
192 .setStiffness(state, k)
193 .setDamping(state, c);
194
195 REPORT(state);
196 WAIT_FOR_INPUT("\nDefault state -- hit ENTER\n");
197
198 state.updQ() = Test::randVector(state.getNQ());
199 state.updU() = Test::randVector(state.getNU());
200 state.updU()(3,3) = 0; // kill translational velocity
201
202 const Real Accuracy = 1e-6;
203 RungeKuttaMersonIntegrator integ(system);
204 integ.setAccuracy(Accuracy);
205 TimeStepper ts(system, integ);
206 ts.initialize(state);
207 State istate = integ.getState();
208 system.realize(istate, Stage::Velocity);
209 Vector xyz = istate.getQ()(7,3); Vector ang = istate.getQ()(10,3);
210 Vector xyzd = istate.getQDot()(7,3); Vector angd = istate.getQDot()(10,3);
211 Vec6 mq = Vec6(ang[0],ang[1],ang[2],xyz[0],xyz[1],xyz[2]);
212 Vec6 mqd = Vec6(angd[0],angd[1],angd[2],xyzd[0],xyzd[1],xyzd[2]);
213
214 SimTK_TEST_EQ(bushing.getQ(istate), mq);
215 SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
216
217 const Real initialEnergy = system.calcEnergy(istate);
218
219 SimTK_TEST( bushing.getDissipatedEnergy(istate) == 0 );
220
221 REPORT(integ.getState());
222
223 cout << "t=" << integ.getTime()
224 << "\nE=" << initialEnergy
225 << "\nmobilizer q=" << mq
226 << "\nbushing q=" << bushing.getQ(istate)
227 << "\nmobilizer qd=" << mqd
228 << "\nbushing qd=" << bushing.getQDot(istate)
229 << endl;
230 WAIT_FOR_INPUT("After initialize -- hit ENTER\n");
231
232 // Simulate it.
233 ts.stepTo(5.0);
234 istate = integ.getState();
235 system.realize(istate, Stage::Velocity);
236 xyz = istate.getQ()(7,3); ang = istate.getQ()(10,3);
237 xyzd = istate.getQDot()(7,3); angd = istate.getQDot()(10,3);
238 mq = Vec6(ang[0],ang[1],ang[2],xyz[0],xyz[1],xyz[2]);
239 mqd = Vec6(angd[0],angd[1],angd[2],xyzd[0],xyzd[1],xyzd[2]);
240
241 SimTK_TEST_EQ(bushing.getQ(istate), mq);
242 SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
243
244 // This should account for all the energy.
245 const Real finalEnergy = system.calcEnergy(istate)
246 + bushing.getDissipatedEnergy(istate);
247
248 SimTK_TEST_EQ_TOL(initialEnergy, finalEnergy, 10*Accuracy);
249
250 // Let's find everything and see if the bushing agrees.
251 const Transform& X_GB1 = body1.getBodyTransform(istate);
252 const Transform& X_GB2 = body2.getBodyTransform(istate);
253 const Transform X_GF = X_GB1*X_B1F;
254 const Transform X_GM = X_GB2*X_B2M;
255 SimTK_TEST_EQ(X_GF, bushing.getX_GF(istate));
256 SimTK_TEST_EQ(X_GM, bushing.getX_GM(istate));
257 SimTK_TEST_EQ(~X_GF*X_GM, bushing.getX_FM(istate));
258
259 // Calculate velocities and ask the bushing for same.
260 const SpatialVec& V_GB1 = body1.getBodyVelocity(istate);
261 const SpatialVec& V_GB2 = body2.getBodyVelocity(istate);
262 const SpatialVec
263 V_GF(V_GB1[0], body1.findStationVelocityInGround(istate,X_B1F.p()));
264 const SpatialVec
265 V_GM(V_GB2[0], body2.findStationVelocityInGround(istate,X_B2M.p()));
266
267 SimTK_TEST_EQ(V_GF, bushing.getV_GF(istate));
268 SimTK_TEST_EQ(V_GM, bushing.getV_GM(istate));
269
270 const SpatialVec
271 V_FM(~X_B1F.R()*body2.findBodyAngularVelocityInAnotherBody(istate,body1),
272 ~X_B1F.R()*body2.findStationVelocityInAnotherBody(istate,X_B2M.p(),body1));
273
274 SimTK_TEST_EQ(V_FM, bushing.getV_FM(istate));
275
276 cout << "t=" << integ.getTime()
277 << "\nE=" << system.calcEnergy(istate)
278 << "\nE-XW=" << finalEnergy << " final-init=" << finalEnergy-initialEnergy
279 << "\nmobilizer q=" << mq
280 << "\nbushing q=" << bushing.getQ(istate)
281 << "\nmobilizer qd=" << mqd
282 << "\nbushing qd=" << bushing.getQDot(istate)
283 << endl;
284
285 WAIT_FOR_INPUT("After simulation -- hit ENTER\n");
286 }
287
288
289 // Here we're going to build a chain like this:
290 //
291 // Ground --> body1 ==> body2
292 //
293 // where body1 is Free, and body2 is connected to body1 by
294 // a Bushing mobilizer, which should have the same kinematics
295 // as a LinearBushing element connected between them.
296 //
297 // We'll verify that the LinearBushing calculates kinematics
298 // that exactly matches the Bushing mobilizer. Then we'll run
299 // for a while and see that energy+dissipation is conserved.
testKinematicsAndEnergyConservationUsingBushingMobilizer()300 void testKinematicsAndEnergyConservationUsingBushingMobilizer() {
301 // Build system.
302 MultibodySystem system;
303 SimbodyMatterSubsystem matter(system);
304 GeneralForceSubsystem forces(system);
305
306 // This is the frame on body1.
307 const Transform X_B1F(Test::randRotation(), Test::randVec3());
308 // This is the frame on body 2.
309 const Transform X_B2M(Test::randRotation(), Test::randVec3());
310 // Material properties.
311 const Vec6 k = 10*Vec6(1,1,1,1,1,1);
312 const Vec6 c = 1*Vec6(1,1,1,1,1,1);
313
314 // Use ugly mass properties to make sure we test all the terms.
315 const Real Mass = 1.234;
316 const Vec3 HalfShape = Vec3(1,.5,.25)/2;
317
318 Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
319 Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
320 aBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
321 .setOpacity(0.25)
322 .setColor(Blue));
323 aBody.addDecoration(X_B1F,
324 DecorativeFrame(0.5).setColor(Red));
325 aBody.addDecoration(X_B2M,
326 DecorativeFrame(0.5).setColor(Green));
327
328 // Ground attachement frame.
329 const Transform X_GA(
330 Test::randRotation(), Test::randVec3());
331
332 MobilizedBody::Free body1(matter.Ground(), X_GA,
333 aBody, X_B2M);
334
335 // This is to keep the system from flying away.
336 Force::TwoPointLinearSpring(forces,matter.Ground(),Vec3(0),body1,Vec3(0),
337 10,0);
338
339 MobilizedBody::Bushing body2(body1, X_B1F,
340 aBody, X_B2M);
341
342 // Set the actual parameters in the State below.
343 Force::LinearBushing bushing
344 (forces, body1, body2, Vec6(0), Vec6(0));
345
346 #ifdef VISUALIZE
347 Visualizer viz(system);
348 viz.setBackgroundType(Visualizer::SolidColor);
349 system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
350 #endif
351
352 // Initialize the system and state.
353
354 system.realizeTopology();
355 State state = system.getDefaultState();
356
357 bushing.setFrameOnBody1(state, X_B1F)
358 .setFrameOnBody2(state, X_B2M)
359 .setStiffness(state, k)
360 .setDamping(state, c);
361
362 REPORT(state);
363 WAIT_FOR_INPUT("\nDefault state -- hit ENTER\n");
364
365 state.updQ() = Test::randVector(state.getNQ());
366 state.updU() = Test::randVector(state.getNU());
367 state.updU()(3,3) = 0; // kill translational velocity
368
369 const Real Accuracy = 1e-6;
370 RungeKuttaMersonIntegrator integ(system);
371 integ.setAccuracy(Accuracy);
372 TimeStepper ts(system, integ);
373 ts.initialize(state);
374 State istate = integ.getState();
375 system.realize(istate, Stage::Velocity);
376
377 // Get the q's and qdot's from the Bushing mobilizer.
378 Vec6 mq = body2.getQ(istate);
379 Vec6 mqd = body2.getQDot(istate);
380
381 SimTK_TEST_EQ(bushing.getQ(istate), mq);
382 SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
383
384 const Real initialEnergy = system.calcEnergy(istate);
385
386 SimTK_TEST( bushing.getDissipatedEnergy(istate) == 0 );
387
388 REPORT(integ.getState());
389
390 cout << "t=" << integ.getTime()
391 << "\nE=" << initialEnergy
392 << "\nmobilizer q=" << mq
393 << "\nbushing q=" << bushing.getQ(istate)
394 << "\nmobilizer qd=" << mqd
395 << "\nbushing qd=" << bushing.getQDot(istate)
396 << endl;
397 WAIT_FOR_INPUT("After initialize -- hit ENTER\n");
398
399 // Simulate it.
400 ts.stepTo(5.0);
401 istate = integ.getState();
402 system.realize(istate, Stage::Velocity);
403 mq = body2.getQ(istate);
404 mqd = body2.getQDot(istate);
405
406 SimTK_TEST_EQ(bushing.getQ(istate), mq);
407 SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
408
409 // This should account for all the energy.
410 const Real finalEnergy = system.calcEnergy(istate)
411 + bushing.getDissipatedEnergy(istate);
412
413 SimTK_TEST_EQ_TOL(initialEnergy, finalEnergy, 10*Accuracy);
414
415 // Let's find everything and see if the bushing agrees.
416 const Transform& X_GB1 = body1.getBodyTransform(istate);
417 const Transform& X_GB2 = body2.getBodyTransform(istate);
418 const Transform X_GF = X_GB1*X_B1F;
419 const Transform X_GM = X_GB2*X_B2M;
420 SimTK_TEST_EQ(X_GF, bushing.getX_GF(istate));
421 SimTK_TEST_EQ(X_GM, bushing.getX_GM(istate));
422 SimTK_TEST_EQ(~X_GF*X_GM, bushing.getX_FM(istate));
423
424 // Calculate velocities and ask the bushing for same.
425 const SpatialVec& V_GB1 = body1.getBodyVelocity(istate);
426 const SpatialVec& V_GB2 = body2.getBodyVelocity(istate);
427 const SpatialVec
428 V_GF(V_GB1[0], body1.findStationVelocityInGround(istate,X_B1F.p()));
429 const SpatialVec
430 V_GM(V_GB2[0], body2.findStationVelocityInGround(istate,X_B2M.p()));
431
432 SimTK_TEST_EQ(V_GF, bushing.getV_GF(istate));
433 SimTK_TEST_EQ(V_GM, bushing.getV_GM(istate));
434
435 const SpatialVec
436 V_FM(~X_B1F.R()*body2.findBodyAngularVelocityInAnotherBody(istate,body1),
437 ~X_B1F.R()*body2.findStationVelocityInAnotherBody(istate,X_B2M.p(),body1));
438
439 SimTK_TEST_EQ(V_FM, bushing.getV_FM(istate));
440
441 cout << "t=" << integ.getTime()
442 << "\nE=" << system.calcEnergy(istate)
443 << "\nE-XW=" << finalEnergy << " final-init=" << finalEnergy-initialEnergy
444 << "\nmobilizer q=" << mq
445 << "\nbushing q=" << bushing.getQ(istate)
446 << "\nmobilizer qd=" << mqd
447 << "\nbushing qd=" << bushing.getQDot(istate)
448 << endl;
449
450 WAIT_FOR_INPUT("After simulation -- hit ENTER\n");
451 }
452
453
454 // Here we're going to build a system containing two parallel multibody
455 // trees, each consisting of a single body connected to ground. For the
456 // first system the body is on a Free mobilizer and a LinearBushing
457 // connects the body and Ground. In the second, a series of mobilizers
458 // exactly mimics the kinematics, and a set of mobility springs and
459 // dampers are used to mimic the forces that should be produced by the
460 // bushing. We'll then evaluate in some arbitrary configuration and
461 // make sure the mobilizer reaction forces match the corresponding
462 // LinearBushing force.
testForces()463 void testForces() {
464 // Create the system.
465 MultibodySystem system;
466 SimbodyMatterSubsystem matter(system);
467 GeneralForceSubsystem forces(system);
468 Force::UniformGravity gravity(forces, matter, 0*Vec3(0, -9.8, 0));
469
470 const Real Mass = 1;
471 const Vec3 HalfShape = Vec3(1,.5,.25)/2;
472 const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
473 Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
474 Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
475 //Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
476 // Mass*UnitInertia::ellipsoid(HalfShape)));
477 brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
478 .setOpacity(0.25)
479 .setColor(Blue));
480 brickBody.addDecoration(BodyAttach,
481 DecorativeFrame(0.5).setColor(Red));
482
483 MobilizedBody::Free brick1(matter.Ground(), Transform(),
484 brickBody, BodyAttach);
485
486 Body::Rigid massless(MassProperties(0, Vec3(0), Inertia(0)));
487 const Rotation ZtoX(Pi/2, YAxis);
488 const Rotation ZtoY(-Pi/2, XAxis);
489
490 // This sequence is used to match the kinematics of a "forward" direction
491 // bushing where Ground is body 1.
492 //MobilizedBody::Cartesian dummy1(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
493 // massless, Transform());
494 //MobilizedBody::Pin dummy2(dummy1, ZtoX,
495 // massless, ZtoX);
496 //MobilizedBody::Pin dummy3(dummy2, ZtoY,
497 // massless, ZtoY);
498 //MobilizedBody::Pin brick2(dummy3, Transform(), // about Z
499 // brickBody, BodyAttach);
500
501 // This sequence is used to match the kinematics of a "reverse" direction
502 // bushing where Ground is body 2 (this is trickier).
503 MobilizedBody::Pin dummy1(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
504 massless, Transform(), MobilizedBody::Reverse);
505 MobilizedBody::Pin dummy2(dummy1, ZtoY,
506 massless, ZtoY, MobilizedBody::Reverse);
507 MobilizedBody::Pin dummy3(dummy2, ZtoX,
508 massless, ZtoX, MobilizedBody::Reverse);
509 MobilizedBody::Cartesian brick2(dummy3, Transform(),
510 brickBody, BodyAttach, MobilizedBody::Reverse);
511
512 const Vec6 k = Test::randVec<6>().abs(); // must be > 0
513 const Vec6 c = Test::randVec<6>().abs(); // "
514 Transform GroundAttach(Rotation(), Vec3(1,1,1));
515 matter.Ground().updBody().addDecoration(GroundAttach,
516 DecorativeFrame(0.5).setColor(Green));
517
518 const Vec6 k1 = k;
519 const Vec6 c1 = c;
520 // This is the Forward version
521 //Force::LinearBushing bushing
522 // (forces, matter.Ground(), GroundAttach,
523 // brick1, BodyAttach, k1, c1);
524
525 // This is the reverse version -- the moving body is the first body
526 // for the bushing; Ground is second. This is a harder test because
527 // the F frame is moving.
528 Force::LinearBushing bushing
529 (forces, brick1, BodyAttach,
530 matter.Ground(), GroundAttach,
531 k1, c1);
532
533 Force::LinearBushing bushing2
534 (forces, brick1, brick2, k, c);
535
536 Transform GroundAttach2(Rotation(), Vec3(1,1,1) + Vec3(2,0,0));
537 matter.Ground().updBody().addDecoration(GroundAttach2,
538 DecorativeFrame(0.5).setColor(Green));
539 const Vec6 k2 = k;
540 const Vec6 c2 = c;
541
542 // This is what you would do for a forward-direction set of
543 // mobilizers to match the forward bushing.
544 //Force::MobilityLinearSpring kqx(forces, dummy2, 0, k2[0], 0);
545 //Force::MobilityLinearDamper cqx(forces, dummy2, 0, c2[0]);
546 //Force::MobilityLinearSpring kqy(forces, dummy3, 0, k2[1], 0);
547 //Force::MobilityLinearDamper cqy(forces, dummy3, 0, c2[1]);
548 //Force::MobilityLinearSpring kqz(forces, brick2, 0, k2[2], 0);
549 //Force::MobilityLinearDamper cqz(forces, brick2, 0, c2[2]);
550 //Force::MobilityLinearSpring kpx(forces, dummy1, 0, k2[3], 0);
551 //Force::MobilityLinearDamper cpx(forces, dummy1, 0, c2[3]);
552 //Force::MobilityLinearSpring kpy(forces, dummy1, 1, k2[4], 0);
553 //Force::MobilityLinearDamper cpy(forces, dummy1, 1, c2[4]);
554 //Force::MobilityLinearSpring kpz(forces, dummy1, 2, k2[5], 0);
555 //Force::MobilityLinearDamper cpz(forces, dummy1, 2, c2[5]);
556
557 // This is the set of force elements that mimics the bushing hooked
558 // up in the reverse direction.
559 Force::MobilityLinearSpring kqx(forces, dummy3, 0, k2[0], 0);
560 Force::MobilityLinearDamper cqx(forces, dummy3, 0, c2[0]);
561 Force::MobilityLinearSpring kqy(forces, dummy2, 0, k2[1], 0);
562 Force::MobilityLinearDamper cqy(forces, dummy2, 0, c2[1]);
563 Force::MobilityLinearSpring kqz(forces, dummy1, 0, k2[2], 0);
564 Force::MobilityLinearDamper cqz(forces, dummy1, 0, c2[2]);
565 Force::MobilityLinearSpring kpx(forces, brick2, 0, k2[3], 0);
566 Force::MobilityLinearDamper cpx(forces, brick2, 0, c2[3]);
567 Force::MobilityLinearSpring kpy(forces, brick2, 1, k2[4], 0);
568 Force::MobilityLinearDamper cpy(forces, brick2, 1, c2[4]);
569 Force::MobilityLinearSpring kpz(forces, brick2, 2, k2[5], 0);
570 Force::MobilityLinearDamper cpz(forces, brick2, 2, c2[5]);
571
572 #ifdef VISUALIZE
573 Visualizer viz(system);
574 viz.setBackgroundType(Visualizer::SolidColor);
575 system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
576 #endif
577
578
579 // Initialize the system and state.
580
581 system.realizeTopology();
582 State state = system.getDefaultState();
583
584 REPORT(state);
585
586 Rotation RR = Test::randRotation();
587 brick1.setQToFitTransform(state, RR);
588
589 Vec3 qRRinv;
590 qRRinv = Rotation(~RR).convertRotationToBodyFixedXYZ();
591
592 brick2.setQToFitTransform(state, ~RR*Vec3(-1,-1,-1));
593 dummy3.setOneQ(state, 0, qRRinv[0]);
594 dummy2.setOneQ(state, 0, qRRinv[1]);
595 dummy1.setOneQ(state, 0, qRRinv[2]);
596
597
598 REPORT(state);
599 WAIT_FOR_INPUT("testForces() trial pose -- hit ENTER\n");
600
601 system.realize(state,Stage::Acceleration);
602
603 //cout << "\nf=" << bushing.getF(state) << endl;
604 //cout << "F_GM=" << bushing.getF_GM(state) << endl;
605 //cout << "F_GF=" << bushing.getF_GF(state) << endl;
606
607 Vector_<SpatialVec> reactions;
608 matter.calcMobilizerReactionForces(state, reactions);
609 //cout << "Reaction force on brick2="
610 // << reactions[brick2.getMobilizedBodyIndex()] << endl;
611
612 SimTK_TEST_EQ(bushing.getF_GF(state),
613 reactions[brick2.getMobilizedBodyIndex()]);
614 }
615
616 // This test is identical to testForces() except we use a reverse Bushing
617 // mobilizer rather than a sequence of Translation and Pins.
618 //
619 // Here we're going to build a system containing two parallel multibody
620 // trees, each consisting of a single body connected to ground. For the
621 // first system the body is on a Free mobilizer and a LinearBushing
622 // connects the body and Ground, although with Ground serving as "body2" for
623 // the bushing (i.e., the M frame is on Ground). In the second, a reversed
624 // Bushing mobilizer exactly mimics the kinematics, and a set of mobility
625 // springs and dampers are used to mimic the forces that should be produced by
626 // the bushing. We'll then evaluate in some arbitrary configuration and
627 // make sure the mobilizer reaction forces match the corresponding
628 // LinearBushing force.
testForcesUsingReverseBushingMobilizer()629 void testForcesUsingReverseBushingMobilizer() {
630 // Create the system.
631 MultibodySystem system;
632 SimbodyMatterSubsystem matter(system);
633 GeneralForceSubsystem forces(system);
634 Force::UniformGravity gravity(forces, matter, 0*Vec3(0, -9.8, 0));
635
636 matter.Ground().addBodyDecoration(Transform(),
637 DecorativeFrame(.25).setColor(Purple));
638
639 const Real Mass = 1;
640 const Vec3 HalfShape = Vec3(1,.5,.25)/2;
641 const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
642 Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
643 Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
644 brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
645 .setOpacity(0.25)
646 .setColor(Blue));
647 brickBody.addDecoration(BodyAttach,
648 DecorativeFrame(0.5).setColor(Red));
649
650 MobilizedBody::Free brick1(matter.Ground(), Transform(),
651 brickBody, BodyAttach);
652 brick1.addBodyDecoration(Transform(), DecorativeText("1").setScale(.25));
653
654 // This mobilizer is used to match the kinematics of a "reverse" direction
655 // bushing where Ground is body 2.
656 MobilizedBody::Bushing brick2(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
657 brickBody, BodyAttach, MobilizedBody::Reverse);
658 brick2.addBodyDecoration(Transform(), DecorativeText("2").setScale(.25));
659
660 const Vec6 k = Test::randVec<6>().abs(); // must be > 0
661 const Vec6 c = Test::randVec<6>().abs(); // "
662 Transform GroundAttach(Rotation(), Vec3(1,1,1));
663 matter.Ground().updBody().addDecoration(GroundAttach,
664 DecorativeFrame(0.5).setColor(Green));
665
666 const Vec6 k1 = k;
667 const Vec6 c1 = c;
668
669 // This is reversed -- the moving body is the first body
670 // for the bushing; Ground is second. This is a hard test because
671 // the F frame is moving.
672 Force::LinearBushing bushing
673 (forces, brick1, BodyAttach, // .5,0,0
674 matter.Ground(), GroundAttach, // 1,1,1
675 k1, c1);
676
677 Force::LinearBushing bushing2
678 (forces, brick1, brick2, k, c);
679
680 Transform GroundAttach2(Rotation(), Vec3(1,1,1) + Vec3(2,0,0));
681 matter.Ground().updBody().addDecoration(GroundAttach2,
682 DecorativeFrame(0.5).setColor(Green));
683 const Vec6 k2 = k;
684 const Vec6 c2 = c;
685
686 // This is the set of force elements that mimics the bushing hooked
687 // up in the reverse direction.
688 Force::MobilityLinearSpring kqx(forces, brick2, 0, k2[0], 0);
689 Force::MobilityLinearDamper cqx(forces, brick2, 0, c2[0]);
690 Force::MobilityLinearSpring kqy(forces, brick2, 1, k2[1], 0);
691 Force::MobilityLinearDamper cqy(forces, brick2, 1, c2[1]);
692 Force::MobilityLinearSpring kqz(forces, brick2, 2, k2[2], 0);
693 Force::MobilityLinearDamper cqz(forces, brick2, 2, c2[2]);
694 Force::MobilityLinearSpring kpx(forces, brick2, 3, k2[3], 0);
695 Force::MobilityLinearDamper cpx(forces, brick2, 3, c2[3]);
696 Force::MobilityLinearSpring kpy(forces, brick2, 4, k2[4], 0);
697 Force::MobilityLinearDamper cpy(forces, brick2, 4, c2[4]);
698 Force::MobilityLinearSpring kpz(forces, brick2, 5, k2[5], 0);
699 Force::MobilityLinearDamper cpz(forces, brick2, 5, c2[5]);
700
701 #ifdef VISUALIZE
702 Visualizer viz(system);
703 viz.setBackgroundType(Visualizer::SolidColor);
704 system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
705 #endif
706
707
708 // Initialize the system and state.
709
710 system.realizeTopology();
711 State state = system.getDefaultState();
712
713 REPORT(state);
714
715 Rotation RR = Test::randRotation();
716 brick1.setQToFitTransform(state, RR);
717 system.realize(state, Stage::Position);
718
719 cout << "\nbrick1 .q=" << brick1.getQ(state) << endl;
720 cout << "bushing.q=" << bushing.getQ(state) << endl;
721
722 brick2.setQToFitTransform(state, Transform(RR, -Vec3(1,1,1)));
723
724 cout << "brick2 .q=" << brick2.getQ(state) << endl;
725
726
727 REPORT(state);
728 WAIT_FOR_INPUT("testForcesUsingBushing() trial pose -- hit ENTER\n");
729
730 system.realize(state,Stage::Acceleration);
731
732 //cout << "\nf=" << bushing.getF(state) << endl;
733 //cout << "F_GM=" << bushing.getF_GM(state) << endl;
734 cout << "F_GF=" << bushing.getF_GF(state) << endl;
735
736 Vector_<SpatialVec> reactions;
737 matter.calcMobilizerReactionForces(state, reactions);
738 cout << "Reaction force on brick2="
739 << reactions[brick2.getMobilizedBodyIndex()] << endl;
740
741 SimTK_TEST_EQ(bushing.getF_GF(state),
742 reactions[brick2.getMobilizedBodyIndex()]);
743 }
744
main()745 int main() {
746 SimTK_START_TEST("TestLinearBushing");
747 SimTK_SUBTEST(testParameterSetting);
748 SimTK_SUBTEST(testKinematicsAndEnergyConservation);
749 SimTK_SUBTEST(testKinematicsAndEnergyConservationUsingBushingMobilizer);
750 SimTK_SUBTEST(testForces);
751 SimTK_SUBTEST(testForcesUsingReverseBushingMobilizer);
752 SimTK_END_TEST();
753 }
754
755