1# Mechanics IO 2import os 3import sys 4from math import cos, sin, asin, atan2 5import numpy as np 6import h5py 7## fix compatibility with h5py version 8if (h5py.version.version_tuple.major >=3 ): 9 h5py_vlen_dtype = h5py.vlen_dtype 10else: 11 h5py_vlen_dtype = h5py.new_vlen 12 13import pickle 14import tempfile 15from contextlib import contextmanager 16 17# Siconos Mechanics imports 18from siconos.mechanics.collision.tools import Volume 19 20# Constants 21joint_points_axes = { 22 'KneeJointR': (1, 0), 23 'PivotJointR': (1, 1), 24 'PrismaticJointR': (0, 1), 25 'CylindricalJointR': (1, 1), 26 'FixedJointR': (0, 0), 27} 28 29 30# Utility functions 31def floatv(v): 32 return [float(x) for x in v] 33 34 35def arguments(): 36 """Returns tuple containing dictionary of calling function's 37 named arguments and a list of calling function's unnamed 38 positional arguments. 39 """ 40 from inspect import getargvalues, stack 41 posname, kwname, args = getargvalues(stack()[1][0])[-3:] 42 posargs = args.pop(posname, []) 43 args.update(args.pop(kwname, [])) 44 return args, posargs 45 46 47def check_points_axes(name, joint_class, points, axes): 48 def check(x, idx): 49 def er(): 50 n = joint_points_axes[joint_class][idx] 51 raise ValueError('{} ({}) expects {} {} (got {})' 52 .format(joint_class, name, n, 53 ['point', 'points', 'axis', 54 'axes'][idx * 2 + 1 * (n != 1)], 55 x)) 56 if np.shape(x) == (0,) or np.shape(x) == (): 57 num = 0 58 else: 59 if len(np.shape(x)) != 2 or np.shape(x)[1] != 3: 60 er() 61 num = np.shape(x)[0] 62 if joint_class in joint_points_axes and \ 63 joint_points_axes[joint_class][idx] != num: 64 er() 65 check(points, 0) 66 check(axes, 1) 67 68 69@contextmanager 70def tmpfile(suffix='', prefix='siconos_io', contents=None, 71 debug=False): 72 """ 73 A context manager for a named temporary file. 74 """ 75 (_, tfilename) = tempfile.mkstemp(suffix=suffix, prefix=prefix) 76 fid = open(tfilename, 'w') 77 if contents is not None: 78 fid.write(contents) 79 fid.flush() 80 81 class TmpFile: 82 83 def __init__(self, fid, name): 84 self.fid = fid 85 self.name = name 86 87 def __getitem__(self, n): 88 if n == 0: 89 return self.fid 90 elif n == 1: 91 return self.name 92 else: 93 raise IndexError 94 95 r = TmpFile(fid, tfilename) 96 97 yield r 98 fid.close() 99 if not debug: 100 os.remove(tfilename) 101 102 103def warn(msg): 104 sys.stderr.write('{0}: {1}'.format(sys.argv[0], msg)) 105 106 107def object_id(obj): 108 """returns an unique object identifier""" 109 return obj.__hash__() 110 111 112def group(h, name, must_exist=True): 113 try: 114 return h[name] 115 except KeyError: 116 if must_exist: 117 return h.create_group(name) 118 else: 119 try: 120 return h.create_group(name) 121 except ValueError: 122 # could not create group, return None 123 # (file is probably in read-only mode) 124 return None 125 126 127def data(h, name, nbcolumns, use_compression=False): 128 try: 129 return h[name] 130 except KeyError: 131 comp = use_compression and nbcolumns > 0 132 return h.create_dataset(name, (0, nbcolumns), 133 maxshape=(None, nbcolumns), 134 chunks=[None, (4000, nbcolumns)][comp], 135 compression=[None, 'gzip'][comp], 136 compression_opts=[None, 9][comp]) 137 138 139def add_line(dataset, line): 140 dataset.resize(dataset.shape[0] + 1, 0) 141 dataset[dataset.shape[0] - 1, :] = line 142 143 144# 145# misc fixes 146# 147# fix ctr.'name' in old hdf5 files 148# 149def upgrade_io_format(filename): 150 151 with MechanicsHdf5(filename, mode='a') as io: 152 153 for instance_name in io.instances(): 154 for contactor_instance_name in io.instances()[instance_name]: 155 contactor = io.instances()[instance_name][ 156 contactor_instance_name] 157 if 'name' in contactor.attrs: 158 warn(""" 159contactor {0} attribute 'name': renamed in 'shape_name' 160 """) 161 contactor.attrs['shape_name'] = contactor['name'] 162 del contactor['name'] 163 164 165def str_of_file(filename): 166 with open(filename, 'r') as f: 167 return str(f.read()) 168 169 170def file_of_str(filename, string): 171 if not os.path.exists(os.path.dirname(filename)): 172 try: 173 os.makedirs(os.path.dirname(filename)) 174 except OSError as exc: 175 if exc.errno != exc.errno.EEXIST: 176 raise 177 178 with open(filename, "w") as f: 179 f.write(string) 180 181 182# 183# fix orientation -> rotation ? 184# 185def quaternion_get(orientation): 186 """ 187 Get quaternion from orientation 188 """ 189 if len(orientation) == 2: 190 # axis + angle 191 axis = orientation[0] 192 assert len(axis) == 3 193 angle = orientation[1] 194 assert type(angle) is float 195 n = sin(angle / 2.) / np.linalg.norm(axis) 196 197 ori = [cos(angle / 2.), axis[0] * n, axis[1] * n, axis[2] * n] 198 else: 199 assert(len(orientation) == 4) 200 # a given quaternion 201 ori = orientation 202 return ori 203 204 205def quaternion_multiply(q1, q0): 206 w0, x0, y0, z0 = q0 207 w1, x1, y1, z1 = q1 208 return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 209 x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 210 -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 211 x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64) 212 213 214def phi(q0, q1, q2, q3): 215 """ 216 Euler angle phi from quaternion. 217 """ 218 return atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) 219 220 221def theta(q0, q1, q2, q3): 222 """ 223 Euler angle theta from quaternion. 224 """ 225 return asin(2 * (q0 * q2 - q3 * q1)) 226 227 228def psi(q0, q1, q2, q3): 229 """ 230 Euler angle psi from quaternion. 231 """ 232 return atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) 233 234 235# vectorized versions 236phiv = np.vectorize(phi) 237thetav = np.vectorize(theta) 238psiv = np.vectorize(psi) 239 240 241# 242# inertia 243# 244def compute_inertia_and_center_of_mass(shapes, io=None): 245 """ 246 Compute inertia from a list of Shapes. 247 248 Returns 249 ------- 250 mass 251 center_of_mass 252 inertia 253 inertia_matrix 254 """ 255 from OCC.GProp import GProp_GProps 256 from OCC.BRepGProp import brepgprop_VolumeProperties 257 from OCC.gp import gp_Ax1, gp_Dir 258 from siconos.mechanics import occ 259 260 system = GProp_GProps() 261 262 for shape in shapes: 263 264 iprops = GProp_GProps() 265 266 if shape.data is None: 267 if io is not None: 268 shape.data = io._shape.get(shape.shape_name, new_instance=True) 269 else: 270 warn('cannot get shape {0}'.format(shape.shape_name)) 271 return None 272 273 iishape = shape.data 274 275 ishape = occ.OccContactShape(iishape).data() 276 # the shape relative displacement 277 occ.occ_move(ishape, list(shape.translation) + list(shape.orientation)) 278 279 brepgprop_VolumeProperties(iishape, iprops) 280 281 density = None 282 283 if hasattr(shape, 'mass') and shape.mass is not None: 284 density = shape.mass / iprops.Mass() 285 286 elif shape.parameters is not None and hasattr(shape.parameters, 'density'): 287 density = shape.parameters.density 288 #print('shape.parameters.density:', shape.parameters.density) 289 else: 290 density = 1. 291 292 assert density is not None 293 # print("shape", shape.shape_name) 294 # print('density:', density) 295 # print('iprops.Mass():', iprops.Mass()) 296 297 system.Add(iprops, density) 298 299 mass = system.Mass() 300 assert (system.Mass() > 0.) 301 302 computed_com = system.CentreOfMass() 303 304 gp_mat = system.MatrixOfInertia() 305 inertia_matrix = np.zeros((3, 3)) 306 for i in range(0, 3): 307 for j in range(0, 3): 308 inertia_matrix[i, j] = gp_mat.Value(i + 1, j + 1) 309 310 I1 = system.MomentOfInertia( 311 gp_Ax1(computed_com, gp_Dir(1, 0, 0))) 312 I2 = system.MomentOfInertia( 313 gp_Ax1(computed_com, gp_Dir(0, 1, 0))) 314 I3 = system.MomentOfInertia( 315 gp_Ax1(computed_com, gp_Dir(0, 0, 1))) 316 317 inertia = [I1, I2, I3] 318 center_of_mass = np.array([computed_com.Coord(1), 319 computed_com.Coord(2), 320 computed_com.Coord(3)]) 321 322 return mass, center_of_mass, inertia, inertia_matrix 323 324 325def occ_topo_list(shape): 326 """ return the edges & faces from `shape` 327 328 :param shape: a TopoDS_Shape 329 :return: a list of edges and faces 330 """ 331 332 from OCC.TopAbs import TopAbs_FACE 333 from OCC.TopAbs import TopAbs_EDGE 334 from OCC.TopExp import TopExp_Explorer 335 from OCC.TopoDS import topods_Face, topods_Edge 336 337 338 topExp = TopExp_Explorer() 339 topExp.Init(shape, TopAbs_FACE) 340 faces = [] 341 edges = [] 342 343 while topExp.More(): 344 face = topods_Face(topExp.Current()) 345 faces.append(face) 346 topExp.Next() 347 348 topExp.Init(shape, TopAbs_EDGE) 349 350 while topExp.More(): 351 edge = topods_Edge(topExp.Current()) 352 edges.append(edge) 353 topExp.Next() 354 355 return faces, edges 356 357 358def occ_load_file(filename): 359 """ 360 load in pythonocc a igs or step file 361 362 :param filename: a filename with extension 363 :return: a topods_shape 364 """ 365 366 from OCC.STEPControl import STEPControl_Reader 367 from OCC.IGESControl import IGESControl_Reader 368 from OCC.BRep import BRep_Builder 369 from OCC.TopoDS import TopoDS_Compound 370 from OCC.IFSelect import IFSelect_RetDone, IFSelect_ItemsByEntity 371 372 reader_switch = {'stp': STEPControl_Reader, 373 'step': STEPControl_Reader, 374 'igs': IGESControl_Reader, 375 'iges': IGESControl_Reader} 376 377 builder = BRep_Builder() 378 comp = TopoDS_Compound() 379 builder.MakeCompound(comp) 380 381 reader = reader_switch[os.path.splitext(filename)[1][1:].lower()]() 382 383 status = reader.ReadFile(filename) 384 385 if status == IFSelect_RetDone: # check status 386 failsonly = False 387 reader.PrintCheckLoad( 388 failsonly, IFSelect_ItemsByEntity) 389 reader.PrintCheckTransfer( 390 failsonly, IFSelect_ItemsByEntity) 391 392 reader.TransferRoots() 393 nbs = reader.NbShapes() 394 395 for i in range(1, nbs + 1): 396 shape = reader.Shape(i) 397 builder.Add(comp, shape) 398 399 return comp 400 401 402def topods_shape_reader(shape, deflection=0.001): 403 404 from OCC.StlAPI import StlAPI_Writer 405 from OCC.BRepMesh import BRepMesh_IncrementalMesh 406 407 import vtk 408 409 stl_writer = StlAPI_Writer() 410 411 with tmpfile(suffix='.stl') as tmpf: 412 mesh = BRepMesh_IncrementalMesh(shape, deflection) 413 mesh.Perform() 414 assert mesh.IsDone() 415 stl_writer.SetASCIIMode(False) 416 stl_writer.Write(shape, tmpf[1]) 417 tmpf[0].flush() 418 419 reader = vtk.vtkSTLReader() 420 reader.SetFileName(tmpf[1]) 421 reader.Update() 422 423 return reader 424 425 426def brep_reader(brep_string, indx): 427 428 from OCC.StlAPI import StlAPI_Writer 429 from OCC.BRepTools import BRepTools_ShapeSet 430 import vtk 431 432 shape_set = BRepTools_ShapeSet() 433 shape_set.ReadFromString(brep_string) 434 shape = shape_set.Shape(shape_set.NbShapes()) 435 location = shape_set.Locations().Location(indx) 436 shape.Location(location) 437 438 stl_writer = StlAPI_Writer() 439 440 with tmpfile(suffix='.stl') as tmpf: 441 stl_writer.Write(shape, tmpf[1]) 442 tmpf[0].flush() 443 444 reader = vtk.vtkSTLReader() 445 reader.SetFileName(tmpf[1]) 446 reader.Update() 447 448 return reader 449 450 451class MechanicsHdf5(object): 452 453 """a MechanicsHdf5 context manager, used to prepare a simulation description 454 to be executed by MechanicsRunner. 455 456 Parameters 457 ---------- 458 io_filename: string, optional 459 hdf5 file name, default = <caller>.hdf5, caller being the name 460 without ext of the file that instanciates the Runner. 461 mode: string, optional 462 h5 mode (w, r, append), default = 'w' 463 io_filename_backup: string, optional 464 name of a backup (copy) file for hdf5 outputs. 465 Backup every <output_frequency> step. Default = <caller>_last.hdf5 466 use_compression: boolean, optional 467 true to use compression for h5 file, default=False 468 output_domains: boolean, optional 469 if trueoutputs info regarding contact point domains 470 default=False 471 verbose: boolean, optional 472 default=True 473 """ 474 475 def __init__(self, io_filename=None, mode='w', io_filename_backup=None, 476 use_compression=False, output_domains=False, verbose=True): 477 if io_filename is None: 478 self._io_filename = '{0}.hdf5'.format( 479 os.path.splitext(os.path.basename(sys.argv[0]))[0]) 480 else: 481 self._io_filename = io_filename 482 483 if io_filename_backup is None: 484 self._io_filename_backup = '{0}_last.hdf5'.format( 485 os.path.splitext(self._io_filename)[0]) 486 else: 487 self._io_filename_backup = io_filename_backup 488 489 self._output_backup = False 490 self._mode = mode 491 self._static_data = None 492 self._velocities_data = None 493 self._dynamic_data = None 494 self._cf_data = None 495 self._domain_data = None 496 self._solv_data = None 497 self._log_data = None 498 self._input = None 499 self._nslaws_data = None 500 self._nslaws = dict() 501 self._out = None 502 self._data = None 503 self._ref = None 504 self._permanent_interactions = None 505 self._joints = None 506 self._boundary_conditions = None 507 self._plugins = None 508 self._external_functions = None 509 self._number_of_shapes = 0 510 self._number_of_permanent_interactions = 0 511 self._number_of_dynamic_objects = 0 512 self._number_of_static_objects = 0 513 self._use_compression = use_compression 514 self._should_output_domains = output_domains 515 self._verbose = verbose 516 517 def __enter__(self): 518 """Reminder: this function will be called when a 'with' 519 statement 520 will be executed with the present class. 521 522 Warning : it means that this class must be called inside 523 a with statement to be properly initialized! 524 """ 525 # -- Creates the Open the hdf5 object -- 526 self._out = h5py.File(self._io_filename, self._mode) 527 # -- And read its content -- 528 # Important : since the mode might be write or read, most 529 # of the attributes and fields 530 # must have a default value to tackle the 'write' case. 531 self._dimension = self._out.attrs.get('dimension', 3) 532 self._data = group(self._out, 'data') 533 self._ref = group(self._data, 'ref') 534 self._permanent_interactions = group(self._data, 535 'permanent_interactions', 536 must_exist=False) 537 self._joints = group(self._data, 'joints', must_exist=False) 538 self._plugins = group(self._data, 'plugins', must_exist=False) 539 self._external_functions = group(self._data, 'external_functions', 540 must_exist=False) 541 try: 542 self._boundary_conditions = group(self._data, 543 'boundary_conditions', 544 must_exist=(self._mode == 'w')) 545 except Exception as e: 546 print('Warning - group(self._data, boundary_conditions ) : ', e) 547 self._static_data = data(self._data, 'static', 9, 548 use_compression=self._use_compression) 549 550 self._velocities_data = data(self._data, 'velocities', 8, 551 use_compression=self._use_compression) 552 if self._mode == 'w': 553 self._velocities_data.attrs['info'] = 'time, ds id ,' 554 self._velocities_data.attrs['info'] += 'translational velocities ,' 555 self._velocities_data.attrs['info'] += 'angular velocities' 556 557 self._dynamic_data = data(self._data, 'dynamic', 9, 558 use_compression=self._use_compression) 559 if self._mode == 'w': 560 self._dynamic_data.attrs['info'] = 'time, ds id , translation ,' 561 self._dynamic_data.attrs['info'] += 'orientation' 562 563 self._cf_data = data(self._data, 'cf', 26, 564 use_compression=self._use_compression) 565 if self._mode == 'w': 566 self._cf_data.attrs['info'] = 'time, mu, contact point A ,' 567 self._cf_data.attrs['info'] += 'contact point B, contact normal, ' 568 self._cf_data.attrs['info'] += 'relative gap relative velocity,' 569 self._cf_data.attrs['info'] += 'reaction impulse, interaction id,' 570 self._cf_data.attrs['info'] += 'ds 1 number, ds 2 number ' 571 572 if self._should_output_domains or 'domain' in self._data: 573 self._domain_data = data(self._data, 'domain', 3, 574 use_compression=self._use_compression) 575 self._solv_data = data(self._data, 'solv', 4, 576 use_compression=self._use_compression) 577 try: 578 self._log_data = group(self._data, 'log') 579 except Exception as e: 580 print('Warning - group(self._data, log ) : ', e) 581 582 self._input = group(self._data, 'input') 583 584 self._nslaws_data = group(self._data, 'nslaws') 585 return self 586 587 def __exit__(self, type_, value, traceback): 588 self._out.close() 589 590 def print_verbose(self, *args, **kwargs): 591 if self._verbose: 592 print('[io.mechanics]', *args, **kwargs) 593 594 # hdf5 structure 595 def dimension(self): 596 """ 597 dimension : get the dimension (2 or 3) of the scene 598 """ 599 return self._dimension 600 601 def shapes(self): 602 """ 603 Shapes : parameterized primitives or user defined 604 (convex set or meshes) 605 """ 606 return self._ref 607 608 def permanent_interactions(self): 609 """ 610 Permanent interactions. 611 """ 612 return self._permanent_interactions 613 614 def static_data(self): 615 """ 616 Coordinates and orientations of static objects. 617 """ 618 return self._static_data 619 620 def dynamic_data(self): 621 """ 622 Coordinates and orientations of dynamic objects. 623 """ 624 return self._dynamic_data 625 626 def velocities_data(self): 627 """ 628 Velocities of dynamic objects 629 """ 630 return self._velocities_data 631 632 def contact_forces_data(self): 633 """ 634 Contact points information. 635 """ 636 return self._cf_data 637 638 def domains_data(self): 639 """ 640 Contact point domain information. 641 """ 642 return self._domain_data 643 644 def solver_data(self): 645 """ 646 Solver output 647 """ 648 return self._solv_data 649 650 def log_data(self): 651 """ 652 log output 653 """ 654 return self._log_data 655 656 def instances(self): 657 """ 658 Scene objects. 659 """ 660 return self._input 661 662 def nonsmooth_laws(self): 663 """ 664 Non smooth laws between group of contactors. 665 """ 666 return self._nslaws_data 667 668 def joints(self): 669 """ 670 Joints between dynamic objects or between an object and the scenery. 671 """ 672 return self._joints 673 674 def boundary_conditions(self): 675 """ 676 Boundary conditions applied to dynamic objects 677 """ 678 return self._boundary_conditions 679 680 def add_plugin_source(self, name, filename): 681 """ 682 Add C source plugin 683 """ 684 685 if name not in self._plugins: 686 plugin_src = self._plugins.create_dataset(name, (1,), 687 dtype=h5py_vlen_dtype(str)) 688 plugin_src[:] = str_of_file(filename) 689 plugin_src.attrs['filename'] = filename 690 691 def add_external_function(self, name, body_name, function_name, 692 plugin_name, plugin_function_name): 693 694 if name not in self._external_functions: 695 ext_fun = group(self._external_functions, name) 696 ext_fun.attrs['body_name'] = body_name 697 ext_fun.attrs['function_name'] = function_name 698 ext_fun.attrs['plugin_name'] = plugin_name 699 ext_fun.attrs['plugin_function_name'] = plugin_function_name 700 701 def add_external_bc_function(self, name, body_name, bc_indices, 702 plugin_name, plugin_function_name): 703 704 if name not in self._external_functions: 705 ext_fun = group(self._external_functions, name) 706 ext_fun.attrs['body_name'] = body_name 707 ext_fun.attrs['plugin_name'] = plugin_name 708 ext_fun.attrs['plugin_function_name'] = plugin_function_name 709 ext_fun.attrs['bc_indices'] = bc_indices 710 711 def add_mesh_from_string(self, name, shape_data, scale=None, 712 insideMargin=None, outsideMargin=None): 713 """ 714 Add a mesh shape from a string. 715 Accepted format : mesh encoded in VTK .vtp format 716 """ 717 718 if name not in self._ref: 719 720 shape = self._ref.create_dataset(name, (1,), 721 dtype=h5py_vlen_dtype(str)) 722 shape[:] = shape_data 723 shape.attrs['id'] = self._number_of_shapes 724 shape.attrs['type'] = 'vtp' 725 if scale is not None: 726 shape.attrs['scale'] = scale 727 if insideMargin is not None: 728 shape.attrs['insideMargin'] = insideMargin 729 if outsideMargin is not None: 730 shape.attrs['outsideMargin'] = outsideMargin 731 self._number_of_shapes += 1 732 733 def add_mesh_from_file(self, name, filename, scale=None, 734 insideMargin=None, outsideMargin=None): 735 """ Add a mesh shape from a file. 736 Accepted format : .stl or mesh encoded in VTK .vtp format 737 """ 738 739 import vtk 740 741 if filename[0] != os.path.sep: 742 filename = os.path.join( 743 os.path.split(os.path.abspath(sys.argv[0]))[0], 744 filename) 745 if name not in self._ref: 746 747 if os.path.splitext(filename)[-1][1:] == 'stl': 748 reader = vtk.vtkSTLReader() 749 reader.SetFileName(filename) 750 reader.Update() 751 752 if reader.GetErrorCode() != 0: 753 print('vtkSTLReader error', reader.GetErrorCode()) 754 sys.exit(1) 755 756 with tmpfile() as tmpf: 757 writer = vtk.vtkXMLPolyDataWriter() 758 writer.SetInputData(reader.GetOutput()) 759 writer.SetFileName(tmpf[1]) 760 writer.Write() 761 762 shape_data = str_of_file(tmpf[1]) 763 764 else: 765 assert os.path.splitext(filename)[-1][1:] == 'vtp' 766 shape_data = str_of_file(filename) 767 768 self.add_mesh_from_string(name, shape_data, scale=scale, 769 insideMargin=insideMargin, 770 outsideMargin=outsideMargin) 771 772 def add_height_map(self, name, heightmap, rectangle, 773 insideMargin=None, outsideMargin=None): 774 """ 775 Add a heightmap represented as a SiconosMatrix 776 """ 777 assert(heightmap.shape[0] >= 2) 778 assert(heightmap.shape[1] >= 2) 779 if name not in self._ref: 780 shape = self._ref.create_dataset(name, data=heightmap) 781 shape.attrs['id'] = self._number_of_shapes 782 shape.attrs['type'] = 'heightmap' 783 784 # measurements of the heightfield, i.e. length of sides of 785 # the rectangle where heightmap will be placed -- height 786 # is represented by heightmap values 787 assert(len(rectangle) == 2) 788 shape.attrs['rect'] = rectangle # tuple (length x, length y) 789 790 if insideMargin is not None: 791 shape.attrs['insideMargin'] = insideMargin 792 if outsideMargin is not None: 793 shape.attrs['outsideMargin'] = outsideMargin 794 self._number_of_shapes += 1 795 796 def add_brep_from_string(self, name, shape_data): 797 """ 798 Add a brep contained in a string. 799 """ 800 if name not in self._ref: 801 shape = self._ref.create_dataset(name, (1,), 802 dtype=h5py_vlen_dtype(str)) 803 if type(shape_data) == str: 804 # raw str 805 shape[:] = shape_data 806 else: 807 # __getstate__ as with pythonocc 808 shape[:] = shape_data[0] 809 shape.attrs['occ_indx'] = shape_data[1] 810 811 shape.attrs['id'] = self._number_of_shapes 812 shape.attrs['type'] = 'brep' 813 814 self._number_of_shapes += 1 815 816 def add_occ_shape(self, name, occ_shape): 817 """ 818 Add an OpenCascade TopoDS_Shape. 819 """ 820 821 if name not in self._ref: 822 823 from OCC.STEPControl import STEPControl_Writer, STEPControl_AsIs 824 825 # step format is used for the storage. 826 step_writer = STEPControl_Writer() 827 828 step_writer.Transfer(occ_shape, STEPControl_AsIs) 829 830 shape_data = None 831 832 with tmpfile() as tmpf: 833 834 step_writer.Write(tmpf[1]) 835 836 tmpf[0].flush() 837 shape_data = str_of_file(tmpf[1]) 838 839 shape = self._ref.create_dataset(name, (1,), 840 dtype=h5py_vlen_dtype(str)) 841 shape[:] = shape_data 842 shape.attrs['id'] = self._number_of_shapes 843 shape.attrs['type'] = 'step' 844 self._number_of_shapes += 1 845 846 def add_shape_data_from_file(self, name, filename): 847 """ 848 Add shape data from a file. 849 """ 850 if name not in self._ref: 851 shape = self._ref.create_dataset(name, (1,), 852 dtype=h5py_vlen_dtype(str)) 853 shape[:] = str_of_file(filename) 854 shape.attrs['id'] = self._number_of_shapes 855 try: 856 shape.attrs['type'] = os.path.splitext(filename)[1][1:] 857 except: 858 shape.attrs['type'] = 'unknown' 859 860 self._number_of_shapes += 1 861 862 def add_interaction(self, name, body1_name, contactor1_name=None, 863 body2_name=None, contactor2_name=None, 864 distance_calculator='cadmbtb', 865 offset1=0.0, offset2=0.0): 866 """ 867 Add permanent interactions between two objects contactors. 868 """ 869 if name not in self.permanent_interactions(): 870 pinter = self.permanent_interactions().create_dataset( 871 name, (1,), dtype=h5py_vlen_dtype(str)) 872 pinter.attrs['id'] = self._number_of_permanent_interactions 873 pinter.attrs['type'] = 'permanent_interaction' 874 pinter.attrs['body1_name'] = body1_name 875 pinter.attrs['body2_name'] = body2_name 876 if contactor1_name is not None: 877 pinter.attrs['contactor1_name'] = contactor1_name 878 if contactor2_name is not None: 879 pinter.attrs['contactor2_name'] = contactor2_name 880 pinter.attrs['distance_calculator'] = distance_calculator 881 pinter.attrs['offset1'] = offset1 882 pinter.attrs['offset2'] = offset2 883 884 self._number_of_permanent_interactions += 1 885 886 def add_convex_shape(self, name, points, 887 insideMargin=None, outsideMargin=None): 888 """ 889 Add a convex shape defined by a list of points. 890 """ 891 # infer the dimension of the problem 892 if np.shape(points)[1] == 2: 893 self._dimension = 2 894 else: 895 if self._dimension == 2: 896 raise ValueError('It is not yet possible to mix 2D and 3D primitives shapes') 897 self._dimension == 3 898 self._out.attrs['dimension'] = self._dimension 899 900 if name not in self._ref: 901 shape = self._ref.create_dataset(name, 902 (np.shape(points)[0], 903 np.shape(points)[1])) 904 if insideMargin is not None: 905 shape.attrs['insideMargin'] = insideMargin 906 if outsideMargin is not None: 907 shape.attrs['outsideMargin'] = outsideMargin 908 shape[:] = points[:] 909 shape.attrs['type'] = 'convex' 910 shape.attrs['id'] = self._number_of_shapes 911 self._number_of_shapes += 1 912 913 def add_primitive_shape(self, name, primitive, params, 914 insideMargin=None, outsideMargin=None): 915 """ 916 Add a primitive shape. 917 """ 918 # infer the dimension of the problem 919 if primitive == 'Disk' or primitive == 'Box2d': 920 self._dimension = 2 921 else: 922 if self._dimension == 2: 923 raise ValueError('It is not yet possible to mix 2D and 3D primitives shapes') 924 self._dimension == 3 925 self._out.attrs['dimension'] = self._dimension 926 927 if name not in self._ref: 928 shape = self._ref.create_dataset(name, (1, len(params))) 929 shape.attrs['id'] = self._number_of_shapes 930 shape.attrs['type'] = 'primitive' 931 shape.attrs['primitive'] = primitive 932 if insideMargin is not None: 933 shape.attrs['insideMargin'] = insideMargin 934 if outsideMargin is not None: 935 shape.attrs['outsideMargin'] = outsideMargin 936 shape[:] = params 937 self._number_of_shapes += 1 938 939 def add_object(self, name, shapes, 940 translation, 941 orientation=None, 942 velocity= None, 943 use_volume_centroid_as_initial_translation=False, 944 mass=None, center_of_mass=[0, 0, 0], inertia=None, 945 time_of_birth=-1, time_of_death=-1, 946 fext=None, 947 allow_self_collide=False): 948 """Add an object with associated shapes as a list of Volume or 949 Contactor objects. Contact detection and processing is 950 defined by the Contactor objects. The Volume objects are used for 951 the computation of inertia and center of mass if not provided. 952 953 The body-fixed frame is assumed to be the global inertial 954 frame. This means that 955 1. By default, the center of mass is located at the origin. 956 The initial translation is applied from this point, so that x_g(0) = translation 957 2. the orientation is identical to the inertial frame. 958 The initial orientation is applied to the inertial frame to obtain 959 the body-fixed frame. 960 961 962 Each Contactor and Volume object may have a relative 963 translation and a relative orientation expressed in the bodyframe 964 coordinates. 965 966 Parameters 967 ---------- 968 name: string 969 The name of the object. 970 971 shapes: iterable 972 The list of associated Contactor or Volume objects. 973 974 translation: array_like of length 3 or 2 (dimension =2) 975 Initial translation of the object (mandatory) 976 977 orientation: array_like of length 3 (Euler Angles) or 4 978 (unit quaternion), or 1 (dimension =2) 979 Initial orientiation of the object. By default, identity 980 981 velocity: array_like of length 6, or 3 (dimension =2) 982 Initial velocity of the object. The default velocity is zero. 983 dimension =3 : 984 The components are those of the translation velocity along 985 x, y and z axis and the rotation velocity around x, y and 986 z axis. 987 dimension =2 : 988 The components are those of the translation velocity along 989 x, y and the rotation velocity z axis. 990 991 mass: float 992 The mass of the object, if it is None the object is defined as 993 a static object involved only in contact detection. 994 The default value is None. 995 996 center_of_mass: array_like of length 3 997 The position of the center of mass expressed in the body frame 998 coordinates. 999 1000 inertia: array_like of length 3 or 3x3 matrix. 1001 The principal moments of inertia (array of length 3) or 1002 a full 3x3 inertia matrix 1003 1004 use_volume_centroid_as_initial_translation: boolean. 1005 if True and if a Volume is given is the list of shape, 1006 the position of 1007 the volume centroid is used as initial translation. 1008 """ 1009 # print(arguments()) 1010 1011 if (self._dimension == 3): 1012 if orientation is None: 1013 orientation = [1, 0, 0, 0] 1014 if velocity is None: 1015 velocity = [0, 0, 0, 0, 0, 0] 1016 ori = quaternion_get(orientation) 1017 assert (len(translation) == 3) 1018 assert (len(ori) == 4) 1019 1020 elif (self._dimension == 2): 1021 if orientation is None: 1022 orientation = [0.] 1023 if velocity is None: 1024 velocity = [0, 0, 0] 1025 assert (len(translation) == 2) 1026 ori = orientation 1027 1028 is_center_of_mass_computed = False 1029 if name not in self._input: 1030 1031 if (inertia is None) or (mass is None): 1032 if any(map(lambda s: isinstance(s, Volume), shapes)): 1033 1034 # a computed inertia and center of mass 1035 # occ only 1036 volumes = filter(lambda s: isinstance(s, Volume), 1037 shapes) 1038 1039 computed_mass, com, computed_inertia, computed_inertia_matrix = compute_inertia_and_center_of_mass(volumes, self) 1040 self.print_verbose('{0}: computed mass from Volume'.format(name)) 1041 self.print_verbose('{0}: computed center of mass:'.format(name), 1042 com[0], 1043 com[1], 1044 com[2]) 1045 self.print_verbose('{0}: computed mass:'.format(name), 1046 computed_mass) 1047 self.print_verbose('{0}: computed inertia:'.format(name), 1048 computed_inertia[0], computed_inertia[1], 1049 computed_inertia[2]) 1050 self.print_verbose('{0}: computed inertia matrix:'.format(name), 1051 computed_inertia_matrix) 1052 is_center_of_mass_computed = True 1053 if mass is None: 1054 mass = computed_mass 1055 1056 if inertia is None: 1057 inertia = computed_inertia_matrix 1058 1059 obj = group(self._input, name) 1060 1061 if use_volume_centroid_as_initial_translation and is_center_of_mass_computed: 1062 translation = com 1063 for s in shapes: 1064 s.translation = s.translation - com 1065 1066 if time_of_birth >= 0: 1067 obj.attrs['time_of_birth'] = time_of_birth 1068 if time_of_death >= 0: 1069 obj.attrs['time_of_death'] = time_of_death 1070 1071 if mass is not None: 1072 obj.attrs['mass'] = mass 1073 obj.attrs['type'] = 'dynamic' 1074 if np.isscalar(mass) and mass <= 0.: 1075 1076 self.print_verbose("The use of a mass equal to zero to define a static object is deprecated.") 1077 self.print_verbose("Do not give the mass or set mass=None to define a static object") 1078 else: 1079 obj.attrs['type'] = 'static' 1080 obj.attrs['translation'] = translation 1081 obj.attrs['orientation'] = ori 1082 obj.attrs['velocity'] = velocity 1083 obj.attrs['center_of_mass'] = center_of_mass 1084 1085 if inertia is not None: 1086 obj.attrs['inertia'] = inertia 1087 1088 if fext is not None: 1089 obj.attrs['fext'] = fext 1090 1091 if allow_self_collide is not None: 1092 obj.attrs['allow_self_collide'] = allow_self_collide 1093 1094 contactors = shapes 1095 1096 for num, ctor in enumerate(contactors): 1097 1098 if ctor.instance_name is not None: 1099 # a specified name 1100 instance_name = ctor.instance_name 1101 else: 1102 # the default name for contactor 1103 instance_name = '{0}-{1}'.format(ctor.shape_name, num) 1104 1105 dat = data(obj, instance_name, 0, 1106 use_compression=self._use_compression) 1107 1108 dat.attrs['instance_name'] = instance_name 1109 dat.attrs['shape_name'] = ctor.shape_name 1110 if hasattr(ctor, 'group'): 1111 dat.attrs['group'] = ctor.group 1112 1113 if hasattr(ctor, 'parameters') and \ 1114 ctor.parameters is not None: 1115 # we add np.void to manage writing string in hdf5 files see http://docs.h5py.org/en/latest/strings.html 1116 dat.attrs['parameters'] = np.void(pickle.dumps(ctor.parameters)) 1117 1118 if hasattr(ctor, 'contact_type') and \ 1119 ctor.contact_type is not None: 1120 dat.attrs['type'] = ctor.contact_type 1121 1122 if hasattr(ctor, 'contact_index') and \ 1123 ctor.contact_index is not None: 1124 dat.attrs['contact_index'] = ctor.contact_index 1125 1126 dat.attrs['translation'] = ctor.translation 1127 dat.attrs['orientation'] = quaternion_get(ctor.orientation) 1128 1129 if mass is None or mass == 0: 1130 obj.attrs['id'] = -(self._number_of_static_objects + 1) 1131 self._number_of_static_objects += 1 1132 1133 else: 1134 obj.attrs['id'] = (self._number_of_dynamic_objects + 1) 1135 self._number_of_dynamic_objects += 1 1136 1137 return obj 1138 1139 def add_Newton_impact_rolling_friction_nsl(self, name, mu, mu_r, e=0, 1140 collision_group1=0, 1141 collision_group2=0): 1142 """ 1143 Add a nonsmooth law for contact between 2 groups. 1144 Only NewtonImpactFrictionNSL are supported. 1145 name is an user identifiant and must be unique, 1146 mu is the coefficient of friction, 1147 e is the coefficient of restitution on the contact normal, 1148 gid1 and gid2 define the group identifiants. 1149 1150 """ 1151 if name not in self._nslaws_data: 1152 nslaw = self._nslaws_data.create_dataset(name, (0,)) 1153 nslaw.attrs['type'] = 'NewtonImpactRollingFrictionNSL' 1154 nslaw.attrs['mu'] = mu 1155 nslaw.attrs['mu_r'] = mu_r 1156 nslaw.attrs['e'] = e 1157 nslaw.attrs['gid1'] = collision_group1 1158 nslaw.attrs['gid2'] = collision_group2 1159 1160 def add_Newton_impact_friction_nsl(self, name, mu, e=0, collision_group1=0, 1161 collision_group2=0): 1162 """ 1163 Add a nonsmooth law for contact between 2 groups. 1164 Only NewtonImpactFrictionNSL are supported. 1165 name is an user identifiant and must be unique, 1166 mu is the coefficient of friction, 1167 e is the coefficient of restitution on the contact normal, 1168 gid1 and gid2 define the group identifiants. 1169 1170 """ 1171 if name not in self._nslaws_data: 1172 nslaw = self._nslaws_data.create_dataset(name, (0,)) 1173 nslaw.attrs['type'] = 'NewtonImpactFrictionNSL' 1174 nslaw.attrs['mu'] = mu 1175 nslaw.attrs['e'] = e 1176 nslaw.attrs['gid1'] = collision_group1 1177 nslaw.attrs['gid2'] = collision_group2 1178 1179 # Note, default groups are -1 here, indicating not to add them to 1180 # the nslaw lookup table for contacts, since 1D impacts are 1181 # useless in this case. They are however useful for joint stops. 1182 def add_Newton_impact_nsl(self, name, e=0, collision_group1=-1, 1183 collision_group2=-1): 1184 """ 1185 Add a nonsmooth law for contact between 2 groups. 1186 Only NewtonImpactNSL are supported. 1187 name is a user identifier and must be unique, 1188 e is the coefficient of restitution on the contact normal, 1189 gid1 and gid2 define the group identifiers. 1190 1191 As opposed to add_Newton_impact_friction_nsl, the default groups are 1192 -1, making the NSL unassociated with point contacts. It can 1193 by used for joint stops however. 1194 """ 1195 if name not in self._nslaws_data: 1196 nslaw = self._nslaws_data.create_dataset(name, (0,)) 1197 nslaw.attrs['type'] = 'NewtonImpactNSL' 1198 nslaw.attrs['e'] = e 1199 nslaw.attrs['gid1'] = collision_group1 1200 nslaw.attrs['gid2'] = collision_group2 1201 1202 # Note, default groups are -1 here, indicating not to add them to 1203 # the nslaw lookup table for contacts, since 1D impacts are 1204 # useless in this case. They are however useful for joint friction. 1205 def add_relay_nsl(self, name, lb, ub, size=1, collision_group1=-1, 1206 collision_group2=-1): 1207 """ 1208 Add a nonsmooth law for contact between 2 groups. 1209 Only NewtonImpactNSL are supported. 1210 name is a user identifier and must be unique, 1211 e is the coefficient of restitution on the contact normal, 1212 gid1 and gid2 define the group identifiers. 1213 1214 As opposed to add_Newton_impact_friction_nsl, the default groups are 1215 -1, making the NSL unassociated with point contacts. It can 1216 by used for joint stops however. 1217 """ 1218 if name not in self._nslaws_data: 1219 nslaw = self._nslaws_data.create_dataset(name, (0,)) 1220 nslaw.attrs['type'] = 'RelayNSL' 1221 nslaw.attrs['size'] = size 1222 nslaw.attrs['lb'] = lb 1223 nslaw.attrs['ub'] = ub 1224 nslaw.attrs['gid1'] = collision_group1 1225 nslaw.attrs['gid2'] = collision_group2 1226 1227 def add_joint(self, name, object1, object2=None, 1228 points=[[0, 0, 0]], axes=[[0, 1, 0]], 1229 joint_class='PivotJointR', absolute=None, 1230 allow_self_collide=None, nslaws=None, stops=None, 1231 friction=None, coupled=None,references=None): 1232 """ 1233 add a joint between two objects 1234 """ 1235 if name in self.joints(): 1236 raise ValueError('Joint {} already in simulation!'.format(name)) 1237 else: 1238 joint = self.joints().create_dataset(name, (0,)) 1239 joint.attrs['object1'] = object1 1240 if object2 is not None: 1241 joint.attrs['object2'] = object2 1242 joint.attrs['type'] = joint_class 1243 check_points_axes(name, joint_class, points, axes) 1244 if points is not None: 1245 joint.attrs['points'] = points 1246 if axes is not None: 1247 joint.attrs['axes'] = axes 1248 if absolute in [True, False]: 1249 joint.attrs['absolute'] = absolute 1250 1251 if allow_self_collide in [True, False]: 1252 joint.attrs['allow_self_collide'] = allow_self_collide 1253 if nslaws is not None: 1254 # either name of one nslaw, or a list of names same length as stops 1255 joint.attrs['nslaws'] = np.array(nslaws, dtype='S') 1256 if stops is not None: 1257 joint.attrs['stops'] = stops # must be a table of [[axis,pos,dir]..] 1258 if friction is not None: 1259 # must be an NSL name (e.g. RelayNSL), or list of same 1260 joint.attrs['friction'] = np.array(friction, dtype='S') 1261 if coupled is not None: 1262 # must be a list of tuples of two integers (DoF 1263 # indexes) and a float (ratio) 1264 for c in coupled: 1265 assert(len(c) == 3) 1266 joint.attrs['coupled'] = np.array(coupled) 1267 if references is not None: 1268 # must be a list of two joint names and one DS name 1269 assert(len(references) == 2 or len(references)==3) 1270 joint.attrs['references'] = np.array(references, dtype='S') 1271 1272 def add_boundary_condition(self, name, object1, indices=None, 1273 bc_class='HarmonicBC', 1274 v=None, a=None, b=None, omega=None, phi=None): 1275 """ 1276 add boundarycondition to the object object1 1277 1278 implementation only works for HarmonicBC for the moment 1279 """ 1280 if name not in self.boundary_conditions(): 1281 boundary_condition = self.boundary_conditions().create_dataset( 1282 name, (0,)) 1283 boundary_condition.attrs['object1'] = object1 1284 boundary_condition.attrs['indices'] = indices 1285 boundary_condition.attrs['type'] = bc_class 1286 if bc_class == 'HarmonicBC': 1287 boundary_condition.attrs['a'] = a 1288 boundary_condition.attrs['b'] = b 1289 boundary_condition.attrs['omega'] = omega 1290 boundary_condition.attrs['phi'] = phi 1291 elif bc_class == 'BoundaryCondition': 1292 boundary_condition.attrs['v'] = v 1293 elif bc_class == 'FixedBC': 1294 pass # nothing to do 1295 else: 1296 raise NotImplementedError 1297