1# Mechanics IO 2"""Run a pre-configured Siconos "mechanics" HDF5 file.""" 3 4from __future__ import print_function 5 6import os 7import sys 8 9from math import cos, sin, asin, atan2 10from scipy import constants 11 12import numpy as np 13import h5py 14## fix compatibility with h5py version 15if (h5py.version.version_tuple.major >=3 ): 16 h5py_vlen_dtype = h5py.vlen_dtype 17else: 18 h5py_vlen_dtype = h5py.new_vlen 19 20import bisect 21import time 22import numbers 23import shutil 24 25 26import tempfile 27from contextlib import contextmanager 28 29# Siconos imports 30import siconos.io.mechanics_hdf5 31import siconos.numerics as sn 32import siconos.kernel as sk 33from siconos.kernel import \ 34 EqualityConditionNSL, \ 35 Interaction, DynamicalSystem, TimeStepping,\ 36 SICONOS_OSNSP_TS_VELOCITY,\ 37 cast_FrictionContact 38 39# Siconos Mechanics imports 40from siconos.mechanics.collision.tools import Contactor, Shape 41from siconos.mechanics import joints 42from siconos.io.io_base import MechanicsIO 43from siconos.io.FrictionContactTrace import GlobalFrictionContactTrace as GFCTrace 44from siconos.io.FrictionContactTrace import FrictionContactTrace as FCTrace 45from siconos.io.FrictionContactTrace import GlobalRollingFrictionContactTrace as GRFCTrace 46from siconos.io.mechanics_hdf5 import MechanicsHdf5 47 48 49# Imports for mechanics 'collision' submodule 50from siconos.mechanics.collision import RigidBodyDS, RigidBody2dDS, \ 51 SiconosSphere, SiconosBox, SiconosCylinder, SiconosCone, SiconosCapsule, \ 52 SiconosPlane, SiconosConvexHull, \ 53 SiconosDisk, SiconosBox2d, SiconosConvexHull2d, \ 54 SiconosContactor, SiconosContactorSet, \ 55 SiconosMesh, SiconosHeightMap 56 57# It is necessary to select a back-end, although currently only Bullet 58# is supported for general objects. 59backend = 'bullet' 60 61 62def set_backend(b): 63 global backend 64 backend = b 65 setup_default_classes() 66 67 68have_bullet = False 69have_occ = False 70 71try: 72 from siconos.mechanics.collision.bullet import \ 73 SiconosBulletCollisionManager, SiconosBulletOptions, SICONOS_BULLET_2D 74 have_bullet = True 75except Exception: 76 have_bullet = False 77 78# OCC imports 79try: 80 from siconos.mechanics import occ 81 have_occ = True 82except Exception: 83 have_occ = False 84 85# Configuration 86 87default_manager_class = None 88default_simulation_class = None 89default_body_class = None 90use_bullet = False 91 92 93def setup_default_classes(): 94 global default_manager_class 95 global default_simulation_class 96 global default_body_class 97 global use_bullet 98 default_simulation_class = TimeStepping 99 default_body_class = RigidBodyDS 100 if backend == 'bullet': 101 if not have_bullet: 102 msg = '[mechanics_run] WARNING: Bullet is the collision backend' 103 msg += 'but the module siconos.mechanics.collision.bullet' 104 msg += 'can not be imported. Check your installation.' 105 print(msg) 106 107 def m(bullet_options): 108 if bullet_options is None: 109 bullet_options = SiconosBulletOptions() 110 return SiconosBulletCollisionManager(bullet_options) 111 default_manager_class = m 112 use_bullet = have_bullet 113 elif backend == 'occ': 114 default_manager_class = lambda options: occ.OccSpaceFilter() 115 #def default_manager_class(options): 116 # occ.OccSpaceFilter() 117 default_simulation_class = occ.OccTimeStepping 118 default_body_class = occ.OccBody 119 use_bullet = False 120 121 122setup_default_classes() 123 124# Constants 125 126joint_points_axes = { 127 'KneeJointR': (1, 0), 128 'PivotJointR': (1, 1), 129 'PrismaticJointR': (0, 1), 130 'CylindricalJointR': (1, 1), 131 'FixedJointR': (0, 0), 132} 133 134 135# Utility functions 136def floatv(v): 137 return [float(x) for x in v] 138 139 140def arguments(): 141 """Returns tuple containing dictionary of calling function's 142 named arguments and a list of calling function's unnamed 143 positional arguments. 144 """ 145 from inspect import getargvalues, stack 146 posname, kwname, args = getargvalues(stack()[1][0])[-3:] 147 posargs = args.pop(posname, []) 148 args.update(args.pop(kwname, [])) 149 return args, posargs 150 151 152@contextmanager 153def tmpfile(suffix='', prefix='siconos_io', contents=None, 154 debug=False): 155 """ 156 A context manager for a named temporary file. 157 """ 158 (_, tfilename) = tempfile.mkstemp(suffix=suffix, prefix=prefix) 159 fid = open(tfilename, 'w') 160 if contents is not None: 161 fid.write(contents) 162 fid.flush() 163 164 class TmpFile: 165 166 def __init__(self, fid, name): 167 self.fid = fid 168 self.name = name 169 170 def __getitem__(self, n): 171 if n == 0: 172 return self.fid 173 elif n == 1: 174 return self.name 175 else: 176 raise IndexError 177 178 r = TmpFile(fid, tfilename) 179 180 yield r 181 fid.close() 182 if not debug: 183 os.remove(tfilename) 184 185time_measure = time.perf_counter 186if (sys.version_info.major+0.1*sys.version_info.minor < 3.3): 187 time_measure= time.clock 188 189 190 191class Timer(): 192 193 def __init__(self): 194 self._t0 = time_measure() 195 196 def elapsed(self): 197 return time_measure() - self._t0 198 199 def update(self): 200 self._t0 = time_measure() 201 202 203def warn(msg): 204 sys.stderr.write('{0}: {1}'.format(sys.argv[0], msg)) 205 206 207def object_id(obj): 208 """returns an unique object identifier""" 209 return obj.__hash__() 210 211 212def group(h, name, must_exist=True): 213 try: 214 return h[name] 215 except KeyError: 216 if must_exist: 217 return h.create_group(name) 218 else: 219 try: 220 return h.create_group(name) 221 except ValueError: 222 # could not create group, return None 223 # (file is probably in read-only mode) 224 return None 225 226 227def data(h, name, nbcolumns, use_compression=False): 228 try: 229 return h[name] 230 except KeyError: 231 comp = use_compression and nbcolumns > 0 232 return h.create_dataset(name, (0, nbcolumns), 233 maxshape=(None, nbcolumns), 234 chunks=[None, (4000, nbcolumns)][comp], 235 compression=[None, 'gzip'][comp], 236 compression_opts=[None, 9][comp]) 237 238# 239# misc fixes 240# 241# fix ctr.'name' in old hdf5 files 242# 243def upgrade_io_format(filename): 244 245 with MechanicsHdf5(filename, mode='a') as io: 246 247 for instance_name in io.instances(): 248 for contactor_instance_name in io.instances()[instance_name]: 249 contactor = io.instances()[instance_name][ 250 contactor_instance_name] 251 if 'name' in contactor.attrs: 252 warn(""" 253contactor {0} attribute 'name': renamed in 'shape_name' 254 """) 255 contactor.attrs['shape_name'] = contactor['name'] 256 del contactor['name'] 257 258 259def str_of_file(filename): 260 with open(filename, 'r') as f: 261 return str(f.read()) 262 263 264def file_of_str(filename, string): 265 if not os.path.exists(os.path.dirname(filename)): 266 try: 267 os.makedirs(os.path.dirname(filename)) 268 except OSError as exc: 269 if exc.errno != exc.errno.EEXIST: 270 raise 271 272 with open(filename, "w") as f: 273 f.write(string) 274 275 276class Quaternion(): 277 278 def __init__(self, *args): 279 import vtk 280 self._vtkmath = vtk.vtkMath() 281 self._data = vtk.vtkQuaternion[float](*args) 282 283 def __mul__(self, q): 284 r = Quaternion() 285 self._vtkmath.MultiplyQuaternion(self._data, q._data, r._data) 286 return r 287 288 def __getitem__(self, i): 289 return self._data[i] 290 291 def conjugate(self): 292 r = Quaternion((self[0], self[1], self[2], self[3])) 293 r._data.Conjugate() 294 return r 295 296 def rotate(self, v): 297 pv = Quaternion((0, v[0], v[1], v[2])) 298 rv = self * pv * self.conjugate() 299 # assert(rv[0] == 0) 300 return [rv[1], rv[2], rv[3]] 301 302 def axisAngle(self): 303 r = [0, 0, 0] 304 a = self._data.GetRotationAngleAndAxis(r) 305 return r, a 306 307 308# 309# fix orientation -> rotation ? 310# 311def quaternion_get(orientation): 312 """ 313 Get quaternion from orientation 314 """ 315 if len(orientation) == 2: 316 # axis + angle 317 axis = orientation[0] 318 if not (len(axis) == 3): 319 raise AssertionError("quaternion_get. axis in not a 3D vector") 320 angle = orientation[1] 321 if not isinstance(angle, float): 322 raise AssertionError("quaternion_get. angle must be a float") 323 n = sin(angle / 2.) / np.linalg.norm(axis) 324 ori = [cos(angle / 2.), axis[0] * n, axis[1] * n, axis[2] * n] 325 else: 326 if not (len(orientation) == 4): 327 raise AssertionError( 328 "quaternion_get. The quaternion must be of size 4") 329 # a given quaternion 330 ori = orientation 331 return ori 332 333 334def quaternion_multiply(q1, q0): 335 w0, x0, y0, z0 = q0 336 w1, x1, y1, z1 = q1 337 return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 338 x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 339 -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 340 x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64) 341 342 343def phi(q0, q1, q2, q3): 344 """ 345 Euler angle phi from quaternion. 346 """ 347 return atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) 348 349 350def theta(q0, q1, q2, q3): 351 """ 352 Euler angle theta from quaternion. 353 """ 354 return asin(2 * (q0 * q2 - q3 * q1)) 355 356 357def psi(q0, q1, q2, q3): 358 """ 359 Euler angle psi from quaternion. 360 """ 361 return atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) 362 363 364# vectorized versions 365phiv = np.vectorize(phi) 366thetav = np.vectorize(theta) 367psiv = np.vectorize(psi) 368 369 370# 371# load .vtp file 372# 373def load_siconos_mesh(shape_filename, scale=None): 374 """ 375 loads a vtk .vtp file and returns a SiconosMesh shape 376 WARNING triangles cells assumed! 377 """ 378 import vtk 379 380 reader = vtk.vtkXMLPolyDataReader() 381 reader.SetFileName(shape_filename) 382 reader.Update() 383 384 polydata = reader.GetOutput() 385 points = polydata.GetPoints().GetData() 386 num_points = points.GetNumberOfTuples() 387 num_triangles = polydata.GetNumberOfCells() 388 389 shape = None 390 391 if polydata.GetCellType(0) == 5: 392 apoints = np.empty((3, num_points), dtype='f8') 393 for i in range(0, points.GetNumberOfTuples()): 394 p = points.GetTuple(i) 395 apoints[0, i] = p[0] 396 apoints[1, i] = p[1] 397 apoints[2, i] = p[2] 398 399 if scale is not None: 400 apoints *= scale 401 402 aindices = np.empty(num_triangles * 3, dtype=np.int) 403 404 for i in range(0, num_triangles): 405 c = polydata.GetCell(i) 406 aindices[i * 3 + 0] = c.GetPointIds().GetId(0) 407 aindices[i * 3 + 1] = c.GetPointIds().GetId(1) 408 aindices[i * 3 + 2] = c.GetPointIds().GetId(2) 409 410 shape = SiconosMesh(list(aindices), apoints) 411 dims = apoints.max(axis=0) - apoints.min(axis=0) 412 413 else: # assume convex shape 414 coors = dict() 415 for i in range(0, points.GetNumberOfTuples()): 416 coors[points.GetTuple(i)] = 1 417 coors = np.array(coors.keys()) 418 dims = coors.max(axis=0) - coors.min(axis=0) 419 shape = SiconosConvexHull(coors.keys()) 420 421 return shape, dims 422 423 424class ShapeCollection(): 425 """ 426 Instantiation of added contact shapes 427 """ 428 429 def __init__(self, io, collision_margin=0.04): 430 self._io = io 431 self._shapes = dict() 432 self._tri = dict() 433 self._primitive = {'Sphere': SiconosSphere, 434 'Box': SiconosBox, 435 'Cylinder': SiconosCylinder, 436 'Capsule': SiconosCapsule, 437 'Cone': SiconosCone, 438 'Plane': SiconosPlane, 439 'Disk': SiconosDisk, 440 'Box2d': SiconosBox2d} 441 442 def shape(self, shape_name): 443 return self._io.shapes()[shape_name] 444 445 def attributes(self, shape_name): 446 return self._io.shapes()[shape_name].attrs 447 448 def url(self, shape_name): 449 if 'url' in self.attributes(shape_name): 450 shape_url = self.shape(shape_name).\ 451 attrs['url'] 452 453 elif 'filename' in self.attributes(shape_name): 454 shape_url = self.shape(shape_name).\ 455 attrs['filename'] 456 457 else: 458 shape_url = self.shape(shape_name) 459 460 return shape_url 461 462 def get(self, shape_name, shape_class=None, face_class=None, 463 edge_class=None, new_instance=False): 464 465 if new_instance or shape_name not in self._shapes: 466 467 # load shape if it is an existing file 468 if not isinstance(self.url(shape_name), str) and \ 469 'primitive' not in self.attributes(shape_name): 470 # assume a vtp file (xml) stored in a string buffer 471 472 if self.attributes(shape_name)['type'] == 'vtp': 473 if self.shape(shape_name).dtype == h5py_vlen_dtype(str): 474 with tmpfile() as tmpf: 475 data = self.shape(shape_name)[:][0] 476 ## fix compatibility with h5py version: to be removed in the future 477 if (h5py.version.version_tuple.major >=3 ): 478 tmpf[0].write(data.decode("utf-8")) 479 else: 480 tmpf[0].write(data) 481 tmpf[0].flush() 482 scale = self.attributes(shape_name).get( 483 'scale', None) 484 mesh, dims = load_siconos_mesh( 485 tmpf[1], scale=scale) 486 self._shapes[shape_name] = mesh 487 mesh.setInsideMargin( 488 self.shape(shape_name).attrs.get( 489 'insideMargin', min(dims) * 0.02)) 490 mesh.setOutsideMargin( 491 self.shape(shape_name).attrs.get( 492 'outsideMargin', 0)) 493 else: 494 raise AssertionError( 495 'ShapeCollection.get(), attributes(shape_name)["type"] != vtp ') 496 497 elif self.attributes(shape_name)['type'] in ['step', 'stp']: 498 from OCC.STEPControl import STEPControl_Reader 499 from OCC.BRep import BRep_Builder 500 from OCC.TopoDS import TopoDS_Compound 501 from OCC.IFSelect import IFSelect_RetDone,\ 502 IFSelect_ItemsByEntity 503 504 builder = BRep_Builder() 505 comp = TopoDS_Compound() 506 builder.MakeCompound(comp) 507 508 if self.shape(shape_name).dtype != h5py_vlen_dtype(str) : 509 raise AssertionError("self.shape(shape_name).dtype != h5py_vlen_dtype(str)") 510 511 ## fix compatibility with h5py version: to be removed in the future 512 if (h5py.version.version_tuple.major >=3 ): 513 tmp_contents =self.shape(shape_name)[:][0].decode('utf-8') 514 else : 515 tmp_contents =self.shape(shape_name)[:][0] 516 517 with tmpfile(contents=tmp_contents) as tmpf: 518 step_reader = STEPControl_Reader() 519 520 status = step_reader.ReadFile(tmpf[1]) 521 522 if status == IFSelect_RetDone: # check status 523 failsonly = False 524 step_reader.PrintCheckLoad( 525 failsonly, IFSelect_ItemsByEntity) 526 step_reader.PrintCheckTransfer( 527 failsonly, IFSelect_ItemsByEntity) 528 529 # ok = step_reader.TransferRoot(1) 530 step_reader.TransferRoots() 531 # VA : We decide to loads all shapes in the step file 532 nbs = step_reader.NbShapes() 533 534 for i in range(1, nbs + 1): 535 shape = step_reader.Shape(i) 536 builder.Add(comp, shape) 537 538 self._shapes[shape_name] = comp 539 self._io._keep.append(self._shapes[shape_name]) 540 541 elif self.attributes(shape_name)['type'] in ['iges', 'igs']: 542 from OCC.IGESControl import IGESControl_Reader 543 from OCC.BRep import BRep_Builder 544 from OCC.TopoDS import TopoDS_Compound 545 from OCC.IFSelect import IFSelect_RetDone,\ 546 IFSelect_ItemsByEntity 547 548 builder = BRep_Builder() 549 comp = TopoDS_Compound() 550 builder.MakeCompound(comp) 551 552 if not (self.shape(shape_name).dtype == h5py_vlen_dtype(str)): 553 raise AssertionError("ShapeCollection.get()") 554 555 #assert(self.shape(shape_name).dtype == h5py_vlen_dtype(str)) 556 557 with tmpfile(contents=self.shape(shape_name)[:][0]) as tmpf: 558 iges_reader = IGESControl_Reader() 559 560 status = iges_reader.ReadFile(tmpf[1]) 561 562 if status == IFSelect_RetDone: # check status 563 failsonly = False 564 iges_reader.PrintCheckLoad( 565 failsonly, IFSelect_ItemsByEntity) 566 iges_reader.PrintCheckTransfer( 567 failsonly, IFSelect_ItemsByEntity) 568 569 iges_reader.TransferRoots() 570 nbs = iges_reader.NbShapes() 571 572 for i in range(1, nbs + 1): 573 shape = iges_reader.Shape(i) 574 builder.Add(comp, shape) 575 576 self._shapes[shape_name] = comp 577 self._io._keep.append(self._shapes[shape_name]) 578 579 elif self.attributes(shape_name)['type'] in['brep']: 580 if 'contact' not in self.attributes(shape_name): 581 582 # the reference brep 583 if shape_class is None: 584 brep_class = occ.OccContactShape 585 else: 586 brep_class = shape_class 587 588 if 'occ_indx' in self.attributes(shape_name): 589 590 from OCC.BRepTools import BRepTools_ShapeSet 591 shape_set = BRepTools_ShapeSet() 592 shape_set.ReadFromString( 593 self.shape(shape_name)[:][0]) 594 the_shape = shape_set.Shape(shape_set.NbShapes()) 595 location = shape_set.Locations().Location( 596 self.attributes(shape_name)['occ_indx']) 597 the_shape.Location(location) 598 brep = brep_class() 599 brep.setData(the_shape) 600 601 else: 602 # raw brep 603 brep = brep_class() 604 brep.importBRepFromString( 605 self.shape(shape_name)[:][0]) 606 607 self._shapes[shape_name] = brep 608 self._io._keep.append(self._shapes[shape_name]) 609 610 else: 611 # a contact on a brep 612 if not ('contact' in self.attributes(shape_name)): 613 raise AssertionError("contact not in self.attributes(shape_name)") 614 if not ('contact_index' in self.attributes(shape_name)): 615 raise AssertionError("contact_index not in self.attributes(shape_name)") 616 if not ('brep' in self.attributes(shape_name)): 617 raise AssertionError("brep not in self.attributes(shape_name)") 618 619 620 #assert 'contact' in self.attributes(shape_name) 621 #assert 'contact_index' in self.attributes(shape_name) 622 #assert 'brep' in self.attributes(shape_name) 623 624 contact_index = self.attributes(shape_name)['contact_index'] 625 626 if shape_class is None: 627 brep_class = occ.OccContactShape 628 else: 629 brep_class = shape_class 630 631 ref_brep = self.get( 632 self.attributes(shape_name)['brep'], shape_class) 633 634 if self.attributes(shape_name)['contact'] == 'Face': 635 if face_class is None: 636 face_maker = occ.OccContactFace 637 else: 638 face_maker = face_class 639 640 self._shapes[shape_name] = \ 641 face_maker(brep_class(ref_brep), 642 contact_index) 643 644 elif self.attributes(shape_name)['contact'] == 'Edge': 645 if edge_class is None: 646 edge_maker = occ.OccContactEdge 647 else: 648 edge_maker = edge_class 649 self._shapes[shape_name] = \ 650 edge_maker(ref_brep, 651 contact_index) 652 653 self._io._keep.append(self._shapes[shape_name]) 654 655 elif self.attributes(shape_name)['type'] in ['heightmap']: 656 # a heightmap defined by a matrix 657 hm_data = self.shape(shape_name) 658 r = hm_data.attrs['rect'] 659 if (len(r) != 2): 660 raise AssertionError("len(r) != 2") 661 #assert(len(r) == 2) 662 hm = SiconosHeightMap(hm_data, r[0], r[1]) 663 dims = list(r) + [np.max(hm_data) - np.min(hm_data)] 664 hm.setInsideMargin( 665 hm_data.attrs.get('insideMargin', np.min(dims) * 0.02)) 666 hm.setOutsideMargin( 667 hm_data.attrs.get('outsideMargin', 0)) 668 669 self._shapes[shape_name] = hm 670 671 elif self.attributes(shape_name)['type'] in ['convex']: 672 # a convex point set 673 points = self.shape(shape_name) 674 if self._io._dimension == 3: 675 convex = SiconosConvexHull(points) 676 dims = [points[:, 0].max() - points[:, 0].min(), 677 points[:, 1].max() - points[:, 1].min(), 678 points[:, 2].max() - points[:, 2].min()] 679 elif self._io._dimension == 2: 680 convex = SiconosConvexHull2d(points) 681 dims = [points[:, 0].max() - points[:, 0].min(), 682 points[:, 1].max() - points[:, 1].min()] 683 684 convex.setInsideMargin( 685 self.shape(shape_name).attrs.get('insideMargin', 686 min(dims) * 0.02)) 687 convex.setOutsideMargin( 688 self.shape(shape_name).attrs.get('outsideMargin', 0)) 689 self._shapes[shape_name] = convex 690 691 elif isinstance(self.url(shape_name), str) and \ 692 os.path.exists(self.url(shape_name)): 693 self._tri[shape_name], self._shapes[shape_name] = loadMesh( 694 self.url(shape_name), _collision_margin) 695 else: 696 # it must be a primitive with attributes 697 if isinstance(self.url(shape_name), str): 698 name = self.url(shape_name) 699 attrs = [float(x) for x in self.shape(shape_name)[0]] 700 else: 701 name = self.attributes(shape_name)['primitive'] 702 attrs = [float(x) for x in self.shape(shape_name)[0]] 703 primitive = self._primitive[name] 704 705 if name in ['Box']: 706 box = primitive(attrs) 707 self._shapes[shape_name] = box 708 box.setInsideMargin( 709 self.shape(shape_name).attrs.get('insideMargin', 710 min(attrs)*0.02)) 711 box.setOutsideMargin( 712 self.shape(shape_name).attrs.get('outsideMargin', 0)) 713 # elif name in ['Compound']: 714 # obj1 = attrs[0] 715 # orig1 = attrs[1:4] 716 # orie1 = attrs[4:8] 717 # obj2 = attrs[8] 718 # orig2 = attrs[9:12] 719 # orie2 = attrs[12:16] 720 # bcols = btCompoundShape() 721 # bcols.addChildShape(... 722 else: # e.g. name in ['Sphere']: 723 prim = self._shapes[shape_name] = primitive(*attrs) 724 shp = self.shape(shape_name) 725 prim.setInsideMargin( 726 shp.attrs.get('insideMargin', min(attrs)*0.02)) 727 prim.setOutsideMargin(shp.attrs.get('outsideMargin', 0)) 728 729 return self._shapes[shape_name] 730 731 732class MechanicsHdf5Runner(siconos.io.mechanics_hdf5.MechanicsHdf5): 733 734 """a Hdf5 context manager that reads the translations and 735 orientations of collision objects from hdf5 file 736 737 It provides functions to output translations and orientations in 738 the same file during simulation (output is done by default in 739 pos.dat) 740 741 Parameters 742 ---------- 743 io_filename: string, optional 744 hdf5 file name, default = <caller>.hdf5, caller being the name 745 without ext of the file that instanciates the Runner. 746 mode: string, optional 747 h5 mode (w, r, append), default = 'w' 748 interaction_manager: SiconosCollisionManager, optional 749 user-defined interaction handler (e.g. from Bullet), default=None 750 nsds: NonSmoothDynamicalSystem, optional 751 default = None 752 simulation: Simulation, optional 753 default = None 754 osi: OneStepIntegrator, optional 755 default = None 756 shape_filename: string 757 vtk file describing a mesh, default = None 758 set_external_forces: python function, optional 759 function used to apply forces onto the body. 760 Must be : 761 762 def funcname(body) 763 764 body being a siconos Body (DS) 765 Default : apply gravity forces. 766 gravity_scale: real, optional 767 multiplication factor for the gravity. 768 1. for meters (default). 769 1./100 for centimeters. 770 This parameter may be needed for small 771 objects because of Bullet collision margin. 772 collision_margin: real number, optional 773 tolerance for collision, default = None (0.04 in Shape builder) 774 use_compression: boolean, optional 775 true to use compression for h5 file, default=False 776 output_domains: boolean, optional 777 if trueoutputs info regarding contact point domains 778 default=False 779 verbose: boolean, optional 780 default=True 781 782 """ 783 784 def __init__(self, io_filename=None, io_filename_backup=None, mode='w', 785 interaction_manager=None, nsds=None, simulation=None, 786 osi=None, shape_filename=None, 787 set_external_forces=None, gravity_scale=None, 788 collision_margin=None, 789 use_compression=False, output_domains=False, verbose=True): 790 791 super(MechanicsHdf5Runner, self).__init__(io_filename, mode, 792 io_filename_backup, 793 use_compression, 794 output_domains, verbose) 795 self._interman = interaction_manager 796 self._nsds = nsds 797 self._simulation = simulation 798 self._osi = osi 799 self._static = {} 800 self._shape = None 801 self._occ_contactors = dict() 802 self._io = MechanicsIO() 803 self._set_external_forces = set_external_forces 804 self._shape_filename = shape_filename 805 self._number_of_shapes = 0 806 self._number_of_permanent_interactions = 0 807 self._number_of_dynamic_objects = 0 808 self._number_of_static_objects = 0 809 self._gravity_scale = gravity_scale 810 self._collision_margin = collision_margin 811 self._output_frequency = 1 812 self._output_backup_frequency = 1 813 self._keep = [] 814 self._scheduled_births = [] 815 self._scheduled_deaths = [] 816 self._births = dict() 817 self._deaths = dict() 818 self._initializing = True 819 self._contact_index_set = 1 820 self._scheduled_births = [] 821 self._start_run_iteration_hook = None 822 self._end_run_iteration_hook = None 823 824 def __enter__(self): 825 super(MechanicsHdf5Runner, self).__enter__() 826 827 if self._gravity_scale is None: 828 self._gravity_scale = 1 # 1 => m, 1/100. => cm 829 830 if self._set_external_forces is None: 831 self._set_external_forces = self.apply_gravity 832 833 if self._shape_filename is None: 834 if self._collision_margin: 835 self._shape = ShapeCollection( 836 io=self, collision_margin=self._collision_margin) 837 838 else: 839 self._shape = ShapeCollection(io=self) 840 else: 841 if self._collision_margin: 842 self._shape = ShapeCollection( 843 io=self._shape_filename, 844 collision_margin=self._collision_margin) 845 else: 846 self._shape = ShapeCollection(io=self._shape_filename) 847 return self 848 849 def log(self, fun, with_timer=False, before=True): 850 if with_timer: 851 t = Timer() 852 853 def logged(*args): 854 t.update() 855 if before: 856 print('[io.mechanics]| {0:50s} ...'.format(fun.__name__), 857 end='', flush=True) 858 else: 859 print('[io.mechanics]|-->start {0:42s} ...'. 860 format(fun.__name__), flush=True) 861 output = fun(*args) 862 endt = t.elapsed() 863 if not before: 864 print('[io.mechanics]|-->end {0:44s} ...'. 865 format(fun.__name__), end='', flush=True) 866 print('..... {0:6.2e} s'.format(endt)) 867 siconos.io.mechanics_hdf5.group(self.log_data(), fun.__name__) 868 siconos.io.mechanics_hdf5.add_line( 869 siconos.io.mechanics_hdf5.data(self.log_data() 870 [fun.__name__], 871 'timing', 1), endt) 872 if (isinstance(output, numbers.Number)): 873 siconos.io.mechanics_hdf5.add_line( 874 siconos.io.mechanics_hdf5.data(self.log_data() 875 [fun.__name__], 876 'value', 1), 877 float(output)) 878 return output 879 return logged 880 else: 881 def silent(*args): 882 output = fun(*args) 883 return output 884 return silent 885 886 def apply_gravity(self, body): 887 g = constants.g / self._gravity_scale 888 if self._dimension == 3: 889 weight = [0, 0, - body.scalarMass() * g] 890 elif self._dimension == 2: 891 weight = [0, - body.scalarMass() * g, 0.] 892 body.setFExtPtr(weight) 893 894 def import_nonsmooth_law(self, name): 895 if self._interman is not None: 896 nslawClass = getattr(sk, self._nslaws_data[name].attrs['type']) 897 if nslawClass == sk.NewtonImpactFrictionNSL: 898 nslaw = nslawClass( 899 float(self._nslaws_data[name].attrs['e']), 0., 900 float(self._nslaws_data[name].attrs['mu']), 901 self._dimension) 902 elif nslawClass == sk.NewtonImpactRollingFrictionNSL: 903 if self._dimension == 3: 904 nslaw = nslawClass( 905 float(self._nslaws_data[name].attrs['e']), 0., 906 float(self._nslaws_data[name].attrs['mu']), 907 float(self._nslaws_data[name].attrs['mu_r']), 5) 908 elif self._dimension == 2: 909 nslaw = nslawClass( 910 float(self._nslaws_data[name].attrs['e']), 0., 911 float(self._nslaws_data[name].attrs['mu']), 912 float(self._nslaws_data[name].attrs['mu_r']), 3) 913 elif nslawClass == sk.NewtonImpactNSL: 914 nslaw = nslawClass(float(self._nslaws_data[name].attrs['e'])) 915 elif nslawClass == sk.RelayNSL: 916 nslaw = nslawClass(int(self._nslaws_data[name].attrs['size']), 917 float(self._nslaws_data[name].attrs['lb']), 918 float(self._nslaws_data[name].attrs['ub'])) 919 if not nslaw: 920 raise AssertionError("no nslaw") 921 #assert(nslaw) 922 self._nslaws[name] = nslaw 923 gid1 = int(self._nslaws_data[name].attrs['gid1']) 924 gid2 = int(self._nslaws_data[name].attrs['gid2']) 925 if gid1 >= 0 and gid2 >= 0: 926 self._interman.insertNonSmoothLaw(nslaw, gid1, gid2) 927 928 def import_occ_object(self, name, translation, orientation, 929 velocity, contactors, mass, given_inertia, 930 body_class, shape_class, face_class, edge_class, 931 birth=False, number=None): 932 933 934 if mass is None: 935 # a static object 936 body = None 937 938 self._static[name] = { 939 'number': number, 940 'origin': translation, 941 'orientation': orientation} 942 flag='static' 943 else: 944 945 if not np.isscalar(mass) or mass <= 0: 946 self.print_verbose('Warning mass must be a positive scalar') 947 raise RuntimeError('Warning mass must be a positive scalar') 948 949 if body_class is None: 950 body_class = occ.OccBody 951 952 #assert (given_inertia is not None) 953 if given_inertia is None: 954 raise AssertionError("given_inertia is None") 955 inertia = given_inertia 956 if inertia is not None: 957 if np.shape(inertia) == (3,): 958 inertia = np.diag(inertia) 959 elif np.shape(inertia) != (3, 3): 960 self.print_verbose('Wrong shape of inertia') 961 962 body = body_class( 963 list(translation) + list(orientation), velocity, mass, inertia) 964 965 if number is not None: 966 body.setNumber(int(number)) 967 flag='dynamic' 968 969 ref_shape = {ctor.instance_name: occ.OccContactShape( 970 self._shape.get(ctor.shape_name, 971 shape_class, face_class, 972 edge_class, new_instance=True)) 973 for ctor in contactors} 974 975 ref_added = dict() 976 for contactor in contactors: 977 978 contact_shape = None 979 reference_shape = ref_shape[contactor.instance_name] 980 981 self._keep.append(reference_shape) 982 983 if hasattr(contactor, 'contact_type'): 984 985 if contactor.contact_type == 'Face': 986 contact_shape = occ.OccContactFace( 987 reference_shape, contactor.contact_index) 988 989 elif contactor.contact_type == 'Edge': 990 contact_shape = occ.OccContactEdge( 991 reference_shape, contactor.contact_index) 992 993 if contact_shape is not None: 994 995 if name not in self._occ_contactors: 996 self._occ_contactors[name] = dict() 997 998 self._occ_contactors[name][contactor.instance_name] = \ 999 contact_shape 1000 1001 if body is not None: 1002 body.addContactShape(contact_shape, 1003 contactor.translation, 1004 contactor.orientation, 1005 contactor.group) 1006 1007 if reference_shape not in ref_added: 1008 if body is not None: 1009 body.addShape(reference_shape.shape(), 1010 contactor.translation, 1011 contactor.orientation) 1012 else: 1013 occ.occ_move( 1014 reference_shape.shape(), 1015 list(contactor.translation) + list(contactor.orientation)) 1016 ref_added[reference_shape] = True 1017 1018 if body is not None: 1019 self._set_external_forces(body) 1020 1021 # add the dynamical system to the non smooth 1022 # dynamical system 1023 self._nsds.insertDynamicalSystem(body) 1024 self._nsds.setName(body, str(name)) 1025 1026 if birth and self._verbose: 1027 self.print_verbose('birth of body named {0}, translation {1}, orientation {2}'.format(name, translation, orientation)) 1028 1029 return body, flag 1030 1031 def import_bullet_object(self, name, translation, orientation, 1032 velocity, contactors, mass, inertia, 1033 body_class, shape_class, birth=False, 1034 number=None): 1035 1036 if body_class is None: 1037 body_class = default_body_class 1038 1039 if self._dimension == 2: 1040 body_class = RigidBody2dDS 1041 1042 if self._interman is not None and 'input' in self._data: 1043 body = None 1044 # --------------- 1045 # a static object 1046 # --------------- 1047 if mass is None: 1048 1049 cset = SiconosContactorSet() 1050 csetpos = (translation + orientation) 1051 for c in contactors: 1052 shp = self._shape.get(c.shape_name) 1053 pos = list(c.translation) + list(c.orientation) 1054 cset.append(SiconosContactor(shp, pos, c.group)) 1055 self.print_verbose(' Adding shape %s to static contactor'%c.shape_name, 'at relative position', pos) 1056 1057 staticContactorSetID = self._interman.insertStaticContactorSet(cset, csetpos) 1058 1059 self._static[name] = { 1060 'number': number, 1061 'origin': translation, 1062 'orientation': orientation, 1063 'shape': shp, 1064 } 1065 # In the case of a static object, we return the staticContactorSetID and a flag 1066 # this will be used to remove the contactor if we have a time of death 1067 return staticContactorSetID, 'static' 1068 1069 # --------------- 1070 # a dynamic object 1071 # --------------- 1072 else: 1073 1074 if not np.isscalar(mass) or mass <= 0: 1075 self.print_verbose('Warning mass must be a positive scalar') 1076 1077 inertia_ok = False 1078 if self._dimension == 3: 1079 if inertia is not None: 1080 if np.shape(inertia) == (3,): 1081 inertia = np.diag(inertia) 1082 inertia_ok = True 1083 if np.shape(inertia) == (3, 3): 1084 inertia_ok = True 1085 elif self._dimension == 2: 1086 if inertia is not None: 1087 if np.shape(inertia) == (1, 1) or\ 1088 np.shape(inertia) == (1, ) or np.isscalar(inertia): 1089 inertia_ok = True 1090 1091 if inertia_ok: 1092 # create the dynamics object with mass and inertia 1093 body = body_class(translation + orientation, 1094 velocity, mass, inertia) 1095 body.setUseContactorInertia(False) 1096 else: 1097 if inertia is not None: 1098 if self._dimension == 3: 1099 self.print_verbose('**** Warning inertia for object named {0} does not have the correct shape: {1} instead of (3, 3) or (3,)'.format(name, np.shape(inertia))) 1100 self.print_verbose('**** Inertia will be computed with the shape of the first contactor') 1101 elif self._dimension == 2: 1102 self.print_verbose('**** Warning inertia for object named {0} does not have the correct shape: {1} instead of (1, 1) or (1,) or scalar'.format(name, np.shape(inertia))) 1103 self.print_verbose('**** Inertia will be computed with the shape of the first contactor') 1104 1105 body = body_class(translation + orientation, 1106 velocity, mass) 1107 body.setUseContactorInertia(True) 1108 1109 1110 fext = self._input[name].get('allow_self_collide', 1111 None) 1112 if fext is not None: 1113 body.setFextPtr(fext) 1114 1115 1116 self_collide = self._input[name].get('allow_self_collide', 1117 None) 1118 if self_collide is not None: 1119 body.setAllowSelfCollide(not not self_collide) 1120 1121 cset = SiconosContactorSet() 1122 for c in contactors: 1123 shp = self._shape.get(c.shape_name) 1124 pos = list(c.translation) + list(c.orientation) 1125 cset.append(SiconosContactor(shp, pos, c.group)) 1126 self.print_verbose(' Adding shape %s to dynamic contactor'%c.shape_name, 'at relative position', pos) 1127 1128 body.setContactors(cset) 1129 1130 if body: 1131 # set id number 1132 if number is not None: 1133 body.setNumber(int(number)) 1134 1135 # set external forces 1136 self._set_external_forces(body) 1137 1138 # add the dynamical system to the non smooth 1139 # dynamical system 1140 self._nsds.insertDynamicalSystem(body) 1141 self._nsds.setName(body, str(name)) 1142 1143 return body, 'dynamic' 1144 1145 def make_coupler_jointr(self, ds1_name, ds2_name, coupled, references): 1146 topo = self._nsds.topology() 1147 dof1, dof2, ratio = coupled[0, :] 1148 refds_name = None 1149 if len(references) > 0: 1150 joint1_name = references[0] 1151 joint1 = joints.cast_NewtonEulerJointR( 1152 topo.getInteraction(str(joint1_name)).relation()) 1153 joint2 = joint1 1154 joint1_ds1 = self.joints()[joint1_name].attrs['object1'] 1155 joint1_ds2 = self.joints()[joint1_name].attrs.get('object2', None) 1156 if len(references) > 1: 1157 joint2_name = references[1] 1158 joint2 = joints.cast_NewtonEulerJointR( 1159 topo.getInteraction(str(joint2_name)).relation()) 1160 joint2_ds1 = self.joints()[joint2_name].attrs['object1'] 1161 joint2_ds2 = self.joints()[joint2_name].attrs.get('object2', None) 1162 1163 if len(references) > 2: 1164 refds_name = references[2] 1165 else: 1166 # if second joint provided but no reference, then 1167 # infer refds_name from the reference joints 1168 dss = set([joint1_ds1, joint1_ds2, joint2_ds1, joint2_ds2]) 1169 diff = list(dss.difference(set([ds1_name, ds2_name]))) 1170 # there must be exactly one reference in common that 1171 # is not either of the DSs 1172 refds_name = diff[0] 1173 1174 if refds_name: 1175 refds = sk.cast_NewtonEulerDS(topo.getDynamicalSystem(str(refds_name))) 1176 1177 # Determine reference indexes: 1178 # Assert if neither ds in reference joints is the 1179 # ref ds and that the other ds is ds1/ds2 as 1180 # appropriate. 1181 refidx1, refidx2 = 0, 0 1182 if joint1_ds1 == ds1_name: 1183 refidx1 = 1 1184 if (joint1_ds2 != refds_name): 1185 raise AssertionError("joint1_ds2 != refds_name") 1186 #assert(joint1_ds2 == refds_name) 1187 else: 1188 if (joint1_ds1 != refds_name): 1189 raise AssertionError("joint1_ds1 != refds_name") 1190 #assert(joint1_ds1 == refds_name) 1191 if (joint1_ds2 != ds1_name): 1192 raise AssertionError("joint1_ds2 != ds1_name") 1193 #assert(joint1_ds2 == ds1_name) 1194 if joint2_ds1 == ds2_name: 1195 refidx2 = 1 1196 if (joint2_ds2 != refds_name): 1197 raise AssertionError("joint2_ds2 != refds_name") 1198 #assert(joint2_ds2 == refds_name) 1199 else: 1200 #assert(joint2_ds1 == refds_name) 1201 if (joint2_ds1 != refds_name): 1202 raise AssertionError("joint2_ds1 != refds_name") 1203 #assert(joint2_ds2 == ds2_name) 1204 if (joint2_ds2 != ds2_name): 1205 raise AssertionError("joint2_ds2 != ds1_name") 1206 1207 joint = joints.CouplerJointR(joint1, int(dof1), 1208 joint2, int(dof2), ratio, 1209 refds, refidx1, refds, refidx2) 1210 else: 1211 # otherwise we assume no reference, coupler is directly 1212 # between ds1 and ds2 1213 joint = joints.CouplerJointR(joint1, int(dof1), 1214 joint2, int(dof2), ratio) 1215 return joint 1216 1217 def import_joint(self, name): 1218 if self._interman is not None: 1219 nsds = self._nsds 1220 topo = nsds.topology() 1221 1222 joint_type = self.joints()[name].attrs['type'] 1223 joint_class = getattr(joints, joint_type) 1224 absolute = not not self.joints()[name].attrs.get('absolute', True) 1225 allow_self_collide = self.joints()[name].attrs.get( 1226 'allow_self_collide', None) 1227 stops = self.joints()[name].attrs.get('stops', None) 1228 nslaws = self.joints()[name].attrs.get('nslaws', None) 1229 friction = self.joints()[name].attrs.get('friction', None) 1230 coupled = self.joints()[name].attrs.get('coupled', None) 1231 references = self.joints()[name].attrs.get('references', None) 1232 1233 # work-around h5py unicode bug 1234 # https://github.com/h5py/h5py/issues/379 1235 if references is not None: 1236 references = [r.decode('utf-8') if isinstance(r, bytes) else r 1237 for r in references] 1238 1239 points = self.joints()[name].attrs.get('points', []) 1240 axes = self.joints()[name].attrs.get('axes', []) 1241 siconos.io.mechanics_hdf5.check_points_axes(name, joint_class, 1242 points, axes) 1243 1244 ds1_name = self.joints()[name].attrs['object1'] 1245 ds1 = topo.getDynamicalSystem(ds1_name) 1246 ds2 = None 1247 1248 if 'object2' in self.joints()[name].attrs: 1249 ds2_name = self.joints()[name].attrs['object2'] 1250 ds2 = topo.getDynamicalSystem(ds2_name) 1251 1252 if joint_class == joints.CouplerJointR: 1253 # This case is a little different, handle it specially 1254 #assert(references is not None) 1255 #assert(np.shape(coupled) == (1, 3)) 1256 if (references is None): 1257 raise AssertionError("references is None") 1258 if (np.shape(coupled) != (1, 3)): 1259 raise AssertionError("np.shape(coupled) != (1, 3)") 1260 1261 joint = self.make_coupler_jointr(ds1_name, ds2_name, 1262 coupled, references) 1263 coupled = None # Skip the use for "coupled" below, to 1264 # install joint-local couplings 1265 1266 else: 1267 # Generic NewtonEulerJointR interface 1268 joint = joint_class() 1269 for n, p in enumerate(points): 1270 joint.setPoint(n, p) 1271 for n, a in enumerate(axes): 1272 joint.setAxis(n, a) 1273 joint.setAbsolute(absolute) 1274 1275 q1 = ds1.q() 1276 q2 = None if ds2 is None else ds2.q() 1277 1278 joint.setBasePositions(q1, q2) 1279 1280 if allow_self_collide is not None: 1281 joint.setAllowSelfCollide(not not allow_self_collide) 1282 joint_nslaw = EqualityConditionNSL(joint.numberOfConstraints()) 1283 joint_inter = Interaction(joint_nslaw, joint) 1284 self._nsds.\ 1285 link(joint_inter, ds1, ds2) 1286 nsds.setName(joint_inter, str(name)) 1287 1288 # Add a e=0 joint by default, otherwise user can specify 1289 # the impact law by name or a list of names for each axis. 1290 if stops is not None: 1291 if np.shape(stops)[1] != 3: 1292 raise AssertionError("np.shape(stops)[1] != 3") 1293 #assert np.shape(stops)[1] == 3, 'Joint stops shape must be (?, 3)' 1294 if nslaws is None: 1295 nslaws = [sk.NewtonImpactNSL(0.0)] * np.shape(stops)[0] 1296 elif isinstance(nslaws, bytes): 1297 nslaws = [ 1298 self._nslaws[nslaws.decode('utf-8')]] * np.shape(stops)[0] 1299 elif isinstance(nslaws, str): 1300 nslaws = [self._nslaws[nslaws]] * np.shape(stops)[0] 1301 else: 1302 if np.shape(nslaws)[0] != np.shape(stops)[0]: 1303 raise AssertionError("np.shape(nslaws)[0] != np.shape(stops)[0]") 1304 #assert(np.shape(nslaws)[0] == np.shape(stops)[0]) 1305 nslaws = [self._nslaws[nsl] for nsl in nslaws] 1306 for n, (nsl, (axis, pos, _dir)) in enumerate(zip(nslaws, stops)): 1307 # "bool()" is needed because type of _dir is 1308 # numpy.bool_, which SWIG doesn't handle well. 1309 stop = joints.JointStopR(joint, pos, bool(_dir < 0), 1310 int(axis)) 1311 stop_inter = Interaction(nsl, stop) 1312 self._nsds.\ 1313 link(stop_inter, ds1, ds2) 1314 nsds.setName(stop_inter, '%s_stop%d' % (str(name), n)) 1315 1316 # The per-axis friction NSL, can be '' 1317 if friction is not None: 1318 if isinstance(friction, str): 1319 friction = [friction] 1320 elif isinstance(friction, bytes): 1321 friction = [friction.decode('utf-8')] 1322 else: 1323 friction = [(f.decode('utf-8') if isinstance(f, bytes) else f) 1324 for f in friction] 1325 for ax, fr_nslaw in enumerate(friction): 1326 if fr_nslaw == '': # no None in hdf5, use empty string 1327 continue # instead for no NSL on an axis 1328 nslaw = self._nslaws[fr_nslaw] 1329 fr = joints.JointFrictionR(joint, [ax]) 1330 fr_inter = Interaction(nslaw, fr) 1331 self._nsds.\ 1332 link(fr_inter, ds1, ds2) 1333 nsds.setName(fr_inter, '%s_friction%d' % (str(name), ax)) 1334 1335 # An array of tuples (dof1, dof2, ratio) specifies 1336 # coupling between a joint's DoFs (e.g., to turn a 1337 # cylindrical joint into a screw joint) 1338 if coupled is not None: 1339 if len(coupled.shape) == 1: 1340 coupled = np.array([coupled]) 1341 if coupled.shape[1] != 3 : 1342 raise AssertionError("coupled.shape[1] != 3") 1343 #assert(coupled.shape[1] == 3) 1344 for n, (dof1, dof2, ratio) in enumerate(coupled): 1345 cpl = joints.CouplerJointR(joint, int(dof1), 1346 joint, int(dof2), ratio) 1347 cpl.setBasePositions(q1, q2) 1348 cpl_inter = Interaction(EqualityConditionNSL(1), cpl) 1349 self._nsds.\ 1350 link(cpl_inter, ds1, ds2) 1351 nsds.setName(cpl_inter, '%s_coupler%d' % (str(name), n)) 1352 1353 def import_boundary_conditions(self, name): 1354 if self._interman is not None: 1355 topo = self._nsds.\ 1356 topology() 1357 1358 bc_type = self.boundary_conditions()[name].attrs['type'] 1359 bc_class = getattr(sk, bc_type) 1360 1361 ds1_name = self.boundary_conditions()[name].attrs['object1'] 1362 ds1 = topo.getDynamicalSystem(ds1_name) 1363 1364 if bc_type == 'HarmonicBC': 1365 bc = bc_class( 1366 self.boundary_conditions()[name].attrs['indices'], 1367 self.boundary_conditions()[name].attrs['a'], 1368 self.boundary_conditions()[name].attrs['b'], 1369 self.boundary_conditions()[name].attrs['omega'], 1370 self.boundary_conditions()[name].attrs['phi']) 1371 1372 elif bc_type == 'FixedBC': 1373 bc = bc_class( 1374 self.boundary_conditions()[name].attrs['indices']) 1375 1376 elif(bc_type == 'BoundaryCondition'): 1377 bc = bc_class( 1378 self.boundary_conditions()[name].attrs['indices'], 1379 self.boundary_conditions()[name].attrs['v']) 1380 1381 # set bc to the ds1 1382 1383 ds1.setBoundaryConditions(bc) 1384 1385 #joint_inter = Interaction(joint_nslaw, joint) 1386 # self._nsds.\ 1387 # link(joint_inter, ds1) 1388 1389 def import_permanent_interactions(self, name): 1390 """ 1391 """ 1392 if self._interman is not None and 'input' in self._data and\ 1393 self.permanent_interactions() is not None: 1394 topo = self._nsds.topology() 1395 1396 pinter = self.permanent_interactions()[name] 1397 body1_name = pinter.attrs['body1_name'] 1398 body2_name = pinter.attrs['body2_name'] 1399 1400 try: 1401 ds1 = sk.cast_NewtonEulerDS( 1402 topo.getDynamicalSystem(body1_name)) 1403 except Exception: 1404 ds1 = None 1405 1406 try: 1407 ds2 = sk.cast_NewtonEulerDS( 1408 topo.getDynamicalSystem(body2_name)) 1409 except Exception: 1410 ds2 = None 1411 1412 # static object in second 1413 if ds1 is None: 1414 ds2, ds1 = ds1, ds2 1415 1416 contactor1_name = pinter.attrs.get('contactor1_name', None) 1417 contactor2_name = pinter.attrs.get('contactor2_name', None) 1418 1419 if contactor1_name is None: 1420 contactor1_names = self._occ_contactors[body1_name].keys() 1421 else: 1422 contactor1_names = [contactor1_name] 1423 1424 if contactor2_name is None: 1425 contactor2_names = self._occ_contactors[body2_name].keys() 1426 else: 1427 contactor2_names = [contactor2_name] 1428 1429 distance_calculator = pinter.attrs['distance_calculator'] 1430 offset1 = pinter.attrs['offset1'] 1431 offset2 = pinter.attrs['offset2'] 1432 1433 body1 = self._input[body1_name] 1434 body2 = self._input[body2_name] 1435 1436 real_dist_calc = {'cadmbtb': occ.CadmbtbDistanceType, 1437 'occ': occ.OccDistanceType} 1438 1439 for contactor1_name in contactor1_names: 1440 for contactor2_name in contactor2_names: 1441 ctr1 = body1[contactor1_name] 1442 ctr2 = body2[contactor2_name] 1443 1444 cg1 = int(ctr1.attrs['group']) 1445 cg2 = int(ctr2.attrs['group']) 1446 nslaw = self._interman.nonSmoothLaw(cg1, cg2) 1447 1448 cocs1 = self._occ_contactors[body1_name][contactor1_name] 1449 cocs2 = self._occ_contactors[body2_name][contactor2_name] 1450 1451 if ds2 is None: 1452 self.print_verbose('moving contactor {0} of static object {1} to {2}'.format(contactor2_name, body2_name, list(np.array(body2.attrs['translation']) +\ 1453 np.array(ctr2.attrs['translation'])) +\ 1454 list(quaternion_multiply(ctr2.attrs['orientation'], 1455 body2.attrs['orientation'])))) 1456 occ.occ_move(cocs2.data(), 1457 list(np.array(body2.attrs['translation']) +\ 1458 np.array(ctr2.attrs['translation'])) +\ 1459 list(quaternion_multiply(ctr2.attrs['orientation'], 1460 body2.attrs['orientation']))) 1461 1462 cp1 = occ.ContactPoint(cocs1) 1463 cp2 = occ.ContactPoint(cocs2) 1464 1465 relation = occ.OccR(cp1, cp2, 1466 real_dist_calc[distance_calculator]()) 1467 1468 relation.setOffset1(offset1) 1469 relation.setOffset2(offset2) 1470 1471 inter = Interaction(nslaw, relation) 1472 1473 if ds2 is not None: 1474 self._nsds.link(inter, ds1, ds2) 1475 else: 1476 self._nsds.link(inter, ds1) 1477 1478 # keep pointers 1479 self._keep.append([cocs1, cocs2, cp1, 1480 cp2, relation]) 1481 1482 def import_object(self, name, body_class=None, shape_class=None, 1483 face_class=None, edge_class=None, birth=False, 1484 translation=None, orientation=None, velocity=None): 1485 """Import an object by name, 1486 possibly overriding initial position and velocity. 1487 """ 1488 obj = self._input[name] 1489 self.print_verbose ('Import object name:', name) 1490 self.print_verbose (' number (id): {0} '.format(obj.attrs['id'])) 1491 1492 if translation is None: 1493 translation = obj.attrs['translation'] 1494 if orientation is None: 1495 orientation = obj.attrs['orientation'] 1496 if velocity is None: 1497 velocity = obj.attrs['velocity'] 1498 1499 # bodyframe center of mass 1500 center_of_mass = floatv(obj.attrs.get('center_of_mass', [0, 0, 0])) 1501 1502 mass = obj.attrs.get('mass', None) 1503 inertia = obj.attrs.get('inertia', None) 1504 1505 if mass is None: 1506 self.print_verbose (' static object') 1507 self.print_verbose (' position', list(translation) + list(orientation)) 1508 else: 1509 self.print_verbose (' dynamic object') 1510 self.print_verbose (' position', list(translation) + list(orientation)) 1511 self.print_verbose (' velocity', velocity) 1512 1513 # self.print_verbose('mass = ', mass) 1514 # self.print_verbose('inertia = ', inertia) 1515 1516 input_ctrs = [ctr for _n_, ctr in obj.items()] 1517 1518 contactors = [] 1519 occ_type = False 1520 1521 for ctr in input_ctrs: 1522 1523 if 'type' in ctr.attrs: 1524 # occ contact 1525 occ_type = True 1526 contactors.append( 1527 Contactor( 1528 instance_name=ctr.attrs['instance_name'], 1529 shape_name=ctr.attrs['shape_name'], 1530 collision_group=ctr.attrs['group'].astype(int), 1531 contact_type=ctr.attrs['type'], 1532 contact_index=ctr.attrs['contact_index'].astype(int), 1533 relative_translation=np.subtract(ctr.attrs['translation'].astype(float), 1534 center_of_mass), 1535 relative_orientation=ctr.attrs['orientation'].astype(float))) 1536 elif 'group' in ctr.attrs: 1537 # bullet contact 1538 if occ_type : 1539 raise AssertionError("occ type is found") 1540 #assert not occ_type 1541 contactors.append( 1542 Contactor( 1543 instance_name=ctr.attrs['instance_name'], 1544 shape_name=ctr.attrs['shape_name'], 1545 collision_group=ctr.attrs['group'].astype(int), 1546 relative_translation=np.subtract(ctr.attrs['translation'].astype(float), center_of_mass), 1547 relative_orientation=ctr.attrs['orientation'].astype(float))) 1548 else: 1549 # occ shape 1550 occ_type = True 1551 # fix: not only contactors here 1552 contactors.append( 1553 Shape( 1554 instance_name=ctr.attrs['instance_name'], 1555 shape_name=ctr.attrs['shape_name'], 1556 relative_translation=np.subtract(ctr.attrs['translation'].astype(float), center_of_mass), 1557 relative_orientation=ctr.attrs['orientation'].astype(float))) 1558 1559 if occ_type: 1560 # Occ object 1561 body, flag = self.import_occ_object( 1562 name, floatv(translation), floatv(orientation), 1563 floatv(velocity), contactors, mass, 1564 inertia, body_class, shape_class, face_class, 1565 edge_class, birth=birth, 1566 number=self.instances()[name].attrs['id']) 1567 else: 1568 # Bullet object 1569 body, flag = self.import_bullet_object( 1570 name, floatv(translation), floatv(orientation), 1571 floatv(velocity), contactors, mass, 1572 inertia, body_class, shape_class, birth=birth, 1573 number=self.instances()[name].attrs['id']) 1574 1575 # schedule its death immediately 1576 time_of_death = obj.attrs.get('time_of_death', None) 1577 if time_of_death is not None : 1578 bisect.insort_left(self._scheduled_deaths, time_of_death) 1579 if time_of_death in self._deaths: 1580 self._deaths[time_of_death].append((name, obj, body, flag)) 1581 else: 1582 self._deaths[time_of_death] = [(name, obj, body, flag)] 1583 1584 def import_scene(self, time, body_class, shape_class, face_class, 1585 edge_class): 1586 """ 1587 From the specification given in the hdf5 file with the help of 1588 add* functions, import into the NSDS: 1589 - the static objects 1590 - the dynamic objects 1591 - the joints 1592 and into the interaction_manager: 1593 - the nonsmooth laws 1594 that have a specified time of birth <= current time. 1595 """ 1596 1597 # Ensure we count up from zero for implicit DS numbering 1598 DynamicalSystem.resetCount(0) 1599 1600 for _ in self._ref: 1601 self._number_of_shapes += 1 1602 1603 # import dynamical systems 1604 if self._interman is not None and 'input' in self._data: 1605 1606 # get pointers 1607 dpos_data = self.dynamic_data() 1608 velocities = self.velocities_data() 1609 1610 # some dict to prefetch values in order to 1611 # speedup cold start in the case of many objects 1612 xdpos_data = dict() 1613 xvelocities = dict() 1614 1615 if dpos_data is not None and len(dpos_data) > 0: 1616 1617 max_time = max(dpos_data[:, 0]) 1618 id_last = np.where( 1619 abs(dpos_data[:, 0] - max_time) < 1e-9)[0] 1620 id_vlast = np.where(abs(velocities[:, 0] - max_time) < 1e-9)[0] 1621 1622 # prefetch positions and velocities in dictionaries 1623 # this avoids calls to np.where for each object 1624 for ldpos_data in dpos_data[id_last, :]: 1625 xdpos_data[ldpos_data[1]] = ldpos_data 1626 for lvelocities in velocities[id_vlast, :]: 1627 xvelocities[lvelocities[1]] = lvelocities 1628 1629 else: 1630 # should not be used 1631 max_time = None 1632 id_last = None 1633 self.print_verbose('import dynamical systems ...') 1634 for (name, obj) in sorted(self._input.items(), 1635 key=lambda x: x[0]): 1636 1637 mass = obj.attrs.get('mass', None) 1638 time_of_birth = obj.attrs.get('time_of_birth', -1) 1639 1640 if time_of_birth >= time: 1641 # 1642 # in the future 1643 # 1644 bisect.insort_left(self._scheduled_births, time_of_birth) 1645 if time_of_birth in self._births: 1646 self._births[time_of_birth].append((name, obj)) 1647 else: 1648 self._births[time_of_birth] = [(name, obj)] 1649 else: 1650 # 1651 # this is for now 1652 # 1653 # cold restart if output previously done 1654 1655 if mass is not None and dpos_data is not None and\ 1656 len(dpos_data) > 0: 1657 1658 xpos = xdpos_data[obj.attrs['id']] 1659 translation = (xpos[2], xpos[3], xpos[4]) 1660 orientation = (xpos[5], xpos[6], xpos[7], xpos[8]) 1661 1662 xvel = xvelocities[obj.attrs['id']] 1663 velocity = (xvel[2], xvel[3], xvel[4], 1664 xvel[5], xvel[6], xvel[7]) 1665 1666 else: 1667 # start from initial conditions 1668 1669 translation = obj.attrs['translation'] 1670 orientation = obj.attrs['orientation'] 1671 velocity = obj.attrs['velocity'] 1672 1673 self.import_object(name=name, body_class=body_class, 1674 shape_class=shape_class, 1675 face_class=face_class, 1676 edge_class=edge_class, 1677 translation=translation, 1678 orientation=orientation, 1679 velocity=velocity, 1680 birth=False) 1681 1682 # import nslaws 1683 # note: no time of birth for nslaws and joints 1684 self.print_verbose('import nslaws ...') 1685 for name in self._nslaws_data: 1686 self.import_nonsmooth_law(name) 1687 1688 self.print_verbose('import joints ...') 1689 for name in self.joints(): 1690 self.import_joint(name) 1691 1692 self.print_verbose('import boundary conditions ...') 1693 for name in self.boundary_conditions(): 1694 self.import_boundary_conditions(name) 1695 1696 self.print_verbose('import permanent interactions ...') 1697 for name in self.permanent_interactions(): 1698 self.import_permanent_interactions(name) 1699 1700 def current_time(self): 1701 if self._initializing: 1702 return self._simulation.startingTime() 1703 else: 1704 return self._simulation.nextTime() 1705 1706 def import_births(self, body_class=None, shape_class=None, 1707 face_class=None, edge_class=None,): 1708 """ 1709 Import new objects into the NSDS. 1710 """ 1711 time = self.current_time() 1712 1713 ind_time = bisect.bisect_right(self._scheduled_births, time) 1714 1715 current_times_of_births = set(self._scheduled_births[:ind_time]) 1716 self._scheduled_births = self._scheduled_births[ind_time:] 1717 1718 for time_of_birth in current_times_of_births: 1719 for (name, _) in self._births[time_of_birth]: 1720 1721 self.import_object(name, body_class, shape_class, 1722 face_class, edge_class, birth=True) 1723 1724 1725 def execute_deaths(self): 1726 """ 1727 Remove objects from the NSDS 1728 """ 1729 time = self.current_time() 1730 1731 ind_time = bisect.bisect_right(self._scheduled_deaths, time) 1732 1733 current_times_of_deaths = set(self._scheduled_deaths[:ind_time]) 1734 1735 self._scheduled_deaths = self._scheduled_deaths[ind_time:] 1736 1737 for time_of_death in current_times_of_deaths: 1738 print(self._deaths[time_of_death]) 1739 for ( _, _, body, flag) in self._deaths[time_of_death]: 1740 if flag == 'static': 1741 self._interman.removeStaticContactorSet(body) 1742 elif flag == 'dynamic': 1743 self._interman.removeBody(body) 1744 self._nsds.removeDynamicalSystem(body) 1745 else: 1746 msg = 'execute_deaths : unknown object type' 1747 msg += 'It should static or dynamic' 1748 raise RuntimeError(msg) 1749 1750 def output_static_objects(self): 1751 """ 1752 Outputs translations and orientations of static objects 1753 """ 1754 time = self.current_time() 1755 p = 0 1756 self._static_data.resize(len(self._static), 0) 1757 1758 for static in self._static.values(): 1759 translation = static['origin'] 1760 rotation = static['orientation'] 1761 1762 if self._dimension == 3: 1763 self._static_data[p, :] = \ 1764 [time, 1765 static['number'], 1766 translation[0], 1767 translation[1], 1768 translation[2], 1769 rotation[0], 1770 rotation[1], 1771 rotation[2], 1772 rotation[3]] 1773 1774 elif self._dimension == 2: 1775 # VA. change the position such that is corresponds to a 3D object 1776 self._static_data[p, :] = \ 1777 [time, 1778 static['number'], 1779 translation[0], 1780 translation[1], 1781 0.0, 1782 cos(rotation[0] / 2.0), 1783 0.0, 1784 0.0, 1785 sin(rotation[0] / 2.0)] 1786 1787 p += 1 1788 1789 1790 def output_dynamic_objects(self, initial=False): 1791 """ 1792 Outputs translations and orientations of dynamic objects. 1793 """ 1794 1795 current_line = self._dynamic_data.shape[0] 1796 1797 time = self.current_time() 1798 1799 positions = self._io.positions(self._nsds) 1800 if positions is not None: 1801 self._dynamic_data.resize(current_line + positions.shape[0], 0) 1802 1803 times = np.empty((positions.shape[0], 1)) 1804 times.fill(time) 1805 if self._dimension == 3: 1806 self._dynamic_data[current_line:, :] = np.concatenate( 1807 (times, positions), axis=1) 1808 elif self._dimension == 2: 1809 # VA. change the position such that is corresponds to a 3D object 1810 new_positions = np.zeros((positions.shape[0], 8)) 1811 1812 new_positions[:, 0] = positions[:, 0] # ds number 1813 new_positions[:, 1] = positions[:, 1] # x position 1814 new_positions[:, 2] = positions[:, 2] # y position 1815 1816 new_positions[:, 4] = np.cos(positions[:, 3] / 2.0) 1817 new_positions[:, 7] = np.sin(positions[:, 3] / 2.0) 1818 self._dynamic_data[current_line:, :] = np.concatenate( 1819 (times, new_positions), axis=1) 1820 1821 def output_velocities(self): 1822 """ 1823 Output velocities of dynamic objects 1824 """ 1825 1826 current_line = self._dynamic_data.shape[0] 1827 1828 time = self.current_time() 1829 1830 velocities = self._io.velocities(self._nsds) 1831 1832 if velocities is not None: 1833 1834 self._velocities_data.resize(current_line + velocities.shape[0], 0) 1835 1836 times = np.empty((velocities.shape[0], 1)) 1837 times.fill(time) 1838 if self._dimension == 3: 1839 self._velocities_data[current_line:, :] = np.concatenate( 1840 (times, velocities), axis=1) 1841 elif self._dimension == 2: 1842 # VA. change the position such that is corresponds to a 3D object 1843 new_velocities = np.zeros((velocities.shape[0], 7)) 1844 1845 new_velocities[:, 0] = velocities[:, 0] # ds number 1846 new_velocities[:, 1] = velocities[:, 1] # x velocity 1847 new_velocities[:, 2] = velocities[:, 2] # y velocity 1848 1849 new_velocities[:, 6] = velocities[:, 3] # theta velocity 1850 self._velocities_data[current_line:, :] = np.concatenate( 1851 (times, new_velocities), axis=1) 1852 1853 def output_contact_forces(self): 1854 """ 1855 Outputs contact forces 1856 _contact_index_set default value is 1. 1857 """ 1858 if self._nsds.\ 1859 topology().indexSetsSize() > 1: 1860 time = self.current_time() 1861 contact_points = self._io.contactPoints(self._nsds, 1862 self._contact_index_set) 1863 if contact_points is not None: 1864 current_line = self._cf_data.shape[0] 1865 # Increase the number of lines in cf_data 1866 # (h5 dataset with chunks) 1867 self._cf_data.resize(current_line + contact_points.shape[0], 0) 1868 times = np.empty((contact_points.shape[0], 1)) 1869 times.fill(time) 1870 1871 if self._dimension == 3: 1872 new_contact_points = np.zeros( 1873 (contact_points.shape[0], 25)) 1874 1875 new_contact_points[:, 0] = contact_points[:, 0] # mu 1876 new_contact_points[:, 1] = contact_points[:, 1] # posa 1877 new_contact_points[:, 2] = contact_points[:, 2] 1878 #new_contact_points[:, 3] 1879 new_contact_points[:, 4] = contact_points[:, 3] # posb 1880 new_contact_points[:, 5] = contact_points[:, 4] 1881 #new_contact_points[:, 6] 1882 new_contact_points[:, 7] = contact_points[:, 5] # nc 1883 new_contact_points[:, 8] = contact_points[:, 6] 1884 #new_contact_points[:, 9] 1885 new_contact_points[:, 10] = contact_points[:, 7] # cf 1886 new_contact_points[:, 11] = contact_points[:, 8] 1887 #new_contact_points[:, 12] 1888 new_contact_points[:, 13] = contact_points[:, 9] # gap 1889 new_contact_points[:, 14] = contact_points[:, 10] 1890 #new_contact_points[:, 15] 1891 # relative velocity 1892 new_contact_points[:, 16] = contact_points[:, 11] 1893 new_contact_points[:, 17] = contact_points[:, 12] 1894 #new_contact_points[:, 18] 1895 # reaction impulse 1896 new_contact_points[:, 19] = contact_points[:, 13] 1897 new_contact_points[:, 20] = contact_points[:, 14] 1898 #new_contact_points[:, 21] 1899 # inter id 1900 new_contact_points[:, 22] = contact_points[:, 15] 1901 new_contact_points[:, 23] = contact_points[:, 16] # ds 1 1902 new_contact_points[:, 24] = contact_points[:, 17] # ds 2 1903 1904 self._cf_data[current_line:, :] = \ 1905 np.concatenate((times, 1906 contact_points), 1907 axis=1) 1908 1909 elif self._dimension == 2: 1910 1911 # VA. change the contact info such that is corresponds to a 3D object 1912 new_contact_points = np.zeros( 1913 (contact_points.shape[0], 25)) 1914 1915 new_contact_points[:, 0] = contact_points[:, 0] # mu 1916 new_contact_points[:, 1] = contact_points[:, 1] # posa 1917 new_contact_points[:, 2] = contact_points[:, 2] 1918 #new_contact_points[:, 3] 1919 new_contact_points[:, 4] = contact_points[:, 3] # posb 1920 new_contact_points[:, 5] = contact_points[:, 4] 1921 #new_contact_points[:, 6] 1922 new_contact_points[:, 7] = contact_points[:, 5] # nc 1923 new_contact_points[:, 8] = contact_points[:, 6] 1924 #new_contact_points[:, 9] 1925 # cf 1926 new_contact_points[:, 10] = contact_points[:, 7] 1927 new_contact_points[:, 11] = contact_points[:, 8] 1928 #new_contact_points[:, 12] 1929 new_contact_points[:, 13] = contact_points[:, 9] # gap 1930 new_contact_points[:, 14] = contact_points[:, 10] 1931 #new_contact_points[:, 15] 1932 # relative velocity 1933 new_contact_points[:, 16] = contact_points[:, 11] 1934 new_contact_points[:, 17] = contact_points[:, 12] 1935 #new_contact_points[:, 18] 1936 # reaction impulse 1937 new_contact_points[:, 19] = contact_points[:, 13] 1938 new_contact_points[:, 20] = contact_points[:, 14] 1939 #new_contact_points[:, 21] 1940 # inter id 1941 new_contact_points[:, 22] = contact_points[:, 15] 1942 new_contact_points[:, 23] = contact_points[:, 16] # ds 1 1943 new_contact_points[:, 24] = contact_points[:, 17] # ds 2 1944 self._cf_data[current_line:, :] = np.concatenate( 1945 (times, new_contact_points), axis=1) 1946 1947 # return the number of contacts 1948 return len(contact_points) 1949 return 0 1950 return 0 1951 1952 def output_domains(self): 1953 """ 1954 Outputs domains of contact points 1955 """ 1956 if self._nsds.\ 1957 topology().indexSetsSize() > 1: 1958 time = self.current_time() 1959 domains = self._io.domains(self._nsds) 1960 1961 if domains is not None: 1962 1963 current_line = self._domain_data.shape[0] 1964 self._domain_data.resize(current_line + domains.shape[0], 0) 1965 times = np.empty((domains.shape[0], 1)) 1966 times.fill(time) 1967 1968 self._domain_data[current_line:, :] = \ 1969 np.concatenate((times, domains), axis=1) 1970 1971 def output_solver_infos(self): 1972 """ 1973 Outputs solver #iterations & precision reached 1974 """ 1975 1976 time = self.current_time() 1977 so = self._simulation.oneStepNSProblem(0).numericsSolverOptions() 1978 1979 current_line = self._solv_data.shape[0] 1980 self._solv_data.resize(current_line + 1, 0) 1981 iterations = so.iparam[sn.SICONOS_IPARAM_ITER_DONE] 1982 precision = so.dparam[sn.SICONOS_DPARAM_RESIDU] 1983 if so.solverId == sn.SICONOS_GENERIC_MECHANICAL_NSGS: 1984 local_precision = so.dparam[3] # Check this ! 1985 elif so.solverId == sn.SICONOS_FRICTION_3D_NSGS: 1986 local_precision = 0. 1987 else: 1988 local_precision = precision 1989 1990 self._solv_data[current_line, :] = [time, iterations, precision, 1991 local_precision] 1992 1993 def print_solver_infos(self): 1994 """ 1995 Outputs solver #iterations & precision reached 1996 """ 1997 time = self.current_time() 1998 so = self._simulation.oneStepNSProblem(0).numericsSolverOptions() 1999 iterations = so.iparam[sn.SICONOS_IPARAM_ITER_DONE] 2000 precision = so.dparam[sn.SICONOS_DPARAM_RESIDU] 2001 msg = 'Numerics solver info at time : {0:10.6f}'.format(time) 2002 msg += ' iterations = {0:8d}'.format(iterations) 2003 msg += ' precision = {0:5.3e}'.format(precision) 2004 self.print_verbose(msg) 2005 2006 def import_plugins(self): 2007 """ 2008 Plugins extraction and compilation. 2009 """ 2010 import subprocess 2011 2012 for name in self._plugins: 2013 2014 plugin_src = self._plugins[name][:] 2015 plugin_fname = self._plugins[name].attrs['filename'] 2016 2017 if os.path.isfile(plugin_fname): 2018 if str_of_file(plugin_fname) != plugin_src: 2019 warn('plugin {0}: file {1} differs from the one stored hdf5'.format(name, plugin_fname)) 2020 else: 2021 file_of_str(plugin_fname, plugin_src) 2022 2023 # build 2024 subprocess.check_call(['siconos', '--build-plugins']) 2025 2026 def import_external_functions(self): 2027 topo = self._nsds.topology() 2028 2029 for name in self._external_functions: 2030 2031 ext_fun = self._external_functions[name] 2032 plugin_name = ext_fun.attrs['plugin_name'] 2033 plugin_function_name = ext_fun.attrs['plugin_function_name'] 2034 body_name = ext_fun.attrs['body_name'] 2035 2036 ds = sk.cast_NewtonEulerDS(topo.getDynamicalSystem(body_name)) 2037 2038 if 'function_name' in ext_fun.attrs: 2039 function_name = ext_fun.attrs['function_name'] 2040 2041 getattr(ds, function_name)(plugin_name, 2042 plugin_function_name) 2043 else: 2044 bc_indices = ext_fun.attrs['bc_indices'] 2045 # a boundary condition 2046 bc = sk.BoundaryCondition(bc_indices) 2047 bc.setComputePrescribedVelocityFunction(plugin_name, 2048 plugin_function_name) 2049 ds.setBoundaryConditions(bc) 2050 2051 def explode_Newton_solve(self, with_timer): 2052 s = self._simulation 2053 self.log(s.initialize, with_timer)() 2054 self.log(s.resetLambdas, with_timer)() 2055 # Again the access to s._newtonTolerance generates a segfault due to director 2056 #newtonTolerance = s.newtonTolerance() 2057 newtonMaxIteration = s.newtonMaxIteration() 2058 2059 # return _kernel.TimeStepping_newtonSolve(self, criterion, maxStep) 2060 # RuntimeError: accessing protected member newtonSolve 2061 #s.newtonSolve(newtonTolerance, newtonMaxIteration); 2062 2063 newtonNbIterations = 0 2064 isNewtonConverge = False 2065 explode_computeOneStep = False 2066 2067 self.log(s.initializeNewtonLoop, with_timer)() 2068 while (not isNewtonConverge) and newtonNbIterations < newtonMaxIteration: 2069 #self.print_verbose('newtonNbIterations',newtonNbIterations) 2070 info = 0 2071 newtonNbIterations = newtonNbIterations + 1 2072 self.log(s.prepareNewtonIteration, with_timer)() 2073 self.log(s.computeFreeState, with_timer)() 2074 if s.numberOfOSNSProblems() > 0: 2075 if explode_computeOneStep: 2076 # experimental 2077 osnsp = s.oneStepNSProblem(SICONOS_OSNSP_TS_VELOCITY) 2078 #info = self.log(osnsp.compute, with_timer)(s.nextTime()) 2079 fc = cast_FrictionContact(osnsp) 2080 #self.log(fc.updateInteractionBlocks, with_timer)() 2081 self.log(fc.preCompute, with_timer)(s.nextTime()) 2082 self.log(fc.updateMu, with_timer)() 2083 info = self.log(fc.solve, with_timer)() 2084 self.log(fc.postCompute, with_timer)() 2085 else: 2086 info = self.log(s.computeOneStepNSProblem, with_timer)(SICONOS_OSNSP_TS_VELOCITY) 2087 self.log(s.DefaultCheckSolverOutput, with_timer)(info) 2088 self.log(s.updateInput, with_timer)() 2089 self.log(s.updateState, with_timer)() 2090 if (not isNewtonConverge) and (newtonNbIterations < newtonMaxIteration): 2091 self.log(s.updateOutput, with_timer)() 2092 isNewtonConverge = self.log(s.newtonCheckConvergence, with_timer) 2093 if s.displayNewtonConvergence(): 2094 s.displayNewtonConvergenceInTheLoop() 2095 if (not isNewtonConverge) and (not info): 2096 if s.numberOfOSNSProblems() > 0: 2097 self.log(s.saveYandLambdaInOldVariables, with_timer)() 2098 if s.displayNewtonConvergence(): 2099 s.displayNewtonConvergenceAtTheEnd(info, newtonMaxIteration) 2100 2101 def run(self, 2102 with_timer=False, 2103 time_stepping=None, 2104 interaction_manager=None, 2105 bullet_options=None, 2106 body_class=None, 2107 shape_class=None, 2108 face_class=None, 2109 edge_class=None, 2110 controller=None, 2111 gravity_scale=1.0, 2112 t0=0, 2113 T=10, 2114 h=0.0005, 2115 multipoints_iterations=None, 2116 theta=0.50001, 2117 Newton_options=sk.SICONOS_TS_NONLINEAR, 2118 Newton_max_iter=20, 2119 set_external_forces=None, 2120 solver_options=None, 2121 solver_options_pos=None, 2122 osnspb_max_size=0, 2123 exit_tolerance=None, 2124 projection_itermax=20, 2125 projection_tolerance=1e-8, 2126 projection_tolerance_unilateral=1e-8, 2127 numerics_verbose=False, 2128 numerics_verbose_level=0, 2129 violation_verbose=False, 2130 verbose=True, 2131 verbose_progress=True, 2132 output_frequency=None, 2133 output_backup=False, 2134 output_backup_frequency=None, 2135 friction_contact_trace_params=None, 2136 contact_index_set=1, 2137 osi=sk.MoreauJeanOSI, 2138 constraint_activation_threshold=0.0, 2139 explode_Newton_solve=False, 2140 display_Newton_convergence=False, 2141 start_run_iteration_hook=None, 2142 end_run_iteration_hook=None 2143 ): 2144 """Run a simulation from a set of parameters described in a hdf5 file. 2145 2146 Parameters 2147 ---------- 2148 2149 with_timer: boolean, optional 2150 if true, use a timer for log output (default False) 2151 time_stepping: siconos.kernel.Simulation, optional 2152 simulation type, default = sk.TimeStepping 2153 interaction_manager: SiconosCollisionManager, optional 2154 user-defined interaction handler (e.g. from Bullet), default=None 2155 (depends on the backend, e.g. Bullet or OCC). 2156 Warning: overwrite the value 2157 provided during MechanicsHdf5Runner init. 2158 bullet_options: ?, optional 2159 set of options for the interaction manager 2160 (e.g. SiconosBulletOptions), default = None 2161 body_class: siconos.mechanics.RigidBodyDS and heirs, optional 2162 class used for body definition, default = RigidBodyDS 2163 shape_class: ?, optional 2164 class used for shape definition (e.g. occ.OccContactShape) 2165 default = None 2166 face_class: ?, optional 2167 class used for face definition (e.g. occ.OccContactFace) 2168 default = None, (occ only?) 2169 edge_class=None, 2170 class used for edge definition (e.g. occ.OccContactEdge) 2171 default = None, (occ only?) 2172 controller: user-defined class, optional 2173 To apply user defined functions onto the nsds members. 2174 default = None, see example in wheels.py. 2175 The user-defined class must have two methods: 2176 2177 initialize(self, MechanicsHdf5Runner) 2178 step() 2179 gravity_scale : int, optional 2180 multiplication factor for the gravity. 2181 1. for meters (default). 2182 1./100 for centimeters. 2183 This parameter may be needed for small 2184 objects because of Bullet collision margin (0.04). 2185 t0: real, optional 2186 starting time (default 0) 2187 T: real, optional 2188 end time (default 10) 2189 h: real, optional 2190 time step size (default 0.0005) 2191 multiPoint_iterations : boolean, optional 2192 if true (default) use bullet "multipoint iterations" 2193 theta : real, optional 2194 parameter for Moreau-Jean OSI (default 0.50001) 2195 Newton_options: int, optional 2196 sk.TimeStepping options to control the Newton loop 2197 (default sk.SICONOS_TS_NONLINEAR) 2198 Newton_max_iter : int, optional 2199 maximum number of iterations allowed for Newton integrator 2200 (default = 20). 2201 set_external_forces: python function, optional 2202 function used to apply forces onto the body. 2203 Must be : 2204 2205 def funcname(body) 2206 2207 body being a siconos Body (DS) 2208 Default : apply gravity forces. 2209 2210 Note FP : already set in __enter__. Overwrite? 2211 2212 solver_options : numerics SolverOptions, optional 2213 OneStepNsProblem solver options set. 2214 if solver_option is None, we leave Siconos/kernel choosing the default option 2215 (see solvers documentation for details) 2216 2217 solver_options_pos : numerics SolverOptions for the position projection, optional 2218 OneStepNsProblem solver options set. 2219 if solver_option is None, we leave Siconos/kernel choosing the default option 2220 (see solvers documentation for details) 2221 2222 osnspb_max_size : int, optional 2223 estimation of the maximum number of dynamical systems taken 2224 into account. 2225 Useful for memory pre-allocations and optimisation. 2226 if equal to 0 (default), it will be set to 2227 simulation().nonSmoothDynamicalSystem().topology().numberOfConstraints() 2228 2229 exit_tolerance : real, optional 2230 if not None, the simulation will stop if precision >= exit_tolerance 2231 (default None). 2232 2233 projection_itermax: int, optional 2234 max number of iteration for projection 2235 (only for TimeSteppingDirectProjection) 2236 default = 20 2237 projection_tolerance: real, optional 2238 tolerance for the violation of the equality constraints 2239 at the position level (only for TimeSteppingDirectProjection) 2240 default = 1e-8 2241 projection_tolerance_unilateral=1e-8, 2242 tolerance for the violation of the unilateral constraints 2243 at the position level (only for TimeSteppingDirectProjection) 2244 default = 1e-8 2245 2246 numerics_verbose: boolean, optional 2247 True to activate numerics verbosity (default=False), 2248 numerics_verbose_level: int, optional, 2249 Set verbose level in numerics, default=0 2250 violation_verbose: boolean, optional 2251 If true, display information regarding constraint violation 2252 (if any), default=false 2253 verbose_progress: boolean, optional 2254 true to print current step informations (default=True) 2255 output_frequency : int, optional 2256 log and screen outputs frequency (default = 1) 2257 output_backup: boolean, optional 2258 True to backup hdf5 file (default false) 2259 output_backup_frequency: int, optional 2260 hdf5 file backup frequency (default = 1) 2261 friction_contact_trace_params: siconos.io.FrictionContactTraceParams, 2262 optional 2263 Set this to activate the wrapping of the one-step NS problem into 2264 FrictionContactTrace object. More log, more trace. 2265 Default = None. 2266 contact_index_set: int, optional 2267 number of the index set from which contact 2268 point information is retrieved. Default = 1 2269 osi: sk.OneStepIntegrator, optional 2270 class type used to describe one-step integration, 2271 default = sk.MoreauJeanOSI 2272 constraint_activation_threshold: real, optional 2273 threshold under which constraint is assume to be 2274 active. Default = 0.0, 2275 explode_Newton_solve: boolean, optional 2276 True to add more log/trace during Newton loop. Default=False, 2277 start_run_iteration_hook: boolean, optional 2278 if true, launch logging process at the beginning of each time step. 2279 Default = False. 2280 end_run_iteration_hook: boolean, optional 2281 if true, launch logging process at the end of each time step. 2282 Default = False. 2283 """ 2284 self.print_verbose('setup model simulation ...') 2285 if set_external_forces is not None: 2286 self._set_external_forces = set_external_forces 2287 2288 if interaction_manager is None: 2289 interaction_manager = default_manager_class 2290 2291 if time_stepping is None: 2292 time_stepping = default_simulation_class 2293 2294 if output_frequency is not None: 2295 self._output_frequency = output_frequency 2296 2297 if output_backup_frequency is not None: 2298 self._output_backup_frequency = output_backup_frequency 2299 2300 if output_backup is not None: 2301 self._output_backup = output_backup 2302 2303 if gravity_scale is not None: 2304 self._gravity_scale = gravity_scale 2305 2306 2307 2308 # cold restart 2309 times = set() 2310 if self.dynamic_data() is not None and len(self.dynamic_data()) > 0: 2311 dpos_data = self.dynamic_data() 2312 times = set(dpos_data[:, 0]) 2313 t0 = float(max(times)) 2314 2315 # Time-related parameters for this simulation run 2316 k0 = 1 + int(t0 / h) 2317 k = k0 2318 kT = k0 + int((T - t0) / h) 2319 if T > t0: 2320 self.print_verbose('') 2321 msg = 'Simulation will run from time {0:.4f} '.format(t0) 2322 msg += 'to {0:.4f}s, '.format(T) 2323 msg += 'step {} to step {} (h={}, '.format(k0, kT, h) 2324 msg += 'times=[{},{}])'.format( 2325 min(times) if len(times) > 0 else '?', 2326 max(times) if len(times) > 0 else '?') 2327 self.print_verbose(msg) 2328 self.print_verbose('') 2329 else: 2330 msg = 'Simulation time {0} >= T={1}, exiting.'.format(t0, T) 2331 self.print_verbose(msg) 2332 return 2333 2334 # get dimension 2335 self._dimension = self._out.attrs.get('dimension', 3) 2336 2337 # Respect run() parameter for multipoints_iterations for 2338 # backwards compatibility, but this is overridden by 2339 # SiconosBulletOptions if one is provided. 2340 if (multipoints_iterations is not None ) and (bullet_options is not None): 2341 msg = '[io.mechanics] run(): one cannot give multipoints_iterations and bullet_options simultaneously. \n' 2342 msg += ' multipoints_iterations will be marked as obsolete. use preferably bullet_options\n' 2343 msg += ' with bullet_options.perturbationIterations and bullet_options.minimumPointsPerturbationThreshold.' 2344 raise RuntimeError(msg) 2345 2346 if multipoints_iterations and bullet_options is None: 2347 bullet_options = SiconosBulletOptions() 2348 bullet_options.perturbationIterations = 3 * multipoints_iterations 2349 bullet_options.minimumPointsPerturbationThreshold = \ 2350 3 * multipoints_iterations 2351 2352 2353 if (self._dimension == 2): 2354 if bullet_options is None: 2355 bullet_options = SiconosBulletOptions() 2356 bullet_options.dimension = SICONOS_BULLET_2D 2357 2358 self._interman = interaction_manager(bullet_options) 2359 2360 joints = list(self.joints()) 2361 if hasattr(self._interman, 'useEqualityConstraints'): 2362 if len(joints) == 0: 2363 self._interman.useEqualityConstraints(False) 2364 else: 2365 self._interman.useEqualityConstraints(True) 2366 2367 # (0) NonSmooth Dynamical Systems definition 2368 self._nsds = sk.NonSmoothDynamicalSystem(t0, T) 2369 nsds = self._nsds 2370 2371 self.print_verbose('import scene ...') 2372 self.import_scene(t0, body_class, shape_class, face_class, edge_class) 2373 2374 self._contact_index_set = contact_index_set 2375 2376 # (1) OneStepIntegrators 2377 2378 self._osi = osi(theta) 2379 if (osi == sk.MoreauJeanOSI): 2380 self._osi.setConstraintActivationThreshold( 2381 constraint_activation_threshold) 2382 # (2) Time discretisation -- 2383 timedisc = sk.TimeDiscretisation(t0, h) 2384 2385 # (3) choice of default OneStepNonSmoothProblem 2386 # w.r.t the type of nslaws 2387 nslaw_type_list = [] 2388 for name in self._nslaws_data: 2389 nslaw_type_list.append(self._nslaws_data[name].attrs['type']) 2390 2391 #print(set(nslaw_type_list)) 2392 2393 # This trick is used to add the EqualityConditionNSL 2394 # to the list of nslaw type 2395 # this must be improved by adding the EqualityConditionNSL 2396 # in self._nslaws_data 2397 # when a joint is imported. 2398 # For the moment, the nslaw is implicitely added 2399 # when we import_joint but is not stored 2400 # self._nslaws_data 2401 2402 if len(joints) > 0: 2403 nslaw_type_list.append('EqualityConditionNSL') 2404 2405 nb_of_nslaw_type = len(set(nslaw_type_list)) 2406 2407 # Check nslaw/OSI compatibility 2408 if (osi == sk.MoreauJeanGOSI): 2409 checknsl = 'NewtonImpactFrictionNSL' in set(nslaw_type_list) or 'NewtonImpactRollingFrictionNSL' in set(nslaw_type_list) 2410 if not checknsl: 2411 msg = 'MoreauJeanGOSI can only deal ' 2412 msg += 'with NewtonImpactFrictionNSL or NewtonImpactRollingFrictionNSL.' 2413 raise RuntimeError(msg) 2414 if (nb_of_nslaw_type > 1): 2415 msg = 'MoreauJeanGOSI cannot only deal with multiple inpact laws at the same time ' 2416 raise RuntimeError(msg) 2417 2418 # Creates and initialises the one-step nonsmooth problem. 2419 # The OSI and/or the nonsmooth law drives the choice. 2420 2421 # rationale for choosing numerics solver options 2422 # if solver_option is None --> we leave Siconos/kernel choosing the default option 2423 # else we use the user solver_options 2424 2425 if friction_contact_trace_params is None: 2426 # Global friction contact. 2427 if (osi == sk.MoreauJeanGOSI): 2428 if 'NewtonImpactFrictionNSL' in set(nslaw_type_list): 2429 if (solver_options is None): 2430 osnspb = sk.GlobalFrictionContact(3) 2431 else: 2432 osnspb = sk.GlobalFrictionContact(3, solver_options) 2433 elif 'NewtonImpactRollingFrictionNSL' in set(nslaw_type_list): 2434 if (solver_options is None): 2435 osnspb = sk.GlobalRollingFrictionContact(5) 2436 else: 2437 osnspb = sk.GlobalRollingFrictionContact(5, solver_options) 2438 osnspb.setMStorageType(sn.NM_SPARSE) 2439 # if sid == sn.SICONOS_GLOBAL_FRICTION_3D_ADMM: 2440 # osnspb.setMStorageType(sn.NM_SPARSE) 2441 # # which is the default for gfc in kernel, is it? 2442 # else: 2443 # osnspb.setMStorageType(sn.NM_SPARSE_BLOCK) 2444 osnspb.setMaxSize(osnspb_max_size) 2445 else: 2446 if ('EqualityConditionNSL' in set(nslaw_type_list)): 2447 if (solver_options is None): 2448 osnspb = sk.GenericMechanical() 2449 else: 2450 osnspb = sk.GenericMechanical(solver_options) 2451 else: 2452 if ('NewtonImpactFrictionNSL' in set(nslaw_type_list)) or \ 2453 (len(set(nslaw_type_list)) == 0): 2454 if (solver_options is None): 2455 osnspb = sk.FrictionContact(self._dimension) 2456 else: 2457 osnspb = sk.FrictionContact(self._dimension,solver_options) 2458 osnspb.setMaxSize(osnspb_max_size) 2459 osnspb.setMStorageType(sn.NM_SPARSE_BLOCK) 2460 2461 elif 'NewtonImpactRollingFrictionNSL' in set(nslaw_type_list): 2462 if self._dimension ==3: 2463 dimension_contact=5 2464 elif self._dimension ==2: 2465 dimension_contact=3 2466 if (solver_options is None): 2467 osnspb = sk.RollingFrictionContact(dimension_contact) 2468 else: 2469 osnspb = sk.RollingFrictionContact(dimension_contact, solver_options) 2470 else: 2471 msg = "Unknown nslaw type" 2472 msg += str(set(nslaw_type_list)) 2473 raise RuntimeError(msg) 2474 2475 else: # With trace 2476 if solver_options is None: 2477 solver_options = sk.solver_options_create( 2478 sn.SICONOS_FRICTION_3D_NSGS) 2479 sid = solver_options.solverId 2480 if(osi == sk.MoreauJeanGOSI): 2481 if 'NewtonImpactFrictionNSL' in set(nslaw_type_list): 2482 osnspb = GFCTrace(3, solver_options, friction_contact_trace_params, nsds) 2483 osnspb.setMStorageType(sn.NM_SPARSE) 2484 osnspb.setMaxSize(osnspb_max_size) 2485 elif 'NewtonImpactRollingFrictionNSL' in set(nslaw_type_list): 2486 osnspb = GRFCTrace(5, solver_options, friction_contact_trace_params, nsds) 2487 osnspb.setMStorageType(sn.NM_SPARSE) 2488 osnspb.setMaxSize(osnspb_max_size) 2489 else: 2490 msg = "Unknown nslaw type" 2491 msg += str(set(nslaw_type_list)) 2492 raise RuntimeError(msg) 2493 else: 2494 osnspb = FCTrace(3, solver_options, friction_contact_trace_params, nsds) 2495 osnspb.setMaxSize(osnspb_max_size) 2496 osnspb.setMStorageType(sn.NM_DENSE) 2497 2498 2499 osnspb.setNumericsVerboseMode(numerics_verbose) 2500 if numerics_verbose: 2501 sn.numerics_set_verbose(numerics_verbose_level) 2502 2503 # keep previous solution 2504 osnspb.setKeepLambdaAndYState(True) 2505 2506 # (6) Simulation setup with (1) (2) (3) (4) (5) 2507 if time_stepping == sk.TimeSteppingDirectProjection: 2508 if solver_options_pos is None: 2509 osnspb_pos = sk.MLCPProjectOnConstraints(sn.SICONOS_MLCP_ENUM, 1.0) 2510 else: 2511 osnspb_pos = sk.MLCPProjectOnConstraints(solver_options_pos, 1.0) 2512 2513 osnspb_pos.setMaxSize(osnspb_max_size) 2514 osnspb_pos.setMStorageType(sn.NM_DENSE) 2515 # "not yet implemented for sparse storage" 2516 osnspb_pos.setNumericsVerboseMode(numerics_verbose) 2517 osnspb_pos.setKeepLambdaAndYState(True) 2518 simulation = time_stepping(nsds, timedisc, self._osi, 2519 osnspb, osnspb_pos) 2520 simulation.setProjectionMaxIteration(projection_itermax) 2521 simulation.setConstraintTolUnilateral( 2522 projection_tolerance_unilateral) 2523 simulation.setConstraintTol(projection_tolerance) 2524 else: 2525 simulation = time_stepping(nsds, timedisc) 2526 simulation.insertIntegrator(self._osi) 2527 simulation.insertNonSmoothProblem(osnspb) 2528 2529 simulation.insertInteractionManager(self._interman) 2530 2531 simulation.setNewtonOptions(Newton_options) 2532 simulation.setNewtonMaxIteration(Newton_max_iter) 2533 simulation.setNewtonTolerance(1e-10) 2534 if verbose: 2535 simulation.setDisplayNewtonConvergence(display_Newton_convergence) 2536 2537 self._simulation = simulation 2538 2539 if len(self._plugins) > 0: 2540 self.print_verbose('import plugins ...') 2541 self.import_plugins() 2542 2543 if len(self._external_functions) > 0: 2544 self.print_verbose('import external functions ...') 2545 self.import_external_functions() 2546 2547 if controller is not None: 2548 controller.initialize(self) 2549 2550 self.print_verbose('first output static and dynamic objects ...') 2551 self.output_static_objects() 2552 self.output_dynamic_objects() 2553 2554 if self._should_output_domains: 2555 self.log(self.output_domains, with_timer)() 2556 2557 # nsds = model.nonSmoothDynamicalSystem() 2558 # nds= nsds.getNumberOfDS() 2559 # for i in range(nds): 2560 # ds = nsds.dynamicalSystem(i) 2561 # ds.display() 2562 2563 self.print_verbose('start simulation ...') 2564 self._initializing = False 2565 while simulation.hasNextEvent(): 2566 2567 if verbose_progress: 2568 self.print_verbose('step', k, 'of', k0 + int((T - t0) / h) - 1) 2569 2570 if start_run_iteration_hook is not None: 2571 self.log(start_run_iteration_hook, with_timer)(self) 2572 2573 self.log(self.import_births, with_timer)(body_class, 2574 shape_class, 2575 face_class, 2576 edge_class) 2577 2578 self.log(self.execute_deaths, with_timer)() 2579 2580 if controller is not None: 2581 controller.step() 2582 2583 if friction_contact_trace_params is not None: 2584 osnspb._stepcounter = k 2585 2586 if explode_Newton_solve: 2587 if(time_stepping == TimeStepping): 2588 self.log(self.explode_Newton_solve, with_timer, 2589 before=False)(with_timer) 2590 else: 2591 print('simulation of type', type(time_stepping), 2592 ' has no exploded version') 2593 self.log(simulation.computeOneStep, with_timer)() 2594 else: 2595 self.log(simulation.computeOneStep, with_timer)() 2596 2597 cond = self._output_frequency and (k % self._output_frequency == 0) 2598 if cond or k == 1: 2599 if verbose: 2600 self.print_verbose('output in hdf5 file at step ', k) 2601 2602 self.log(self.output_static_objects, with_timer)() 2603 2604 self.log(self.output_dynamic_objects, with_timer)() 2605 2606 self.log(self.output_velocities, with_timer)() 2607 2608 self.log(self.output_contact_forces, with_timer)() 2609 2610 if self._should_output_domains: 2611 self.log(self.output_domains, with_timer)() 2612 2613 self.log(self.output_solver_infos, with_timer)() 2614 2615 self.log(self._out.flush)() 2616 2617 if self._output_backup: 2618 if (k % self._output_backup_frequency == 0) or (k == 1): 2619 2620 # close io file, hdf5 memory is cleaned 2621 self._out.close() 2622 try: 2623 shutil.copyfile(self._io_filename, 2624 self._io_filename_backup) 2625 except shutil.Error as e: 2626 warn(str(e)) 2627 # open the file again 2628 finally: 2629 self.__enter__() 2630 2631 self.log(simulation.clearNSDSChangeLog, with_timer)() 2632 2633 # Note these are not the same and neither is correct. 2634 # "_interman.statistics" gives the number of contacts 2635 # collected by the collision engine, but it's possible some 2636 # are not in indexset1. Meanwhile checking the size of 2637 # the non-smooth problem is wrong when there are joints. 2638 if use_bullet: 2639 number_of_contacts = self._interman.statistics().new_interactions_created 2640 number_of_contacts += self._interman.statistics().existing_interactions_processed 2641 if verbose and number_of_contacts > 0: 2642 self.print_verbose('number of contacts', 2643 number_of_contacts, 2644 '(detected)', 2645 osnspb.getSizeOutput() // 3, 2646 '(active at velocity level. approx)') 2647 self.print_solver_infos() 2648 2649 else: 2650 number_of_contacts = osnspb.getSizeOutput() // 3 2651 if verbose and number_of_contacts > 0: 2652 msg = 'number of active contacts at the velocity level (approx)' 2653 self.print_verbose(msg, number_of_contacts) 2654 self.print_solver_infos() 2655 2656 if violation_verbose and number_of_contacts > 0: 2657 if len(simulation.y(0, 0)) > 0: 2658 self.print_verbose('violation info') 2659 y = simulation.y(0, 0) 2660 yplus = np.zeros((2, len(y))) 2661 yplus[0, :] = y 2662 y = np.min(yplus, axis=1) 2663 violation_max = np.max(-y) 2664 self.print_verbose(' violation max :', violation_max) 2665 if self._collision_margin is not None: 2666 if(violation_max >= self._collision_margin): 2667 self.print_verbose(' violation max is larger than the collision_margin') 2668 lam = simulation.lambda_(1, 0) 2669 self.print_verbose(' lambda max :', np.max(lam)) 2670 #print(' lambda : ',lam) 2671 2672 if len(simulation.y(1, 0)) > 0: 2673 v = simulation.y(1, 0) 2674 vplus = np.zeros((2, len(v))) 2675 vplus[0, :] = v 2676 v = np.max(vplus, axis=1) 2677 self.print_verbose(' velocity max :', np.max(v)) 2678 self.print_verbose(' velocity min :', np.min(v)) 2679 # #print(simulation.output(1,0)) 2680 solver_options = osnspb.numericsSolverOptions() 2681 precision = solver_options.dparam[sn.SICONOS_DPARAM_RESIDU] 2682 if (exit_tolerance is not None): 2683 if (precision > exit_tolerance): 2684 print('precision is larger exit_tolerance') 2685 return False 2686 self.log(simulation.nextStep, with_timer)() 2687 2688 if end_run_iteration_hook is not None: 2689 self.log(end_run_iteration_hook, with_timer)(self) 2690 2691 self.print_verbose('') 2692 k += 1 2693 return True 2694