1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18 #include "MultipleImpact.hpp"
19 #include "SiconosAlgebraProd.hpp"
20 #include "LagrangianDS.hpp"
21 #include "MultipleImpactNSL.hpp"
22 #include "Simulation.hpp"
23 #include "ioMatrix.hpp"
24 #include <boost/numeric/ublas/io.hpp>
25 #include "OSNSMatrix.hpp"
26 #include "NonSmoothDynamicalSystem.hpp"
27
MultipleImpact(std::string newTypeLaw,double newDelP)28 MultipleImpact::MultipleImpact(std::string newTypeLaw, double newDelP): LinearOSNS(), _typeCompLaw(newTypeLaw), _deltaP(newDelP)
29 {
30 if((_typeCompLaw != "MonoStiffness") && (_typeCompLaw != "BiStiffness"))
31 THROW_EXCEPTION("MultipleImpact::_typeCompLaw type of the compliance model must be either MonoStiffness or BiStiffness!");
32 }
33
setTolImpact(double newTolZero)34 void MultipleImpact::setTolImpact(double newTolZero)
35 {
36 _tolImpact = newTolZero;
37 };
38
SetSaveData(bool var)39 void MultipleImpact::SetSaveData(bool var)
40 {
41 _saveData = var;
42 };
43
SetNameOutput(std::string file_name)44 void MultipleImpact::SetNameOutput(std::string file_name)
45 {
46 _namefile = file_name;
47 };
48
SetTolVel(double _var)49 void MultipleImpact::SetTolVel(double _var)
50 {
51 _Tol_Vel = _var;
52 };
53
SetTolEner(double _var)54 void MultipleImpact::SetTolEner(double _var)
55 {
56 _Tol_Ener = _var;
57 };
58
SetZeroVelEndImp(double _var)59 void MultipleImpact::SetZeroVelEndImp(double _var)
60 {
61 _ZeroVel_EndIm = _var;
62 };
63
SetZeroEnerEndImp(double _var)64 void MultipleImpact::SetZeroEnerEndImp(double _var)
65 {
66 _ZeroEner_EndIm = _var;
67 };
68
SetNstepSave(unsigned int var)69 void MultipleImpact::SetNstepSave(unsigned int var)
70 {
71 _nStepSave = var;
72 };
73
SetNstepMax(unsigned int var)74 void MultipleImpact::SetNstepMax(unsigned int var)
75 {
76 _nStepMax = var;
77 };
78
SetStepMinMaxSave(unsigned int var1,unsigned int var2)79 void MultipleImpact::SetStepMinMaxSave(unsigned int var1, unsigned int var2)
80 {
81 _stepMinSave = var1;
82 _stepMaxSave = var2;
83 }
84
set_typeCompLaw(std::string newTypeLaw)85 void MultipleImpact::set_typeCompLaw(std::string newTypeLaw)
86 {
87 _typeCompLaw = newTypeLaw;
88 if((_typeCompLaw != "MonoStiffness") && (_typeCompLaw != "BiStiffness"))
89 THROW_EXCEPTION("MultipleImpact::_typeCompLaw type of the compliance model must be either MonoStiffness or BiStiffness!");
90 };
91
SetSizeDataSave(unsigned int var)92 void MultipleImpact::SetSizeDataSave(unsigned int var)
93 {
94 _sizeDataSave = var;
95 }
96 //---------------------------------------------------------------------------------------------------
WriteVectorIntoMatrix(const SiconosVector & m,const unsigned int pos_row,const unsigned int pos_col)97 void MultipleImpact::WriteVectorIntoMatrix(const SiconosVector& m, const unsigned int pos_row, const unsigned int pos_col)
98 {
99 for(unsigned int i = 0; i < m.size(); ++i)
100 {
101 (*_DataMatrix)(pos_row, pos_col + i) = m(i);
102 }
103 }
104 //----------------------------------------------------------------------------------------------------
isZero(double Var)105 bool MultipleImpact::isZero(double Var)
106 {
107 if(std::abs(Var) <= _tolImpact)
108 return true;
109 else
110 return false;
111 }
112 //------------------------------------------------------------------------------------------------
isVelNegative(double Var)113 bool MultipleImpact::isVelNegative(double Var)
114 {
115 if(Var < - _Tol_Vel)
116 return true;
117 else
118 return false;
119 }
120 //-------------------------------------------------------------------------------------------------
121
isEnerZero(double Var)122 bool MultipleImpact::isEnerZero(double Var)
123 {
124 if(std::abs(Var) <= _Tol_Ener)
125 return true;
126 else
127 return false;
128 }
129 //--------------------------------------------------------------------------------------------------
EstimateNdataCols()130 unsigned int MultipleImpact::EstimateNdataCols()
131 {
132 unsigned int _numberCols = 1;
133 // Number of columns for data at contacts
134 SP::InteractionsGraph indexSet = simulation()->indexSet(0); // get indexSet[0]
135 InteractionsGraph::VIterator ui, uiend;
136 for(std::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
137 {
138 //_numberCols = _numberCols + 3*(indexSet->bundle(*ui)->interaction()->nonSmoothLaw()->size()) + 1;
139 _numberCols = _numberCols + (indexSet->bundle(*ui)->dimension());
140 }
141 // Number of columns for data at particles
142 SP::DynamicalSystemsGraph DSG = simulation()->nonSmoothDynamicalSystem()->dynamicalSystems();
143 DynamicalSystemsGraph::VIterator dsi, dsiend;
144 for(std::tie(dsi, dsiend) = DSG->vertices(); dsi != dsiend; ++dsi)
145 {
146 _numberCols = _numberCols + (DSG->bundle(*dsi)->dimension());
147 }
148 return(_numberCols);
149 }
150 //-----------------------------------------------------------------------------------------------
AllocateMemory()151 void MultipleImpact::AllocateMemory()
152 {
153 if(!_velocityContact)
154 _velocityContact.reset(new SiconosVector(maxSize()));
155 else
156 {
157 if(_velocityContact->size() != maxSize())
158 _velocityContact->resize(maxSize());
159 };
160 //
161 if(!_oldVelocityContact)
162 _oldVelocityContact.reset(new SiconosVector(maxSize()));
163 else
164 {
165 if(_oldVelocityContact->size() != maxSize())
166 _oldVelocityContact->resize(maxSize());
167 };
168 //
169 if(! _energyContact)
170 _energyContact.reset(new SiconosVector(maxSize()));
171 else
172 {
173 if(_energyContact->size() != maxSize())
174 _energyContact->resize(maxSize());
175 };
176 //
177 if(!_WorkcContact)
178 _WorkcContact.reset(new SiconosVector(maxSize()));
179 else
180 {
181 if(_WorkcContact->size() != maxSize())
182 _WorkcContact->resize(maxSize());
183 };
184 //
185 if(!_distributionVector)
186 _distributionVector.reset(new SiconosVector(maxSize()));
187 else
188 {
189 if(_distributionVector->size() != maxSize())
190 _distributionVector->resize(maxSize());
191 };
192 //
193 if(!_stateContact)
194 _stateContact.reset(new IndexInt(maxSize()));
195 else
196 {
197 if(_stateContact->size() != maxSize())
198 _stateContact->resize(maxSize());
199 };
200 //
201 if(!_Kcontact)
202 _Kcontact.reset(new SiconosVector(maxSize()));
203 else
204 {
205 if(_Kcontact->size() != maxSize())
206 _Kcontact->resize(maxSize());
207 };
208 //
209 if(!_restitutionContact)
210 _restitutionContact.reset(new SiconosVector(maxSize()));
211 else
212 {
213 if(_restitutionContact->size() != maxSize())
214 _restitutionContact->resize(maxSize());
215 };
216 //
217 if(!_elasticyCoefficientcontact)
218 _elasticyCoefficientcontact.reset(new SiconosVector(maxSize()));
219 else
220 {
221 if(_elasticyCoefficientcontact->size() != maxSize())
222 _elasticyCoefficientcontact->resize(maxSize());
223 };
224 if(!_tolImpulseContact)
225 _tolImpulseContact.reset(new SiconosVector(maxSize()));
226 else
227 {
228 if(_tolImpulseContact->size() != maxSize())
229 _tolImpulseContact->resize(maxSize());
230 };
231 //
232 if(!_deltaImpulseContact)
233 _deltaImpulseContact.reset(new SiconosVector(maxSize()));
234 else
235 {
236 if(_deltaImpulseContact->size() != maxSize())
237 _deltaImpulseContact->resize(maxSize());
238 };
239 //
240 if(!_impulseContactUpdate)
241 _impulseContactUpdate.reset(new SiconosVector(maxSize()));
242 else
243 {
244 if(_impulseContactUpdate->size() != maxSize())
245 _impulseContactUpdate->resize(maxSize());
246 }
247 //
248 if(!_forceContact)
249 _forceContact.reset(new SiconosVector(maxSize()));
250 else
251 {
252 if(_forceContact->size() != maxSize())
253 _forceContact->resize(maxSize());
254 };
255 // for the data matrix
256 unsigned int _numberCols = EstimateNdataCols();
257 if(!_DataMatrix)
258 _DataMatrix.reset(new SimpleMatrix(_sizeDataSave, _numberCols));
259 else
260 {
261 if((_DataMatrix->size(0) != _sizeDataSave) || (_DataMatrix->size(1) != _numberCols))
262 _DataMatrix->resize(_sizeDataSave, _numberCols);
263 }
264 }
265 //=====================================================================================
BuildParaContact()266 void MultipleImpact::BuildParaContact()
267 {
268 SP::InteractionsGraph indexSet = simulation()->indexSet(1); // get indexSet[1]
269 //Loop over the Interactionof the indexSet(1)
270 InteractionsGraph::VIterator ui, uiend;
271 for(std::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
272 {
273 SP::Interaction inter = indexSet->bundle(*ui);
274 SP::NonSmoothLaw nslaw = inter->nonSmoothLaw();
275 SP::MultipleImpactNSL Mulnslaw = std::dynamic_pointer_cast<MultipleImpactNSL>(nslaw);
276 assert(Mulnslaw && "In MultipleImpact::BuildStiffResCofVec, non-smooth law used must be MultipleImpactNSL!!!");
277 // Get the position of inter-interactionBlock in the vector _velocityContact
278 unsigned int pos = indexSet->properties(*ui).absolute_position;
279
280 (*_restitutionContact)(pos) = Mulnslaw->ResCof();
281 (*_Kcontact)(pos) = Mulnslaw->Stiff();
282 (*_elasticyCoefficientcontact)(pos) = Mulnslaw->ElasCof();
283 }
284 /*
285 std::cout << " Restitution coefficients: " <<std::endl;
286 _restitutionContact->display();
287 std::cout << "Stiffnesses: " <<std::endl;
288 _Kcontact->display();
289 std::cout << "Elasticity coeffients at contacts: " <<std::endl;
290 _elasticyCoefficientcontact->display();
291 */
292
293 }
294 //========================================================================================
PreComputeImpact()295 void MultipleImpact::PreComputeImpact()
296 {
297 //1. Get the number of contacts and bodies involved in the impact
298 if(indexSetLevel() != 1)
299 THROW_EXCEPTION("MultipleImpact::PreComputeImpact==> the levelMin must be equal to 1 in the multiple impact model !!");
300 InteractionsGraph& indexSet = *simulation()->indexSet(indexSetLevel()); // get indexSet[1]
301 _nContact = indexSet.size();
302 //2. Compute matrix _M
303 SP::Topology topology = simulation()->nonSmoothDynamicalSystem()->topology();
304 bool isLinear = simulation()->nonSmoothDynamicalSystem()->isLinear();
305 if(!_hasBeenUpdated || !isLinear)
306 {
307 // Computes new _unitaryBlocks if required
308 updateInteractionBlocks();
309 // Updates matrix M
310 _M->fillW(indexSet, !_hasBeenUpdated);
311 _sizeOutput = _M->size();
312 }
313 if(_nContact != _sizeOutput)
314 THROW_EXCEPTION("MultipleImpact::ComputeWMinvWtrans: number of contacts different from the size of output--> this case is not yet implemented!");
315 //3. Checks size of vectors
316 if(_velocityContact->size() != _sizeOutput)
317 {
318 _velocityContact->resize(_sizeOutput);
319 }
320 _velocityContact->zero();
321 //
322 if(_oldVelocityContact->size() != _sizeOutput)
323 {
324 _oldVelocityContact->resize(_sizeOutput);
325 }
326 _oldVelocityContact->zero();
327 //
328 if(_energyContact->size() != _sizeOutput)
329 {
330 _energyContact->resize(_sizeOutput);
331 }
332 _energyContact->zero();
333 //
334 if(_WorkcContact->size() != _sizeOutput)
335 {
336 _WorkcContact->resize(_sizeOutput);
337 }
338 _WorkcContact->zero();
339 //
340 if(_distributionVector->size() != _sizeOutput)
341 {
342 _distributionVector->resize(_sizeOutput);
343 }
344 _distributionVector->zero();
345 //
346 if(_stateContact->size() != _sizeOutput)
347 {
348 _stateContact->resize(_sizeOutput);
349 }
350 //
351 if(_Kcontact->size() != _sizeOutput)
352 {
353 _Kcontact->resize(_sizeOutput);
354 }
355 _Kcontact->zero();
356 //
357 if(_restitutionContact->size() != _sizeOutput)
358 {
359 _restitutionContact->resize(_sizeOutput);
360 }
361 _restitutionContact->zero();
362 //
363 if(_elasticyCoefficientcontact->size() != _sizeOutput)
364 {
365 _elasticyCoefficientcontact->resize(_sizeOutput);
366 }
367 _elasticyCoefficientcontact->zero();
368 //
369 if(_tolImpulseContact->size() != _sizeOutput)
370 {
371 _tolImpulseContact->resize(_sizeOutput);
372 }
373 _tolImpulseContact->zero();
374 //
375 if(_deltaImpulseContact->size() != _sizeOutput)
376 {
377 _deltaImpulseContact->resize(_sizeOutput);
378 }
379 _deltaImpulseContact->zero();
380 //
381 if(_impulseContactUpdate->size() != _sizeOutput)
382 {
383 _impulseContactUpdate->resize(_sizeOutput);
384 }
385 _impulseContactUpdate->zero();
386 //
387 if(_forceContact->size() != _sizeOutput)
388 {
389 _forceContact->resize(_sizeOutput);
390 }
391 _forceContact->zero();
392 //4. Initialize the relative velocity, potential energy, impulse at contacts
393 InitializeInput();
394 //5. Build the vectors of stifnesseses, of restitution coefficients, and of elaticity coefficients
395 BuildParaContact();
396 }
397 //=======================================================================================
InitializeInput()398 void MultipleImpact::InitializeInput()
399 {
400 //Loop over alls Interactioninvolved in the indexSet[1]
401 InteractionsGraph& indexSet = *simulation()->indexSet(indexSetLevel()); // get indexSet[1]
402 InteractionsGraph::VIterator ui, uiend;
403 for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui)
404 {
405 SP::Interaction inter = indexSet.bundle(*ui);
406 //SP::SiconosVector Vc0 = inter->y(1); // Relative velocity at beginning of impact
407 SP::SiconosVector Vc0 = inter->yOld(1); // Relative velocity at beginning of impact
408
409 unsigned int pos_inter = indexSet.properties(*ui).absolute_position;
410
411 setBlock(*Vc0, _velocityContact, Vc0->size(), 0, pos_inter);
412 SP::SiconosVector ener0(new SiconosVector(Vc0->size()));
413 ener0->zero(); // We suppose that the initial potential energy before impact is equal to zero at any contact
414 // at the beginning of impact
415 setBlock(*ener0, _energyContact, ener0->size(), 0, pos_inter);
416 //SP::SiconosVector impulse0= (inter)->lambda(1))->vector(inter->number());
417 SP::SiconosVector impulse0(new SiconosVector(Vc0->size()));
418 impulse0->zero(); // We suppose that the impulse before impact is equal to zero at any contact
419 // at the beginning of impact
420 setBlock(*impulse0, _tolImpulseContact, impulse0->size(), 0, pos_inter);
421 };
422 /*
423 std::cout << "Initial relative velocity at contacts" <<std::endl;
424 _velocityContact->display();
425 std::cout<< "Initial energy at contacts" <<std::endl;
426 _energyContact->display();
427 std::cout << "Impulse at contact" <<std::endl;
428 _tolImpulseContact->display();
429 */
430
431 }
432 //=========================================================================================
initialize(SP::Simulation sim)433 void MultipleImpact::initialize(SP::Simulation sim)
434 {
435
436 // General initialize for OneStepNSProblem
437 OneStepNSProblem::initialize(sim);
438 // Allocate the memory
439 AllocateMemory();
440 // get topology
441 SP::Topology topology = simulation()->nonSmoothDynamicalSystem()->topology();
442 // Note that _interactionBlocks is up to date since updateInteractionBlocks
443 // has been called during OneStepNSProblem::initialize()
444
445 if(! _M)
446 {
447 if(_numericsMatrixStorageType == NM_DENSE)
448 _M.reset(new OSNSMatrix(maxSize(), NM_DENSE));
449
450 else // if(_numericsMatrixStorageType == 1) size = number of _interactionBlocks
451 // = number of Interactionin the largest considered indexSet
452 _M.reset(new OSNSMatrix(simulation()->indexSet(indexSetLevel())->size(), NM_SPARSE_BLOCK));
453 }
454
455 };
456 //========================================================================================
PrimConVelocity()457 void MultipleImpact::PrimConVelocity()
458 {
459 getMin(*_velocityContact, _relativeVelocityPrimaryContact, _primaryContactId);
460 _energyPrimaryContact = (*_energyContact)(_primaryContactId);
461 if(!isVelNegative(_relativeVelocityPrimaryContact))
462 {
463 THROW_EXCEPTION("MultipleImpact::PrimConVelocity, the velocity at the primary contact must be negative !!");
464 }
465 /*
466 std::cout << "Primary contact according to relative velocity: " << _primaryContactId <<std::endl;
467 std::cout << "Relative velocity at the primary contact: " << _relativeVelocityPrimaryContact <<std::endl;
468 std::cout << "Potential energy at the primary contact: " << _energyPrimaryContact <<std::endl;
469 */
470 }
471 //=======================================================================================
PrimConEnergy()472 void MultipleImpact::PrimConEnergy()
473 {
474 getMax(*_energyContact, _energyPrimaryContact, _primaryContactId);
475 _relativeVelocityPrimaryContact = (*_velocityContact)(_primaryContactId);
476 if(_energyPrimaryContact < 0.0)
477 {
478 THROW_EXCEPTION("MultipleImpact::PrimConEnergy the potential energy at the primary contact must be positive !!");
479 }
480 /*
481 std::cout << "Primary contact according to potenial energy: " << _primaryContactId <<std::endl;
482 std::cout << "Relative velocity at the primary contact: " << _relativeVelocityPrimaryContact <<std::endl;
483 std::cout << "Potential energy at the primary contact: " << _energyPrimaryContact <<std::endl;
484 */
485
486 }
487 //======================================================================================
IsEnermaxZero()488 bool MultipleImpact::IsEnermaxZero()
489 {
490 double MaxEner;
491 unsigned int IdMax;
492 getMax(*_energyContact, MaxEner, IdMax);
493 if(isEnerZero(MaxEner))
494 return true;
495 else
496 return false;
497 }
498 //======================================================================================
IsVcminNegative()499 bool MultipleImpact::IsVcminNegative()
500 {
501 double MinVelCon;
502 unsigned int IdConVmin;
503 getMin(*_velocityContact, MinVelCon, IdConVmin);
504 if(isVelNegative(MinVelCon))
505 return true;
506 else
507 return false;
508 }
509 //=======================================================================================
Check_stateContact()510 void MultipleImpact::Check_stateContact()
511 {
512 for(unsigned int i = 0; i < _nContact; ++i)
513 {
514 if(isEnerZero((*_energyContact)(i))) // potential energy is zero
515 {
516 if(!isVelNegative((*_velocityContact)(i))) // relative velocity is positive or equal to zero
517 (*_stateContact)[i] = 0; // no impact at this contact
518 else // impact happens without potential energy
519 {
520 (*_stateContact)[i] = 1;
521 }
522 }
523 else // impact happens with not zero potential energy
524 {
525 if((*_stateContact)[i] != 2)
526 {
527 (*_stateContact)[i] = 2;
528 }
529 }
530 }
531 }
532 //=======================================================================================
IsMulImpactTerminate()533 bool MultipleImpact::IsMulImpactTerminate()
534 {
535 _IsImpactEnd = true;
536 for(unsigned int i = 0; i < _nContact; ++i)
537 {
538 if(((*_energyContact)(i) > _ZeroEner_EndIm) || ((*_velocityContact)(i) < -_ZeroVel_EndIm)) // if potential energy is not equal to zero or the relative velocity is negative
539 {
540 _IsImpactEnd = false;
541 }
542 }
543 return _IsImpactEnd;
544 // bool var = true;
545 // for(unsigned int i = 0; i < _nContact;++i)
546 // {
547 // if ((*_stateContact)[i] != 0)
548 // {
549 // var = false;
550 // break;
551 // };
552 // };
553 // return var;
554 //
555 //cout << "Is the multiple impacts is terminated: " << _IsImpactEnd <<std::endl;
556 //
557 }
558 //=======================================================================================
SelectPrimaContact()559 void MultipleImpact::SelectPrimaContact()
560 {
561 if(IsEnermaxZero()) // case of no potential energy at any contact
562 {
563 PrimConVelocity(); // Select the primary contact according to the relative velocity at contact
564 _isPrimaryContactEnergy = false;
565 }
566 else
567 {
568 PrimConEnergy(); // Select the primary contact according to the potential energy at contacts
569 _isPrimaryContactEnergy = true;
570 }
571 //
572 // std::cout << "The primary contact is :" << _primaryContactId <<std::endl;
573 // std::cout << "Is the primary contact is selected according to the potential energy: " << _isPrimaryContactEnergy <<std::endl;
574 }
575 //=======================================================================================
Compute_distributionVector()576 void MultipleImpact::Compute_distributionVector()
577 {
578 //Case 1: if no potential energy at any contact
579 double _ratio_mu, ratio_stiff, ratio_ener;
580 double mu_prima = (*_elasticyCoefficientcontact)(_primaryContactId); // Elasticity coefficient at the primary contact
581 double stiff_prima = (*_Kcontact)(_primaryContactId); // Stiffness at the primary contact
582 double _mu, _stiff, _vel, _energy;
583 if(!_isPrimaryContactEnergy) // case of primary contact selected according to the relative velocity
584 {
585 double ratio_vel;
586 for(unsigned int i = 0; i < _nContact; ++i)
587 {
588 if((*_stateContact)[i] != 0) // the impact can takes place at this contact
589 {
590 _mu = (*_elasticyCoefficientcontact)(i); // Elasticity coefficient at the current contact
591 _stiff = (*_Kcontact)(i); // Stiffness at the current contact
592 _vel = (*_velocityContact)(i); // Relative velocity at the current contact
593 _ratio_mu = (std::pow(_mu + 1.0, (_mu / (_mu + 1.0)))) / (std::pow(mu_prima + 1.0, (mu_prima / (mu_prima + 1.0))));
594 ratio_stiff = (std::pow(_stiff, (1.0 / (1.0 + _mu)))) / (std::pow(stiff_prima, (1.0 / (1.0 + mu_prima))));
595 if(!isVelNegative(_vel))
596 {
597 THROW_EXCEPTION("MultipleImpact::Compute_distributionVector, the relative velocity when particle starts to impact must be negative!!");
598 }
599
600 ratio_vel = (std::pow(std::fabs(_vel), (_mu / (_mu + 1.0)))) / (std::pow(std::fabs(_relativeVelocityPrimaryContact), (mu_prima / (1.0 + mu_prima))));
601 (*_distributionVector)(i) = std::pow((_ratio_mu * ratio_stiff * ratio_vel), (1.0 + _mu)) * std::pow(_deltaP, ((_mu - mu_prima) / (1.0 + mu_prima)));
602 }
603 else
604 {
605 (*_distributionVector)(i) = 0.0;
606 }
607 if((*_distributionVector)(i) < 0.0)
608 THROW_EXCEPTION("MultipleImpact::Compute_distributionVector the component of _distributionVector must be positive !!");
609 };
610 }
611 //Case 2: case of primary contact selected according to the potential energy
612 else
613 {
614 for(unsigned int i = 0; i < _nContact; ++i)
615 {
616 //
617 _mu = (*_elasticyCoefficientcontact)(i);
618 _stiff = (*_Kcontact)(i);
619 _ratio_mu = (std::pow(_mu + 1.0, (_mu / (_mu + 1.0)))) / (std::pow(mu_prima + 1.0, (mu_prima / (mu_prima + 1.0))));
620 ratio_stiff = (std::pow(_stiff, (1.0 / (1.0 + _mu)))) / (std::pow(stiff_prima, (1.0 / (1.0 + mu_prima))));
621 if((*_stateContact)[i] == 1) // no potential energy at this contact, including the contacts at which impact repeats
622 {
623 if(!isVelNegative((*_velocityContact)(i)))
624 {
625 THROW_EXCEPTION("MultipleImpact::Compute_distributionVector, the pre-impact velocity must be negative!!");
626 }
627 else
628 {
629 _vel = (*_velocityContact)(i);
630 ratio_ener = (std::pow(std::fabs(_vel * _deltaP), (_mu / (_mu + 1.0)))) / (std::pow(_energyPrimaryContact, (mu_prima / (mu_prima + 1.0))));
631 //
632 // std::cout << "_ratio_m: " << _ratio_mu <<std::endl;
633 // std::cout << "Stiff: " << _stiff <<std::endl;
634 // std::cout << "ratio_stiff: " << ratio_stiff <<std::endl;
635 // std::cout << "energy ratio: " << ratio_ener <<std::endl;
636
637 //
638 (*_distributionVector)(i) = std::pow((_ratio_mu * ratio_stiff * ratio_ener), (1.0 + _mu));
639 }
640 }
641 else if((*_stateContact)[i] == 2) // potential is not zero at this contact
642 {
643 _energy = (*_energyContact)(i); // Potential energy at the current contact
644 ratio_ener = (std::pow(_energy, (_mu / (_mu + 1.0)))) / (std::pow(_energyPrimaryContact, (mu_prima / (mu_prima + 1.0))));
645 (*_distributionVector)(i) = _ratio_mu * ratio_stiff * ratio_ener;
646 }
647 else // no impact at this contact
648 {
649 (*_distributionVector)(i) = 0.0;
650 };
651 if((*_distributionVector)(i) < 0.0)
652 THROW_EXCEPTION("MultipleImpact::Compute_distributionVector the component of _distributionVector must be positive !!");
653 };
654 };
655 }
656 //=======================================================================================
ComputeImpulseContact()657 void MultipleImpact::ComputeImpulseContact()
658 {
659 (*_deltaImpulseContact) = (*_distributionVector) * _deltaP;
660 (*_tolImpulseContact) = (*_tolImpulseContact) + (*_deltaImpulseContact);
661 (*_impulseContactUpdate) = (*_impulseContactUpdate) + (*_deltaImpulseContact);
662 // Compute the contact force
663 double PowCompLaw;
664 for(unsigned int i = 0; i < _nContact; ++i)
665 {
666 PowCompLaw = (*_elasticyCoefficientcontact)(i);
667 if(isEnerZero((*_energyContact)(i))) // if potential energy at this contact is zero
668 {
669 if(isVelNegative((*_velocityContact)(i))) // if the relative velocity at contact is negative
670 {
671 (*_forceContact)(i) = std::pow((1.0 + PowCompLaw), PowCompLaw / (1.0 + PowCompLaw)) * std::pow((*_Kcontact)(i), 1.0 / (1.0 + PowCompLaw)) * std::pow((std::fabs((*_velocityContact)(i)) * (*_deltaImpulseContact)(i)), PowCompLaw / (1.0 + PowCompLaw));
672 }
673 else
674 {
675 (*_forceContact)(i) = 0.0;
676 };
677 }
678 else
679 {
680 (*_forceContact)(i) = std::pow((1.0 + PowCompLaw), PowCompLaw / (1.0 + PowCompLaw)) * std::pow((*_Kcontact)(i), 1.0 / (1.0 + PowCompLaw)) * std::pow((*_energyContact)(i), PowCompLaw / (1.0 + PowCompLaw));
681 }
682 if((*_forceContact)(i) < 0.0)
683 {
684 THROW_EXCEPTION("MultipleImpact::ComputeImpulseContact, the contact force must be positive or equal to zero!!!");
685 }
686 };
687 }
688 //=======================================================================================
Compute_velocityContact()689 void MultipleImpact::Compute_velocityContact()
690 {
691 (*_oldVelocityContact) = (*_velocityContact); //save the relative velocity at the beginning of the step
692 (*_velocityContact) = (*_velocityContact) + prod(*(_M->defaultMatrix()), *_deltaImpulseContact); // compute the relative velocity at the end of the step
693 //
694 /*
695 std::cout << "Relative velocity at contacts at the beginning of step:" <<std::endl;
696 _oldVelocityContact->display();
697 std::cout << "Relative velocity at contacts at the end of step:" <<std::endl;
698 _velocityContact->display();
699 */
700 //
701 }
702 //=======================================================================================
Compute_energyContact()703 void MultipleImpact::Compute_energyContact()
704 {
705 if(_typeCompLaw == "BiStiffness")
706 // For Bistiffness model
707 {
708 for(unsigned int i = 0; i < _nContact; ++i)
709 {
710 if((0.5 * ((*_oldVelocityContact)(i) + (*_velocityContact)(i))) <= 0.0) // Contact located in the compression phase
711 {
712 (*_energyContact)(i) = (*_energyContact)(i) - 0.5 * ((*_oldVelocityContact)(i) + (*_velocityContact)(i)) * ((*_deltaImpulseContact)(i));
713 }
714 else // Contact located in the expansion phase
715 {
716 if(!isZero((*_restitutionContact)(i)))
717 {
718 (*_energyContact)(i) = (*_energyContact)(i) - (1.0 / std::pow((*_restitutionContact)(i), 2)) * 0.5 * ((*_oldVelocityContact)(i) + (*_velocityContact)(i)) * ((*_deltaImpulseContact)(i));
719 //
720 if((*_energyContact)(i) < 0.0)
721 {
722 (*_energyContact)(i) = 0.0;
723 };
724 }
725 else // restitution coefficient equal to zero
726 {
727 (*_energyContact)(i) = 0.0; // In this case, no potential energy at contacts when the contact is located in the compression phase
728 }
729 };
730 //
731 if((*_energyContact)(i) < 0.0)
732 {
733 THROW_EXCEPTION("MultipleImpact::Compute_energyContact, the potential energy during compression phase must be positive!!!");
734 };
735 };
736 }
737 else
738 // For the mono-stiffness model
739 {
740 for(unsigned int i = 0; i < _nContact; ++i)
741 {
742 //1: dertermine the work done by the last compression phase at contacts (nessessary for the mono-stiffness compliance model)
743 // if Vc(k) < 0 and Vc(k +1) >= 0 ==> transition from the compression phase to the expansion phase, Wc = E(k)
744 if(((*_oldVelocityContact)(i) < 0.0) && ((*_velocityContact)(i) >= 0.0))
745 {
746 (*_WorkcContact)(i) = (*_energyContact)(i);
747 };
748 //2: Calculate the potential energy at the end of stap
749 (*_energyContact)(i) = (*_energyContact)(i) - 0.5 * ((*_oldVelocityContact)(i) + (*_velocityContact)(i)) * ((*_deltaImpulseContact)(i));
750 //3: Check if the termination condition is verified or not (if Vc(k+1) > 0.0 and E(k+1) <= (1-e^2)*Wc). If yes, discard the potential energy
751 // in order to respect the energetic constraint
752 if(((*_stateContact)[i] == 2) && (((*_velocityContact)(i) > 0.0) && ((*_energyContact)(i) <= ((1.0 - std::pow((*_restitutionContact)(i), 2)) * (*_WorkcContact)(i)))))
753 {
754 (*_energyContact)(i) = 0.0; // potential energy at this contact is completely dissipated before the compression phase finishes
755 };
756 };
757 }
758 /*
759
760 std::cout << "Potential energy at contacts at the end of step:" <<std::endl;
761 _energyContact->display();
762 std::cout << "Work done during the compression phase at contacts" <<std::endl;
763 _WorkcContact->display();
764
765 */
766 }
767 //======================================================================================
UpdateDuringImpact()768 void MultipleImpact::UpdateDuringImpact()
769 {
770 //1. Copy _velocityContact/_deltaImpulseContact into the vector y/lambda for Interactions
771 InteractionsGraph& indexSet = *simulation()->indexSet(indexSetLevel());
772 // y and lambda vectors
773 SP::SiconosVector lambda;
774 SP::SiconosVector y;
775 // === Loop through "active" Interactions (ie present in indexSets[1]) ===
776 unsigned int pos;
777 InteractionsGraph::VIterator ui, uiend;
778 for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui)
779 {
780 Interaction& inter = *indexSet.bundle(*ui);
781 // Get the relative position of inter-interactionBlock in the vector _velocityContact/_tolImpulseContact
782 pos = indexSet.properties(*ui).absolute_position;
783 // Get Y and Lambda for the current Interaction
784 y = inter.y(inputOutputLevel());
785 lambda = inter.lambda(inputOutputLevel());
786 // Copy _velocityContact/_tolImpulseContact, starting from index pos into y/lambda
787 // save into y !!
788 setBlock(*_velocityContact, y, y->size(), pos, 0);
789 // saved into lambda[1] !!
790 setBlock(*_impulseContactUpdate, lambda, lambda->size(), pos, 0);
791 //setBlock(*_deltaImpulseContact, lambda, lambda->size(), pos, 0);
792 };
793 //2. Update the Input[1], state of DS systems, Output[1]
794 simulation()->update(inputOutputLevel());
795 _impulseContactUpdate->zero(); // reset input[1] to zero after each update
796 }
797 //--------------------------------------------------------------------------------------
SaveDataOneStep(unsigned int _ithPoint)798 void MultipleImpact::SaveDataOneStep(unsigned int _ithPoint)
799 {
800 // Save the total impulse at the primary contacts (time-like independent variable) and the time evolution during impact
801 if(_ithPoint >= _DataMatrix->size(0))
802 THROW_EXCEPTION("In MultipleImpact::ComputeImpact, number of points saved exceeds the size of matrix allocated!!!");
803 //(*_DataMatrix)(_ithPoint,0) = _timeVariable;
804 (*_DataMatrix)(_ithPoint, 0) = _impulseVariable;
805 // Save the data related to UnitaryRelations
806 SP::InteractionsGraph indexSet0 = simulation()->indexSet(0);
807 SP::InteractionsGraph indexSet1 = simulation()->indexSet(indexSetLevel());
808 unsigned int pos;
809 InteractionsGraph::VIterator ui, uiend;
810 unsigned int col_pos = 1;
811 for(std::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
812 {
813 SP::Interaction inter = indexSet0->bundle(*ui);
814 SP::SiconosVector ydot = inter->y(1);
815 SP::SiconosVector P_inter(new SiconosVector(inter->dimension()));
816 SP::SiconosVector F_inter(new SiconosVector(inter->dimension()));
817 SP::SiconosVector E_inter(new SiconosVector(1));
818 if(indexSet1->is_vertex(inter)) // if Interaction belongs to the IndexSet[1]
819 {
820 pos = indexSet0->properties(*ui).absolute_position;
821 setBlock(*_tolImpulseContact, P_inter, P_inter->size(), pos, 0);
822 setBlock(*_forceContact, F_inter, F_inter->size(), pos, 0);
823 setBlock(*_energyContact, E_inter, E_inter->size(), pos, 0);
824 }
825 else
826 {
827 P_inter->zero(); // no impulse at this Interaction
828 F_inter->zero(); // no force at this Interaction
829 E_inter->zero(); // no potential at this Interaction
830 };
831 // Write the force at the Interaction
832 WriteVectorIntoMatrix(*F_inter, _ithPoint, col_pos);
833 //WriteVectorIntoMatrix(*P_inter, _ithPoint, col_pos);
834 //WriteVectorIntoMatrix(*E_inter, _ithPoint, col_pos);
835 col_pos = col_pos + F_inter->size();
836 } // Save the data related to DS
837 SP::DynamicalSystemsGraph DSG = simulation()->nonSmoothDynamicalSystem()->dynamicalSystems();
838 DynamicalSystemsGraph::VIterator dsi, dsiend;
839 for(std::tie(dsi, dsiend) = DSG->vertices(); dsi != dsiend; ++dsi)
840 {
841 SP::DynamicalSystem ds = DSG->bundle(*dsi); // DS
842 SP::LagrangianDS Lagds = std::dynamic_pointer_cast<LagrangianDS>(ds);
843 SP::SiconosVector qdot = Lagds->velocity();
844 // Write
845
846 WriteVectorIntoMatrix(*qdot, _ithPoint, col_pos);
847 col_pos = col_pos + qdot->size();
848 }
849 }
850 //=======================================================================================
ComputeImpact()851 void MultipleImpact::ComputeImpact()
852 {
853 _impulseVariable = 0.0;
854 _timeVariable = 0.0;
855 unsigned int number_step = 1;
856 unsigned int point_save = 0;
857 unsigned int _counterstepsave = 0;
858 // Show computation progress
859 //cout << "*********** Impact computation progress *************" <<std::endl;
860 //boost::progress_display show_progress(_nStepMax);
861 /*
862 std::cout << "----------Before multiple impacts computation---------------" <<std::endl;
863 std::cout << "Velocity at contacts: ";
864 _velocityContact->display();
865 std::cout << "Impulse at contact: ";
866 _tolImpulseContact->display();
867 */
868 //cout << "-------------------Multiple impacts computation starts:-----------------------" <<std::endl;
869 // First save at the beginning of impact computation
870 if((_saveData) && (_stepMinSave == 1))
871 {
872 SaveDataOneStep(point_save); // Save the data
873 point_save++;
874 }
875 //
876 while(1 != 0)
877 {
878 // std::cout << "==================Step==================: " << number_step <<std::endl;
879 // std::cout << "Impulse variable: " << _impulseVariable <<std::endl;
880 // std::cout << "_timeVariable: " << _timeVariable <<std::endl;
881
882 //Step 1: check the state at contacts
883 Check_stateContact();
884 //Step 2: check if the multiple impact is terminated or not
885 if(IsMulImpactTerminate()) // multiple impact terminated
886 {
887 if(_saveData) // Save the date at the end of impact
888 {
889 UpdateDuringImpact(); // Update state of dynamical system
890 SaveDataOneStep(point_save); // Save the data
891 }
892 break;
893 }
894 // Select the primary contact
895 SelectPrimaContact();
896 //Step 3: compute the vector of distributing law
897 Compute_distributionVector();
898 //Step 4: compute the increment of normal impulse and the total impulse at contacts
899 ComputeImpulseContact();
900 // Step 5: compute the relative velocity at contacts
901 Compute_velocityContact();
902 // Step 6: compute the potential energy at contacts
903 Compute_energyContact();
904 //Step 7: Update the time-like variable
905 ++number_step;
906 ++_counterstepsave;
907 //++show_progress;
908 _impulseVariable = _impulseVariable + _deltaP;
909 _timeVariable = _timeVariable + _deltaP / (*_forceContact)(_primaryContactId);
910 // Step 8: update the state of DS and output during impact and write data into output file at the beginning of each step
911 if((_saveData) & (_counterstepsave >= _nStepSave))
912 {
913 if((number_step >= _stepMinSave) && (number_step <= _stepMaxSave))
914 {
915 UpdateDuringImpact(); // Update state of dynamical system
916 SaveDataOneStep(point_save); // Save the data
917 point_save++;
918 _counterstepsave = 0; // reset the counter to 0
919 }
920 }
921 //
922 if(number_step > _nStepMax)
923 {
924 THROW_EXCEPTION("In MultipleImpact::ComputeImpact, number of integration steps performed exceeds the maximal number of steps allowed!!!");
925 //cout << "Causion: so long computation, the computation is stopped even when the impact is not yet terminated!!! " <<std::endl;
926 break;
927 }
928 // std::cout << "Distribution vector: ";
929 // _distributionVector->display();
930 // std::cout << "Incremental Impulse: ";
931 // _deltaImpulseContact->display();
932 // std::cout << "Impulse at contact: ";
933 // _tolImpulseContact->display();
934 // std::cout << "Velocity at contacts: ";
935 // _velocityContact->display();
936 // std::cout << "Potential energy at contacts: ";
937 // _energyContact->display();
938
939 }
940
941 //
942 // std::cout << "*****************Impact computation is terminated******************" <<std::endl;
943 // std::cout << "Number of integration steps: " << number_step <<std::endl;
944 // std::cout << "Velocity at contacts: ";
945 // _velocityContact->display();
946 // std::cout << "Impulse at contact: ";
947 // _tolImpulseContact->display();
948 // std::cout << "Duration of the multiple impacts process: " << _timeVariable << " s" <<std::endl;
949
950 // Close the stream file
951 if(_saveData)
952 {
953 ioMatrix::write(_namefile.c_str(), "ascii", *_DataMatrix, "noDim");
954 }
955 }
956 //=======================================================================================
PostComputeImpact()957 void MultipleImpact::PostComputeImpact()
958 {
959 // === Get index set from Topology ===
960 InteractionsGraph& indexSet = *simulation()->indexSet(indexSetLevel());
961 // y and lambda vectors
962 SP::SiconosVector lambda;
963 SP::SiconosVector y;
964 // === Loop through "active" Interactions (ie present in indexSets[1]) ===
965 unsigned int pos;
966 InteractionsGraph::VIterator ui, uiend;
967 for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui)
968 {
969 Interaction& inter = *indexSet.bundle(*ui);
970 // Get the relative position of inter-interactionBlock in the vector _velocityContact/_tolImpulseContact
971 pos = indexSet.properties(*ui).absolute_position;
972 // Get Y and Lambda for the current Interaction
973 y = inter.y(inputOutputLevel());
974 lambda = inter.lambda(inputOutputLevel());
975 // Copy _velocityContact/_tolImpulseContact, starting from index pos into y/lambda
976 // save into y !!
977 setBlock(*_velocityContact, y, y->size(), pos, 0);// Warning: yEquivalent is
978 // saved into lambda[1] !!
979 setBlock(*_impulseContactUpdate, lambda, lambda->size(), pos, 0);
980 // If the update is performed at the end of the impact process, we update the total normal impulse at contacts
981 // from the beginning to the end of impact (vector _tolImpulseContact). Otherwise, we must reset the lambda[1] to zero because
982 // the post-impact velocity has been calculated during impact
983 // if (!_saveData) // we update the impact state at the end of impact
984 // {
985 // // Copy _velocityContact/_tolImpulseContact, starting from index pos into y/lambda
986 // // save into y !!
987 // setBlock(*_velocityContact, y, y->size(), pos, 0);// Warning: yEquivalent is
988 // // saved into lambda[1] !!
989 // setBlock(*_tolImpulseContact, lambda, lambda->size(), pos, 0);
990 // }
991 // else //
992 // lambda->zero();
993 }
994 }
995
checkCompatibleNSLaw(NonSmoothLaw & nslaw)996 bool MultipleImpact::checkCompatibleNSLaw(NonSmoothLaw& nslaw)
997 {
998
999 float type_number= (float) (Type::value(nslaw) + 0.1 * nslaw.size());
1000 _nslawtype.insert(type_number);
1001
1002 if (Type::value(nslaw) != Type::MultipleImpactNSL )
1003 {
1004 THROW_EXCEPTION("\nFrictionContact::checkCompatibleNSLaw - \n\
1005 The chosen nonsmooth law is not compatible with MultipleImpact one step nonsmooth problem. \n\
1006 Compatible NonSmoothLaw are: MultipleImpactNSL (2D or 3D) \n");
1007 return false;
1008 }
1009 if (_nslawtype.size() > 1)
1010 {
1011 THROW_EXCEPTION("\nFrictionContact::checkCompatibleNSLaw - \n\
1012 Compatible NonSmoothLaw are: NewtonImpactFrictionNSL (2D or 3D), but you cannot mix them \n");
1013 return false;
1014 }
1015
1016 return true;
1017 }
1018
1019
1020
1021
1022 //========================================================================================
compute(double time)1023 int MultipleImpact::compute(double time)
1024 {
1025 // Pre-compute for impact
1026 PreComputeImpact();
1027 // solve the multiple impacts
1028 if((_nContact != 0) && IsVcminNegative()) // if there is at least one contact and the vilocity before impact is negative
1029 {
1030 ComputeImpact();
1031 };
1032 // Post-compute for multiple impacts
1033 PostComputeImpact();
1034 return 0;
1035 }
1036
1037 //========================================================================================
display() const1038 void MultipleImpact::display() const
1039 {
1040 std::cout << "<<<<<<<<<<<<<<<<< Information about the multiple impact >>>>>>>>>>>>>>>>>>>>>" <<std::endl;
1041 std::cout << "Type of the contact compliance law: " << _typeCompLaw <<std::endl;
1042 std::cout << "Number of contacts involved into impacts: " << _nContact <<std::endl;
1043 std::cout << "Step size used: " << _deltaP <<std::endl;
1044 std::cout << "Primary impulse at the end of impact: " << _impulseVariable <<std::endl;
1045 std::cout << "Duration of the multiple impacs process: " << _timeVariable <<std::endl;
1046 // Display post-impact velocities
1047 SP::DynamicalSystemsGraph DSG0 = simulation()->nonSmoothDynamicalSystem()->topology()->dSG(0);
1048 DynamicalSystemsGraph::VIterator ui, uiend;
1049 for(std::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
1050 {
1051 SP::DynamicalSystem ds = DSG0->bundle(*ui);
1052 SP::LagrangianDS lag_ds = std::dynamic_pointer_cast<LagrangianDS>(ds);
1053 std::cout << "DS number: " << ds->number() <<std::endl;
1054 std::cout << "Pre-impact velocity: ";
1055 (lag_ds->velocityMemory().getSiconosVector(1)).display();
1056 std::cout << "Post-impact velocity: ";
1057 (lag_ds->velocity())->display();
1058 }
1059 // Display impulses at contact points
1060 SP::InteractionsGraph IndexSet0 = simulation()->nonSmoothDynamicalSystem()->topology()->indexSet(0);
1061 InteractionsGraph::VIterator vi, viend;
1062 for(std::tie(vi, viend) = IndexSet0->vertices(); vi != viend; ++vi)
1063 {
1064 SP::Interaction inter = IndexSet0->bundle(*vi);
1065 std::cout << "Impulse at contact point " << inter->number() << ":";
1066 (inter->lambda(1))->display();
1067 }
1068 };
1069