1 #include "Contact2dTest.hpp"
2
3 #include "SiconosContactor.hpp"
4 #include "SiconosShape.hpp"
5 #include "SiconosCollisionManager.hpp"
6 #include "SiconosBulletCollisionManager.hpp"
7 #include "RigidBody2dDS.hpp"
8 #include "SolverOptions.h"
9 #include "SiconosKernel.hpp"
10
11 #include <string>
12 #include <sys/time.h>
13
14 // Experimental settings for SiconosBulletCollisionManager
15 extern double extra_margin;
16 extern double breaking_threshold;
17 extern double box_ch_added_dimension;
18 extern double box_convex_hull_margin;
19 extern double bullet_world_scaling;
20
21 // Experimental statistics from SiconosBulletCollisionManager
22 extern int new_interaction_counter;
23 extern int existing_interaction_counter;
24 extern int interaction_warning_counter;
25
26 // test suite registration
27 CPPUNIT_TEST_SUITE_REGISTRATION(Contact2dTest);
28
setUp()29 void Contact2dTest::setUp() {}
tearDown()30 void Contact2dTest::tearDown() {}
31
32 struct BounceParams
33 {
34 bool trace;
35 bool dynamic;
36 double size;
37 double mass;
38 double position;
39 double timestep;
40 double insideMargin;
41 double outsideMargin;
42 SiconosBulletOptions options;
43
dumpBounceParams44 void dump() const
45 {
46 printf(" trace: %s\n", trace?"on":"off");
47 printf(" dynamic: %s\n", trace?"on":"off");
48 printf(" size: %.3g\n", size);
49 printf(" mass: %.3g\n", mass);
50 printf(" position: %.3g\n", position);
51 printf(" insideMargin: %.3g\n", insideMargin);
52 printf(" outsideMargin: %.3g\n", outsideMargin);
53 printf(" breakingThreshold: %.3g\n", options.contactBreakingThreshold);
54 printf(" worldScale: %.3g\n", options.worldScale);
55 }
56 };
57
58 struct BounceResult
59 {
60 double bounce_error_sum;
61 double bounce_error[6];
62 int n_bounce_error;
63 double final_position;
64 double final_position_std;
65 int num_interactions;
66 int num_interaction_warnings;
67 int max_simultaneous_contacts;
68 double avg_simultaneous_contacts;
69 double displacement_on_first_contact;
70 };
71
72 static
bounceTest(std::string moving,std::string ground,const BounceParams & params)73 BounceResult bounceTest(std::string moving,
74 std::string ground,
75 const BounceParams ¶ms)
76 {
77
78 params.dump();
79 // User-defined main parameters
80 double t0 = 0; // initial computation time
81 double T = 20.0; // end of computation time
82 double h = params.timestep; // time step
83 double position_init = params.position; // initial position
84 double velocity_init = 0.0; // initial velocity
85
86 double g = 9.81;
87 double theta = 0.5; // theta for MoreauJeanOSI integrator
88
89 int steps = (T-t0)/h;
90
91 // Statistics of this run
92 int bounce_counter = 0;
93 const int n_desired_bounces = 6;
94 double desired_bounce_ratios[6] = { 0.6358, 0.4048, 0.2577, 0.1630, 0.1033, 0.0647 };
95 double actual_bounce_times[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
96 double actual_bounce_ratios[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
97
98 int local_new_interaction_count=0;
99 int max_simultaneous_contacts=0;
100 double avg_simultaneous_contacts=0.0;
101
102 bool first_contact = true;
103 double displacement_on_first_contact=0.0;
104
105 // -- OneStepIntegrators --
106 SP::OneStepIntegrator osi;
107 osi.reset(new MoreauJeanOSI(theta));
108
109 // -- Model --
110 SP::NonSmoothDynamicalSystem nsds(new NonSmoothDynamicalSystem(t0, T));
111
112 SP::SiconosVector q0(new SiconosVector(3));
113 SP::SiconosVector v0(new SiconosVector(3));
114 q0->zero();
115 v0->zero();
116 (*q0)(1) = position_init;
117 (*v0)(1) = velocity_init;
118
119 SP::SiconosVector q1(new SiconosVector(3));
120 SP::SiconosVector v1(new SiconosVector(3));
121 q1->zero();
122 (*q1)(1) = 1.0;
123 v1->zero();
124
125 /////////
126
127 // Bodies
128
129 printf("== Testing: %s falling on %s .. ",
130 moving.c_str(), ground.c_str());
131 if(params.trace) printf("==\n");
132 fflush(stdout);
133
134 // Set up a Siconos Mechanics environment:
135 // A RigidBody2dDS with a contactor consisting of a single disk.
136
137 SP::SiconosMatrix mass(new SimpleMatrix(3,3));
138 mass->setValue(0,0,params.mass);
139 mass->setValue(1,1,params.mass);
140 mass->setValue(2,2,params.mass);
141 SP::RigidBody2dDS body(new RigidBody2dDS(q0, v0, mass));
142 SP::SiconosContactorSet contactors(new SiconosContactorSet());
143 SP::SiconosDisk disk;
144 if(moving=="disk")
145 {
146 disk.reset(new SiconosDisk(params.size));
147 disk->setInsideMargin(params.insideMargin);
148 disk->setOutsideMargin(params.outsideMargin);
149 contactors->push_back(std::make_shared<SiconosContactor>(disk));
150 }
151 else if(moving=="box")
152 {
153 SP::SiconosBox box(new SiconosBox(params.size,params.size,params.size));
154 box->setInsideMargin(params.insideMargin);
155 box->setOutsideMargin(params.outsideMargin);
156 contactors->push_back(std::make_shared<SiconosContactor>(box));
157 }
158 else if(moving=="ch2d")
159 {
160 float siz = params.size;
161 SP::SiconosMatrix pts(new SimpleMatrix(4,2));
162 (*pts)(0,0) = 0.0;
163 (*pts)(0,1) = 0.0;
164 (*pts)(1,1) = siz;
165 (*pts)(1,1) = 0.0;
166 (*pts)(2,0) = 0.0;
167 (*pts)(2,1) = siz;
168 SP::SiconosConvexHull2d ch2d(new SiconosConvexHull2d(pts));
169 ch2d->setInsideMargin(params.insideMargin);
170 ch2d->setOutsideMargin(params.outsideMargin);
171 contactors->push_back(std::make_shared<SiconosContactor>(ch2d));
172 }
173 body->setContactors(contactors);
174
175 // A contactor with no body (static contactor) consisting of a plane
176 // positioned such that bouncing and resting position = 0.0
177 SP::SiconosContactorSet static_contactors(std::make_shared<SiconosContactorSet>());
178
179 if(ground=="disk")
180 {
181 SP::SiconosDisk floordisk(new SiconosDisk(params.size));
182 floordisk->setInsideMargin(params.insideMargin);
183 floordisk->setOutsideMargin(params.outsideMargin);
184 SP::SiconosVector pos(new SiconosVector(7));
185 pos->zero();
186 (*pos)(1) = -2.0;//-params.size/2;
187
188 (*pos)(3) = 1.0; //unit quaternion
189 static_contactors->push_back(std::make_shared<SiconosContactor>(floordisk, pos));
190 }
191 else if(ground=="box")
192 {
193 SP::SiconosBox2d floorbox(new SiconosBox2d(100,1.));
194 floorbox->setInsideMargin(params.insideMargin);
195 floorbox->setOutsideMargin(params.outsideMargin);
196 SP::SiconosVector pos(new SiconosVector(7));
197 (*pos)(1) = -2.0;//-params.size/2;
198 (*pos)(3) = 1.0;
199 static_contactors->push_back(std::make_shared<SiconosContactor>(floorbox, pos));
200 }
201 else if(ground=="sphere")
202 {
203 SP::SiconosSphere floorsphere(new SiconosSphere(1.0));
204 floorsphere->setInsideMargin(params.insideMargin);
205 floorsphere->setOutsideMargin(params.outsideMargin);
206 SP::SiconosVector pos(new SiconosVector(7));
207 (*pos)(1) = -1.0 -params.size/2;
208 (*pos)(3) = 1.0;
209 static_contactors->push_back(std::make_shared<SiconosContactor>(floorsphere, pos));
210 }
211
212 /////////
213
214 // -- Set external forces (weight) --
215 SP::SiconosVector FExt;
216 FExt.reset(new SiconosVector(3));
217 FExt->zero();
218 FExt->setValue(1, - g * params.mass);
219 body->setFExtPtr(FExt);
220
221 // -- Add the dynamical systems into the non smooth dynamical system
222 nsds->insertDynamicalSystem(body);
223
224 // -- Time discretisation --
225 SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));
226
227 // -- OneStepNsProblem --
228 SP::FrictionContact osnspb(new FrictionContact(2));
229
230 // -- Some configuration
231 osnspb->numericsSolverOptions()->iparam[SICONOS_IPARAM_MAX_ITER] = 1000;
232 osnspb->numericsSolverOptions()->dparam[SICONOS_DPARAM_TOL] = 1e-5;
233
234 osnspb->setMaxSize(16384);
235 osnspb->setMStorageType(NM_SPARSE_BLOCK);
236 osnspb->setNumericsVerboseMode(0);
237 osnspb->setKeepLambdaAndYState(true);
238
239 /////////
240
241 // --- Simulation initialization ---
242
243 // -- MoreauJeanOSI Time Stepping
244 SP::TimeStepping simulation(new TimeStepping(nsds, timedisc));
245
246 simulation->insertIntegrator(osi);
247 simulation->insertNonSmoothProblem(osnspb);
248
249
250 // Object to manage the Bullet implementation of collisionMan
251 SP::SiconosBulletCollisionManager collisionMan(
252 new SiconosBulletCollisionManager(params.options));
253
254 simulation->insertInteractionManager(collisionMan);
255
256 // Add static shapes (centered at zero by default)
257 collisionMan->insertStaticContactorSet(static_contactors);
258
259 // Add a non-smooth law
260 SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 2));
261 collisionMan->insertNonSmoothLaw(nslaw, 0, 0);
262
263 ///////
264
265 int k=0;
266 double std=0, final_pos=0;
267 double last_pos=position_init, last_vel=0;
268 while(simulation->hasNextEvent() and k <= 1000)
269 {
270 // // Update a property at step 500
271 // if (params.dynamic && k==500 && moving=="disk") {
272 // disk->setRadius(0.3);
273 // }
274
275 // Update integrator and solve constraints
276 simulation->computeOneStep();
277 // osnspb->setNumericsVerboseMode(true);
278 // osnspb->setNumericsVerboseLevel(1);
279 // if (nsds->getNumberOfInteractions()>0)
280 // osnspb->display();
281 double vel = (*body->velocity())(1);
282 double pos = (*body->q())(1);
283
284 if(params.trace && (k+1) < steps)
285 {
286 printf("k, %i, pos, %f, %f, %f\n", k, pos, last_pos - pos, vel);
287 }
288
289 // Peaks (velocity crosses zero towards positive)
290 if(vel <= 0 && last_vel > 0)
291 {
292 if(bounce_counter < n_desired_bounces)
293 {
294 actual_bounce_times[bounce_counter] = k*h;
295 actual_bounce_ratios[bounce_counter] = pos / position_init;
296 bounce_counter ++;
297 }
298 }
299
300 // Interaction statistics
301 if(collisionMan->statistics().new_interactions_created > 0 && first_contact)
302 {
303 first_contact = false;
304 displacement_on_first_contact = last_pos - pos;
305 }
306
307 int interactions = collisionMan->statistics().new_interactions_created
308 + collisionMan->statistics().existing_interactions_processed;
309
310 local_new_interaction_count += collisionMan->statistics().new_interactions_created;
311
312 if(interactions > max_simultaneous_contacts)
313 max_simultaneous_contacts = interactions;
314
315 avg_simultaneous_contacts += interactions;
316
317 // Standard deviation (cheating by not calculating mean!)
318 if(k==(steps-100))
319 final_pos = pos;
320 if(k>(steps-100))
321 {
322 std += (pos-final_pos)*(pos-final_pos);
323 }
324
325 // State
326 last_pos = pos;
327 last_vel = vel;
328
329 // Advance simulation
330 simulation->nextStep();
331 k++;
332 }
333
334 if(!params.trace)
335 printf("(done, iterations=%d)\n", k - 1);
336
337 BounceResult r;
338 r.bounce_error_sum = 0.0;
339 r.n_bounce_error = 6;
340 for(int i=0; i < r.n_bounce_error; i++)
341 {
342 double er = actual_bounce_ratios[i] - desired_bounce_ratios[i];
343 r.bounce_error[i] = fabs(er);
344 r.bounce_error_sum += er*er;
345 }
346 r.bounce_error_sum = sqrt(r.bounce_error_sum/r.n_bounce_error);
347 r.final_position = (*body->q())(2);
348 r.final_position_std = sqrt(std/100);
349
350 r.num_interactions = local_new_interaction_count;
351 r.num_interaction_warnings = collisionMan->statistics().interaction_warnings;
352 r.max_simultaneous_contacts = max_simultaneous_contacts;
353 r.avg_simultaneous_contacts = avg_simultaneous_contacts / (double)k;
354
355 r.displacement_on_first_contact = displacement_on_first_contact;
356
357 return r;
358 }
359
360
t1()361 void Contact2dTest::t1()
362 {
363 try
364 {
365 printf("\n==== t1\n");
366
367 BounceParams params;
368 params.trace = true;
369 params.dynamic = false;
370 params.size = 1.0;
371 params.mass = 1.0;
372 params.position = 1.0;
373 params.timestep = 0.005;
374 params.insideMargin = 0.0;
375 params.outsideMargin = 0.0;
376 params.options.dimension = SICONOS_BULLET_2D;
377
378 BounceResult r = bounceTest("disk", "disk", params);
379
380 fprintf(stderr, "\nSize: %g\n", params.size);
381 fprintf(stderr, "Final position: %g (std=%g)\n\n",
382 r.final_position, r.final_position_std);
383 }
384 catch(...)
385 {
386 Siconos::exception::process();
387 CPPUNIT_ASSERT(0);
388 }
389
390 CPPUNIT_ASSERT(1);
391 }
t2()392 void Contact2dTest::t2()
393 {
394 try
395 {
396 printf("\n==== t2\n");
397
398 BounceParams params;
399 params.trace = true;
400 params.dynamic = false;
401 params.size = 1.0;
402 params.mass = 1.0;
403 params.position = 1.0;
404 params.timestep = 0.005;
405 params.insideMargin = 0.0;
406 params.outsideMargin = 0.0;
407 params.options.dimension = SICONOS_BULLET_2D;
408
409 BounceResult r = bounceTest("disk", "box", params);
410
411 fprintf(stderr, "\nSize: %g\n", params.size);
412 fprintf(stderr, "Final position: %g (std=%g)\n\n",
413 r.final_position, r.final_position_std);
414 }
415 catch(...)
416 {
417 Siconos::exception::process();
418 CPPUNIT_ASSERT(0);
419 }
420
421 CPPUNIT_ASSERT(1);
422 }
t3()423 void Contact2dTest::t3()
424 {
425 try
426 {
427 printf("\n==== t3\n");
428
429 BounceParams params;
430 params.trace = true;
431 params.dynamic = false;
432 params.size = 1.0;
433 params.mass = 1.0;
434 params.position = 1.0;
435 params.timestep = 0.005;
436 params.insideMargin = 0.0;
437 params.outsideMargin = 0.0;
438 params.options.dimension = SICONOS_BULLET_2D;
439
440 BounceResult r = bounceTest("ch2d", "box", params);
441
442 fprintf(stderr, "\nSize: %g\n", params.size);
443 fprintf(stderr, "Final position: %g (std=%g)\n\n",
444 r.final_position, r.final_position_std);
445 }
446 catch(...)
447 {
448 Siconos::exception::process();
449 CPPUNIT_ASSERT(0);
450 }
451
452 CPPUNIT_ASSERT(1);
453 }
454