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