def main(): # initialization examples_dir = os.path.dirname(__file__) checkpoint = os.path.join(examples_dir, '..', 'weights', 'deeptam_tracker_weights', 'snapshot-300000') datadir = os.path.join(examples_dir, '../../..', 'generating', '_out', 'Town01_0000.npy') # datadir = os.path.join(examples_dir, '../../..', 'generating', '_out', 'kitti', '04.npy') tracking_module_path = os.path.join( examples_dir, '..', 'python/deeptam_tracker/models/networks.py') sequence = np.load(datadir) WIDTH = 320 HEIGHT = 240 fw = WIDTH / (2 * np.tan(90 * np.pi / 360)) fh = HEIGHT / (2 * np.tan(90 * np.pi / 360)) cu = WIDTH / 2 cv = HEIGHT / 2 intrinsics = [[fw, fh, cu, cv]] # use first 3 frames as an example, the fisrt frame is selected as key frame frame_key = sequence[0] print(frame_key[2]) print(Vector3(frame_key[2])) pose_key = Pose(R=Matrix3(frame_key[3]), t=Vector3(frame_key[2])) image_key = np.reshape(frame_key[0], (1, HEIGHT, WIDTH, 3)) frame_1 = sequence[1] pose_1 = Pose(R=Matrix3(frame_1[3]), t=Vector3(frame_1[2])) image_1 = np.reshape(frame_1[0], (1, HEIGHT, WIDTH, 3)) frame_2 = sequence[2] pose_2 = Pose(R=Matrix3(frame_2[3]), t=Vector3(frame_2[2])) image_2 = np.reshape(frame_2[0], (1, HEIGHT, WIDTH, 3)) tracker_core = TrackerCore(tracking_module_path, checkpoint, intrinsics) # set the keyframe of tracker tracker_core.set_keyframe(image_key, np.full((1, 240, 320, 1), 0.01), pose_key) # track frame_1 w.r.t frame_key print('Track frame {} w.r.t key frame:'.format(1)) results_1 = tracker_core.compute_current_pose(image_1, pose_key) pose_pr_1 = results_1['pose'] print('prediction', rotation_matrix_to_angleaxis(pose_pr_1.R), pose_pr_1.t) print('gt', rotation_matrix_to_angleaxis(pose_1.R), pose_1.t) simple_visualization(image_key, image_1, results_1['warped_image'], 1) # track frame_2 w.r.t frame_key incrementally using pose_pr_1 as pose_guess print( 'Track frame {} w.r.t key frame incrementally using previous predicted pose:' .format(2)) results_2 = tracker_core.compute_current_pose(image_2, pose_pr_1) pose_pr_2 = results_2['pose'] print('prediction', rotation_matrix_to_angleaxis(pose_pr_2.R), pose_pr_2.t) print('gt', rotation_matrix_to_angleaxis(pose_2.R), pose_2.t) simple_visualization(image_key, image_2, results_2['warped_image'], 2) del tracker_core
def naive_pose_fusion(cams_poses): ''' Averages the input poses of each camera provided in the list :param cams_poses: list of list of poses for each camera :return: list of poses after fusion ''' from deeptam_tracker.utils.rotation_conversion import rotation_matrix_to_angleaxis, angleaxis_to_rotation_matrix from deeptam_tracker.utils.datatypes import Vector3, Matrix3, Pose assert isinstance(cams_poses, list) assert all( len(cam_poses) == len(cams_poses[0]) for cam_poses in cams_poses) fused_poses = [] num_of_poses = len(cams_poses[0]) for idx in range(num_of_poses): trans = [] orientation_aa = [] for cam_num in range(len(cams_poses)): trans.append(np.array(cams_poses[cam_num][idx].t)) orientation_aa.append( rotation_matrix_to_angleaxis(cams_poses[cam_num][idx].R)) t = np.mean(trans, axis=0) R = angleaxis_to_rotation_matrix( Vector3(np.mean(orientation_aa, axis=0))) fused_poses.append(Pose(R=Matrix3(R), t=Vector3(t))) return fused_poses
def compute_motion_errors(predicted_motion, gt_motion, normalize_translations=True): """ Computes the error of the motion. predicted_motion: 1d numpy array with 6 elements the motions as [aa1, aa2, aa3, tx, ty, tz] aa1,aa2,aa3 is an angle axis representation. The angle is the norm of the axis gt_motion: 1d numpy array with 6 elements ground truth motion in the same format as the predicted motion normalize_translations: bool If True then translations will be normalized before computing the error Returns rotation angular distance in radian tranlation distance of the normalized translations tranlation angular distance in radian """ def _numpy_to_Vector3(arr): tmp = arr.astype(np.float64) return Vector3(tmp[0], tmp[1], tmp[2]) gt_axis = _numpy_to_Vector3(gt_motion[0:3]) gt_angle = gt_axis.norm() if gt_angle < 1e-6: gt_angle = 0 gt_axis = Vector3(1, 0, 0) else: gt_axis.normalize() gt_q = Quaternion(gt_angle, gt_axis) predicted_axis = _numpy_to_Vector3(predicted_motion[0:3]) predicted_angle = predicted_axis.norm() if predicted_angle < 1e-6: predicted_angle = 0 predicted_axis = Vector3(1, 0, 0) else: predicted_axis.normalize() predicted_axis.normalize() predicted_q = Quaternion(predicted_angle, predicted_axis) rotation_angle_dist = gt_q.angularDistance(predicted_q) gt_trans = _numpy_to_Vector3(gt_motion[3:6]) thresh = gt_trans.norm() if normalize_translations: gt_trans.normalize() predicted_trans = _numpy_to_Vector3(predicted_motion[3:6]) if normalize_translations and predicted_trans.norm() > 1e-6: predicted_trans.normalize() translation_dist = (gt_trans - predicted_trans).norm() if thresh>1e-4: translation_angle_diff = math.acos(np.clip(gt_trans.dot(predicted_trans), -1, 1)) else: translation_angle_diff = 0 return np.rad2deg(rotation_angle_dist), translation_dist, np.rad2deg(translation_angle_diff)
def __init__(self, axis=2, useRef=False, origin=Vector3(0, 0, 0), dispScale=1.): """ :param useRef: (bool) use reference positions rather than actual positions :param axis: axis of the cylinder, ∈{0,1,2} """ if axis not in (0, 1, 2): raise IndexError("axis must be one of 0,1,2 (not %d)" % axis) self.useRef, self.axis, self.origin = useRef, axis, Vector3(origin) Flatten.__init__(self, dispScale)
def domainPreProc(self, data): ''' determines the max extends of the domain, the splitting axis is based on the axis of the largest extend. Also used to determine the dimensionality of the configuration, eg: 2D or 3D. ''' domainMin = Vector3(maxVal, maxVal, maxVal) domainMax = Vector3(minVal, minVal, minVal) ndim = -1 symIndex = -1 domain2D = False splitPlaneIndex = -1 maxDim = 3 for x in data: pos = x[0] domainMin[0] = min(domainMin[0], pos[0]) domainMin[1] = min(domainMin[1], pos[1]) domainMin[2] = min(domainMin[2], pos[2]) domainMax[0] = max(domainMax[0], pos[0]) domainMax[1] = max(domainMax[1], pos[1]) domainMax[2] = max(domainMax[2], pos[2]) for j in range(maxDim): if (self.isEqual(domainMin[j], domainMax[j])): domain2D = True symIndex = j ndim = 2 else: ndim = 3 domainAxisL = [abs(domainMax[0] - domainMin[0])] + [ abs(domainMax[1] - domainMin[1]) ] + [abs(domainMax[2] - domainMin[2])] splitPlaneIndex = domainAxisL.index(max(domainAxisL)) #TODO: find minimum of intersection areas between subdomains, the idea is to use this as a condition to determine if subdomains have to be reset. interSectionAreas = [domainAxisL[1] * domainAxisL[2]] + [ domainAxisL[0] * domainAxisL[2] ] + [domainAxisL[0] * domainAxisL[1]] areaMin = None if ndim == 3: areaMin = interSectionAreas.index(min(interSectionAreas)) else: if symIndex == 0: areaMin = min(domainAxisL[1], domainAxisL[2]) if symIndex == 1: areaMin = min(domainAxisL[2], domainAxisL[0]) if symIndex == 2: areaMin = min(domainAxisL[0], domainAxisL[1]) return splitPlaneIndex
def convert_between_c2w_w2c(inp): """Converts camera-to-world pose to world-to-camera pose or vice verse inp: Pose """ T = MatrixX.Identity(4, 4) rot = inp.R trans = inp.t T[0, 0] = rot[0, 0] T[0, 1] = rot[0, 1] T[0, 2] = rot[0, 2] T[1, 0] = rot[1, 0] T[1, 1] = rot[1, 1] T[1, 2] = rot[1, 2] T[2, 0] = rot[2, 0] T[2, 1] = rot[2, 1] T[2, 2] = rot[2, 2] T[0, 3] = trans[0] T[1, 3] = trans[1] T[2, 3] = trans[2] T_inv = T.inverse() rot_inv = rot.transpose() trans_inv = Vector3(T_inv[0, 3], T_inv[1, 3], T_inv[2, 3]) return Pose(R=rot_inv, t=trans_inv)
def makeCloudFromSizes(self, sizes, seed=0, factor=1.05): random.seed(seed) sizes = sorted(sizes, key=lambda s: -s) for i, size in enumerate(sizes): r = .5 * size sph = Sphere(None, r * factor) dmin, dmax = r, SSIZE - r ok = False for i in xrange(1000): pos = [ dmin + (dmax - dmin) * random.random() for _ in (0, 1, 2) ] pos = Vector3(pos) sph.center = pos if any( (sph.center - sph2.center).squaredNorm() < pow(sph.radius + sph2.radius, 2) for sph2 in self.spheres): continue else: ok = True break assert ok, "{} {}".format(i, size) self.spheres.append(sph) for s in self.spheres: s.radius /= factor
def transform_poses_to_base(config, poses): """ Transform the poses to the base frame with settings specified in the config :param config: :param poses: :return: """ ## initialize base to cam transformation tf_t_BC = Vector3(config['base_to_cam_pose']['translation']['x'], config['base_to_cam_pose']['translation']['y'], config['base_to_cam_pose']['translation']['z']) tf_q_BC = Quaternion(config['base_to_cam_pose']['orientation']['w'], config['base_to_cam_pose']['orientation']['x'], config['base_to_cam_pose']['orientation']['y'], config['base_to_cam_pose']['orientation']['z']) tf_R_BC = tf_q_BC.toRotationMatrix() ## transformation of poses tf_poses = [] for pose in poses: t = pose.t - tf_t_BC R = pose.R * tf_R_BC.inverse() tf_pose = Pose(R=R, t=t) tf_poses.append(tf_pose) return tf_poses
def normal(self, pos, vec): ax = Vector3(0, 0, 0) ax[axis] = 1 pos = _getPos(b) circum = ax.Cross(pos) circum.Normalize() return circum.Dot(vec)
def __init__(self, mpiComm): self.comm = mpiComm self.numWorkers = self.comm.Get_size() - 1 # for coloring bodies self.colorscale = [] for i in range(self.numWorkers): self.colorscale.append(Vector3(colorsys.hsv_to_rgb(i*1./self.numWorkers,1,1)))
def v2d(n): lib = self.fem._lib uv = lib.FloatArray() dm = lib.IntArray(3) dm[0] = lib.DofIDItem.D_u dm[1] = lib.DofIDItem.D_v dm[2] = lib.DofIDItem.D_w t = self.fem.problem.giveCurrentStep() n.giveUnknownVector(uv, dm, lib.ValueModeType.VM_Total, t, False) l = list(uv) + (3 - len(uv)) * [0] return Vector3(l)
def v2d(v): lib = self.fem._lib n = d.giveDofManager(v.id) uv = lib.FloatArray() dm = lib.IntArray(3) dm[0] = lib.DofIDItem.D_u dm[1] = lib.DofIDItem.D_v dm[2] = lib.DofIDItem.D_w t = p.giveCurrentStep() n.giveUnknownVector(uv, dm, lib.ValueModeType.VM_Total, t, False) return Vector3(list(uv))
def angleaxis_to_rotation_matrix(aa, epsilon=1e-6): """Converts the angle axis vector with angle encoded as magnitude to the rotation matrix representation. aa: minieigen.Vector3 or np.array axis angle with angle as vector magnitude epsilon: minimum angle in rad If the angle is smaller than epsilon then 0,(1,0,0) will be returned returns the 3x3 rotation matrix as numpy.ndarray """ if not isinstance(aa, Vector3): _tmp = np.squeeze(aa).astype(np.float64) _aa = Vector3(_tmp[0], _tmp[1], _tmp[2]) else: _aa = Vector3(aa) q = angleaxis_to_quaternion(_aa, epsilon) tmp = q.toRotationMatrix() return np.array(tmp)
def __init__(self, config, tracking_module_path, checkpoint, require_depth=False, require_pose=False): """ Creates an object for a single camera tracking instance. It wraps around the DeepTAM Sequence and Tracker classes. This is done to make it easier to extend the setup for a multi-camera tracking class. :param config: (dict) comprising of the camera settings :param tracking_module_path: (str) path to file containing the Tracker network class :param checkpoint: (str) path to the pre-trained network weights :param require_depth: :param require_pose: """ assert isinstance(config, dict) self._startup = False self.name = config['cam_name'] # base to camera transformation self.tf_t_BC = Vector3(config['base_to_cam_pose']['translation']['x'], config['base_to_cam_pose']['translation']['y'], config['base_to_cam_pose']['translation']['z']) self.tf_q_BC = Quaternion( config['base_to_cam_pose']['orientation']['w'], config['base_to_cam_pose']['orientation']['x'], config['base_to_cam_pose']['orientation']['y'], config['base_to_cam_pose']['orientation']['z']) self.tf_R_BC = self.tf_q_BC.toRotationMatrix() self.sequence = RGBDSequence( config['cam_dir'], rgb_parameters=config['rgb_parameters'], depth_parameters=config['depth_parameters'], time_syncing_parameters=config['time_syncing_parameters'], cam_name=config['cam_name'], require_depth=require_depth, require_pose=require_pose) self.intrinsics = self.sequence.get_original_normalized_intrinsics() self.tracker = Tracker( tracking_module_path, checkpoint, self.intrinsics, tracking_parameters=config['tracking_parameters'])
def motion_vector_to_Rt(motion, epsilon=1e-6): """Converts the motion vector to the rotation matrix R and translation t motion: np.ndarray array with 6 elements. The motions is given as [aa1, aa2, aa3, tx, ty, tz]. aa1,aa2,aa3 is an angle axis representation. The angle is the norm of the axis. [tx, ty, tz] is a 3d translation. epsilon: minimum angle in rad If the angle is smaller than epsilon then 0,(1,0,0) will be returned returns the 3x3 rotation matrix and the 3d translation vector """ pass tmp = motion.squeeze().astype(np.float64) t = tmp[3:].copy() R = angleaxis_to_rotation_matrix(Vector3(tmp[0:3]), epsilon) return R, t
def sphere(center,radius,resolution=1): center = Vector3(center) vertices = [ Vector3(+1, 0, 0), Vector3(-1, 0, 0), Vector3( 0, +1, 0), Vector3( 0, -1, 0), Vector3( 0, 0, +1), Vector3( 0, 0, -1), ] cells = [ ( 0, 4, 2 ), ( 2, 4, 1 ), ( 1, 4, 3 ), ( 3, 4, 0 ), ( 0, 2, 5 ), ( 2, 1, 5 ), ( 1, 3, 5 ), ( 3, 0, 5 ), ] # for r in xrange(resolution): newCells = [] centers = {} for i1,i2,i3 in cells: v1,v2,v3 = [vertices[i] for i in (i1,i2,i3)] iExtra = 3*[None] for index in (0,1,2): i = (i1,i2,i3)[index] j = (i1,i2,i3)[(index+1)%3] bij = centers.get((i,j)) bji = centers.get((j,i)) if bij or bji: iExtra[index] = centers[(i,j)] if bij else centers[(j,i)] else: ii = len(vertices) iExtra[index] = ii nn = (vertices[i] + vertices[j]).normalized() centers[(i,j)] = ii vertices.append(nn) i4,i5,i6 = iExtra newCells.extend(( (i1,i4,i6), (i4,i2,i5), (i5,i3,i6), (i4,i5,i6), )) cells = newCells vertices = [center + v*radius for v in vertices] return vertices,cells
def getDsplFromFem(self): p = self.fem.problem d = p.giveDomain(1) def v2d(v): lib = self.fem._lib n = d.giveDofManager(v.id) uv = lib.FloatArray() dm = lib.IntArray(3) dm[0] = lib.DofIDItem.D_u dm[1] = lib.DofIDItem.D_v dm[2] = lib.DofIDItem.D_w t = p.giveCurrentStep() n.giveUnknownVector(uv, dm, lib.ValueModeType.VM_Total, t, False) return Vector3(list(uv)) self.dspl = [Vector3(v2d(v)) for v in self.femMesh.vertices]
def angleaxis_to_angle_axis(aa, epsilon=1e-6): """Converts the angle axis vector with angle encoded as magnitude to the angle axis representation with seperate angle and axis. aa: minieigen.Vector3 axis angle with angle as vector magnitude epsilon: minimum angle in rad If the angle is smaller than epsilon then 0,(1,0,0) will be returned returns the tuple (angle,axis) """ angle = aa.norm() if angle < epsilon: angle = 0 axis = Vector3(1, 0, 0) else: axis = aa.normalized() return angle, axis
def toUnstructuredGrid(self): d = self.problem.giveDomain(1) # vertices = {} for i in xrange(d.numberOfDofManagers): n = d.giveDofManager(i + 1) id = n.number coords = Vector3(n.coordinates) v = Vertex(id, coords) vertices[id] = v # cells = [] for i in xrange(d.giveNumberOfElements()): e = d.giveElement(i + 1) id = e.number nis = e.giveDofManArray() vs = [vertices[ni] for ni in nis] t = self.geomType2type[e.geometryType] c = Cell(id, t, vs) cells.append(c) # vertices = vertices.values() return UnstructuredGrid(vertices, cells)
def normal(self, b, vec): pos = self._getPos(b) ax = Vector3(0, 0, 0) ax[axis] = 1 circum = ax.cross(pos).normalized() return circum.Dot(vec)
dmgTensor((h, h, h), p) dmgTensor((1, 1, 0), p) dmgTensor((h, h, 0), p) dmgTensor((1, 0, 0), p) dmgTensor((h, 0, 0), p) print print 'test many intrs' import random random.seed(1) normals = [] while len(normals) < 10000: x, y, z = [2 * random.random() - 1 for _ in (0, 1, 2)] if x * x + y * y + z * z > 1: continue normals.append(Vector3(x, y, z).normalized()) dd = Matrix3(1, 0, 0, 0, 0, 0, 0, 0, 0) dd = Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1) dd = Matrix3(1, 0, .5, 0, .5, 0, 0, .8, .25) dd = .5 * (dd + dd.transpose()) intrs = [Interaction(n, 0) for n in normals] for i in intrs: n = i.normal i.damage = n.dot(dd * n) p = Particle(intrs) d = p.computeDamageTensor() print dd print d print sorted(dd.spectralDecomposition()[1]) print sorted(d.spectralDecomposition()[1])
def __init__(self, id, coords): self.id = id self.coords = Vector3(coords) # list of cells which the vertex belongs to self.cells = []
class EllGroup(woo.core.Preprocessor, woo.pyderived.PyWooObject): 'Simulation of group of ellipsoids moving in 2-dimensional box.' _classTraits = None _PAT = woo.pyderived.PyAttrTrait # less typing _attrTraits = [ _PAT( Vector2, 'rRange', Vector2(.02, .04), unit='m', doc= 'Range (minimum and maximum) for particle radius (greatest semi-axis); if both are the same, all particles will have the same radius.' ), _PAT(bool, 'spheres', False, doc='Use spherical particles instead of elliptical'), _PAT( float, 'semiMinRelRnd', 0, hideIf='self.spheres', doc= 'Minimum semi-axis length relative to particle radius; minor semi-axes are randomly selected from (:obj:`semiMinRelRnd` … 1) × greatest semi-axis. If non-positive, :obj:`semiRelFixed` is used instead.' ), _PAT( Vector3, 'semiRelFixed', Vector3(1., .5, .5), hideIf='self.spheres', doc= 'Fixed sizes of semi-axes relative (all elements should be ≤ 1). The :math:`z`-component is the out-of-plane size which only indirectly influences contact stiffnesses. This variable is only used if semi-axes are not assigned randomly (see :obj:`semiMinRelRnd`).' ), _PAT(Vector2, 'boxSize', Vector2(2, 2), unit='m', doc='Size of the 2d domain in which particles move.'), _PAT( float, 'vMax', 1., unit='m/s', doc= 'Maximum initial velocity of particle; assigned randomly from 0 to this value; intial angular velocity of all particles is zero.' ), _PAT( woo.models.ContactModelSelector, 'model', woo.models.ContactModelSelector(name='Schwarz', restitution=1., damping=0.01, numMat=(1, 2), matDesc=['ellipsoids', 'walls'], mats=[ woo.dem.HertzMat(density=5000, young=1e6, tanPhi=0.0, surfEnergy=4., alpha=.6) ]), doc= 'Select contact model. The first material is for particles; the second, optional, material is for walls at the boundary (the first material is used if there is no second one).' ), _PAT( str, 'exportFmt', "/tmp/ell2d-{tid}-", filename=True, doc= "Prefix for saving :obj:`woo.dem.VtkExport` data, and :obj:`woo.pre.ell2d.ell2plot` data; formatted with ``format()`` providing :obj:`woo.core.Scene.tags` as keys." ), _PAT( int, 'vtkStep', 0, "How often should :obj:`woo.dem.VtkExport` run. If non-positive, never run the export." ), _PAT( int, 'vtkEllLev', 1, 'Tesselation level of ellipsoids when expored as VTK meshes (see :obj:`woo.dem.VtkExport.ellLev`).' ), _PAT( int, 'ell2Step', 0, "How often should :obj:`woo.pre.ell2d.ell2plot` run. If non-positive, never run that one." ), _PAT( float, 'dtSafety', .5, 'Safety coefficient for critical timestep; should be smaller than one.' ), ] def __init__(self, **kw): woo.core.Preprocessor.__init__(self) self.wooPyInit(self.__class__, woo.core.Preprocessor, **kw) def __call__(self): import woo woo.master.usesApi = 10102 # preprocessor builds the simulation when called pre = self # more readable S = woo.core.Scene(fields=[woo.dem.DemField()], pre=self.deepcopy()) # material definitions ellMat = pre.model.mats[0] wallMat = (pre.model.mats[1] if len(pre.model.mats) > 1 else ellMat) ZZ = pre.rRange[1] * 3 # only generate spheres randomly in 2d box S.engines = [ woo.dem.BoxInlet2d( axis=2, box=((0, 0, ZZ), (pre.boxSize[0], pre.boxSize[1], ZZ)), materials=[ellMat], generator=woo.dem.MinMaxSphereGenerator(dRange=2 * pre.rRange), massRate=0), woo.dem.InsertionSortCollider([woo.dem.Bo1_Sphere_Aabb()]) ] S.one() posRad = [(p.pos, p.shape.radius) for p in S.dem.par] # clear the dem field S.fields = [woo.dem.DemField()] import random def rndOri2d(): q = Quaternion((0, 0, 1), 2 * math.pi * random.random()) q.normalize() return q S.energy['kin0'] = 0 for pos, rad in posRad: if not pre.spheres: if pre.semiMinRelRnd > 0: semiAxes = [ random.uniform(pre.semiMinRelRnd, 1) * rad for i in (0, 1, 2) ] else: semiAxes = Vector3(pre.semiRelFixed) * rad p = woo.utils.ellipsoid(center=pos, semiAxes=semiAxes, ori=rndOri2d(), mat=ellMat) else: p = woo.utils.sphere(center=pos, radius=rad, mat=ellMat) p.vel = rndOri2d() * Vector3(pre.vMax * random.random(), 0, 0) S.energy['kin0'] -= p.Ek p.blocked = 'zXY' S.dem.par.add(p) #for coord,axis,sense in [(0,0,+1),(pre.boxSize[0],0,-1),(0,1,+1),(pre.boxSize[1],1,-1)]: # S.dem.par.add(woo.utils.wall(coord,axis=axis,sense=sense,mat=wallMat,visible=False)) S.dem.par.add([ woo.dem.Wall.make(0, axis=0, mat=wallMat, visible=True), woo.dem.Wall.make(0, axis=1, mat=wallMat, visible=True) ]) S.periodic = True S.cell.setBox((pre.boxSize[0], pre.boxSize[1], 2 * ZZ)) S.engines = woo.dem.DemField.minimalEngines( model=pre.model, dynDtPeriod=10 ) + [ # trace particles and color by z-angVel woo.dem.Tracer(num=100, compress=4, compSkip=1, glSmooth=True, glWidth=2, scalar=woo.dem.Tracer.scalarAngVel, vecAxis=2, stepPeriod=40, minDist=pre.rRange[0]), woo.core.PyRunner( 100, 'S.plot.addData(i=S.step,t=S.time,total=S.energy.total(),relErr=(S.energy.relErr() if S.step>100 else 0),**S.energy)' ), woo.dem.VtkExport(stepPeriod=pre.vtkStep, out=pre.exportFmt, ellLev=pre.vtkEllLev, dead=(pre.vtkStep <= 0)), woo.core.PyRunner( pre.ell2Step, 'import woo.pre.ell2d; mx=woo.pre.ell2d.ell2plot(out="%s-%05d.png"%(S.expandTags(S.pre.exportFmt),engine.nDone),S=S,colorRange=(0,S.lab.maxEll2Color),bbox=((0,0),S.pre.boxSize)); S.lab.maxEll2Color=max(mx,S.lab.maxEll2Color)', dead=(pre.ell2Step <= 0)), woo.dem.WeirdTriaxControl(goal=(-0, -0, 0), stressMask=0, maxStrainRate=(.1, .1, 0), mass=1., label='triax'), ] S.lab.leapfrog.kinSplit = True S.dtSafety = pre.dtSafety S.trackEnergy = True S.uiBuild = 'import woo.pre.ell2d; woo.pre.ell2d.ellGroupUiBuild(S,area)' S.lab.maxEll2Color = 0. # max |angVel| for the start when plotting S.plot.plots = {'i': ('total', '**S.energy'), ' t': ('relErr')} S.plot.data = { 'i': [nan], 'total': [nan], 'relErr': [nan] } # to make plot displayable from the very start try: import woo.gl S.gl.renderer = woo.gl.Renderer(iniUp=(0, 1, 0), iniViewDir=(0, 0, -1), grid=4) except ImportError: pass return S
def __call__(self): import woo woo.master.usesApi = 10102 # preprocessor builds the simulation when called pre = self # more readable S = woo.core.Scene(fields=[woo.dem.DemField()], pre=self.deepcopy()) # material definitions ellMat = pre.model.mats[0] wallMat = (pre.model.mats[1] if len(pre.model.mats) > 1 else ellMat) ZZ = pre.rRange[1] * 3 # only generate spheres randomly in 2d box S.engines = [ woo.dem.BoxInlet2d( axis=2, box=((0, 0, ZZ), (pre.boxSize[0], pre.boxSize[1], ZZ)), materials=[ellMat], generator=woo.dem.MinMaxSphereGenerator(dRange=2 * pre.rRange), massRate=0), woo.dem.InsertionSortCollider([woo.dem.Bo1_Sphere_Aabb()]) ] S.one() posRad = [(p.pos, p.shape.radius) for p in S.dem.par] # clear the dem field S.fields = [woo.dem.DemField()] import random def rndOri2d(): q = Quaternion((0, 0, 1), 2 * math.pi * random.random()) q.normalize() return q S.energy['kin0'] = 0 for pos, rad in posRad: if not pre.spheres: if pre.semiMinRelRnd > 0: semiAxes = [ random.uniform(pre.semiMinRelRnd, 1) * rad for i in (0, 1, 2) ] else: semiAxes = Vector3(pre.semiRelFixed) * rad p = woo.utils.ellipsoid(center=pos, semiAxes=semiAxes, ori=rndOri2d(), mat=ellMat) else: p = woo.utils.sphere(center=pos, radius=rad, mat=ellMat) p.vel = rndOri2d() * Vector3(pre.vMax * random.random(), 0, 0) S.energy['kin0'] -= p.Ek p.blocked = 'zXY' S.dem.par.add(p) #for coord,axis,sense in [(0,0,+1),(pre.boxSize[0],0,-1),(0,1,+1),(pre.boxSize[1],1,-1)]: # S.dem.par.add(woo.utils.wall(coord,axis=axis,sense=sense,mat=wallMat,visible=False)) S.dem.par.add([ woo.dem.Wall.make(0, axis=0, mat=wallMat, visible=True), woo.dem.Wall.make(0, axis=1, mat=wallMat, visible=True) ]) S.periodic = True S.cell.setBox((pre.boxSize[0], pre.boxSize[1], 2 * ZZ)) S.engines = woo.dem.DemField.minimalEngines( model=pre.model, dynDtPeriod=10 ) + [ # trace particles and color by z-angVel woo.dem.Tracer(num=100, compress=4, compSkip=1, glSmooth=True, glWidth=2, scalar=woo.dem.Tracer.scalarAngVel, vecAxis=2, stepPeriod=40, minDist=pre.rRange[0]), woo.core.PyRunner( 100, 'S.plot.addData(i=S.step,t=S.time,total=S.energy.total(),relErr=(S.energy.relErr() if S.step>100 else 0),**S.energy)' ), woo.dem.VtkExport(stepPeriod=pre.vtkStep, out=pre.exportFmt, ellLev=pre.vtkEllLev, dead=(pre.vtkStep <= 0)), woo.core.PyRunner( pre.ell2Step, 'import woo.pre.ell2d; mx=woo.pre.ell2d.ell2plot(out="%s-%05d.png"%(S.expandTags(S.pre.exportFmt),engine.nDone),S=S,colorRange=(0,S.lab.maxEll2Color),bbox=((0,0),S.pre.boxSize)); S.lab.maxEll2Color=max(mx,S.lab.maxEll2Color)', dead=(pre.ell2Step <= 0)), woo.dem.WeirdTriaxControl(goal=(-0, -0, 0), stressMask=0, maxStrainRate=(.1, .1, 0), mass=1., label='triax'), ] S.lab.leapfrog.kinSplit = True S.dtSafety = pre.dtSafety S.trackEnergy = True S.uiBuild = 'import woo.pre.ell2d; woo.pre.ell2d.ellGroupUiBuild(S,area)' S.lab.maxEll2Color = 0. # max |angVel| for the start when plotting S.plot.plots = {'i': ('total', '**S.energy'), ' t': ('relErr')} S.plot.data = { 'i': [nan], 'total': [nan], 'relErr': [nan] } # to make plot displayable from the very start try: import woo.gl S.gl.renderer = woo.gl.Renderer(iniUp=(0, 1, 0), iniViewDir=(0, 0, -1), grid=4) except ImportError: pass return S
def __init__(self, normal, damage): self.normal = Vector3(normal) self.damage = float(damage)
def __init__(self, f, c, sn, ss, cs): self.f = Vector3(f) self.c = c self.sn = list(sn) self.ss = list(ss) self.cs = list(cs)
def __call__(self): d = 100 c = (120, 120) al = .3 * d r = .25 * d paper.rect(c[0] - .5 * d, c[1] - .5 * d, d, d).fill("#ddd") if self.f[0]: paper.path(["M", c[0] - .5 * al, c[1], "h", al]).thick().arrow(1 if self.f[0] > 0 else 2) if self.f[1]: paper.path(["M", c[0], c[1] + .5 * al, "v", -al]).thick().arrow(1 if self.f[1] > 0 else 2) if self.c: paper.path(["M", c[0], c[1] - r, "a", r, r, 0, 0, 0, 0, 2 * r]).thick().arrow(1 if self.c > 0 else 2) # dd = .58 * d if self.sn[0]: paper.path(["M", c[0] + dd, c[1], "h", +al]).thick().arrow(1 if self.sn[0] > 0 else 2) if self.sn[1]: paper.path(["M", c[0] - dd, c[1], "h", -al]).thick().arrow(1 if self.sn[1] > 0 else 2) if self.sn[2]: paper.path(["M", c[0], c[1] + dd, "v", +al]).thick().arrow(1 if self.sn[2] > 0 else 2) if self.sn[3]: paper.path(["M", c[0], c[1] - dd, "v", -al]).thick().arrow(1 if self.sn[3] > 0 else 2) # if self.ss[0]: paper.path(["M", c[0] + dd, c[1] + .5 * al, "v", -al]).thick().arrow(1 if self.ss[0] > 0 else 2) if self.ss[1]: paper.path(["M", c[0] - dd, c[1] - .5 * al, "v", +al]).thick().arrow(1 if self.ss[1] > 0 else 2) if self.ss[2]: paper.path(["M", c[0] - .5 * al, c[1] - dd, "h", +al]).thick().arrow(1 if self.ss[2] > 0 else 2) if self.ss[3]: paper.path(["M", c[0] + .5 * al, c[1] + dd, "h", -al]).thick().arrow(1 if self.ss[3] > 0 else 2) # dd = .8 * d if self.cs[0]: paper.path( ["M", c[0] + dd, c[1] - r, "a", r, r, 0, 0, 1, 0, 2 * r]).thick().arrow(2 if self.cs[0] > 0 else 1) if self.cs[1]: paper.path( ["M", c[0] - dd, c[1] - r, "a", r, r, 0, 0, 0, 0, 2 * r]).thick().arrow(1 if self.cs[1] > 0 else 2) if self.cs[2]: paper.path( ["M", c[0] - r, c[1] - dd, "a", r, r, 0, 0, 1, 2 * r, 0]).thick().arrow(2 if self.cs[2] > 0 else 1) if self.cs[3]: paper.path( ["M", c[0] - r, c[1] + dd, "a", r, r, 0, 0, 0, 2 * r, 0]).thick().arrow(1 if self.cs[3] > 0 else 2) # xs = [Vector3(x, y, 0) for x, y in ((1, 0), (-1, 0), (0, 1), (0, -1))] + [Vector3.Zero] ddd = Vector3(1, 2, 3) xs2 = [x + ddd for x in xs] dirs = xs fns = [d * f for d, f in zip(dirs, self.sn)] dirs = [ Vector3(x, y, 0) for x, y in ((0, 1), (0, -1), (1, 0), (-1, 0)) ] fss = [d * f for d, f in zip(dirs, self.ss)] fs = [fn + fs for fn, fs in zip(fns, fss)] + [self.f] dirs = [Vector3(0, 0, 1) for _ in (1, 2, 3, 4)] cs = [d * c for d, c in zip(dirs, self.cs)] + [Vector3(0, 0, self.c)] stress = sum((x.outer(f) for x, f in zip(xs, fs)), Matrix3.Zero) stress2 = sum((x.outer(f) for x, f in zip(xs2, fs)), Matrix3.Zero) assert (stress - stress2).maxAbsCoeff() < 1e-12, "{} {}".format( stress, stress2) sa = .5 * (stress - stress.transpose()) sa = 2 * Vector3(sa[1, 2], sa[2, 0], sa[0, 1]) cstress = sum((x.outer(c) for x, c in zip(xs, cs)), Matrix3.Zero) cstress2 = sum( (x.outer(c) for x, c in zip(xs2, cs)), Matrix3.Zero) + ddd.outer(sa) assert (cstress - cstress2).maxAbsCoeff() < 1e-12, "{} {}".format( cstress, cstress2) print stress print cstress
def numpy_to_Vector3(arr): tmp = arr.astype(np.float64) return Vector3(tmp[0], tmp[1], tmp[2])
def data(scene, extractor, flattener, con=False, onlyDynamic=True, stDev=None, relThreshold=3., perArea=0, div=(50, 50), margin=(0, 0), radius=1): """Filter all particles/contacts, project them to 2d and extract required scalar value; return either discrete array of positions and values, or smoothed data, depending on whether the stDev value is specified. The ``con`` parameter determines whether we operate on particles or contacts; the extractor provided should expect to receive body/interaction. :param callable extractor: receives :obj:`woo.dem.Particle` (or :obj:`woo.dem.Contact`, if ``con`` is ``True``) instance, should return scalar, a 2-tuple (vector fields) or None (to skip that body/interaction) :param callable flattener: :obj:`woo.post2d.Flatten` instance, receiving body/interaction, returns its 2d coordinates or ``None`` (to skip that body/interaction) :param bool con: operate on contacts rather than particles :param bool onlyDynamic: skip all non-dynamic particles :param float/None stDev: standard deviation for averaging, enables smoothing; ``None`` (default) means raw mode, where discrete points are returned :param float relThreshold: threshold for the gaussian weight function relative to stDev (smooth mode only) :param int perArea: if 1, compute weightedSum/weightedArea rather than weighted average (weightedSum/sumWeights); the first is useful to compute average stress; if 2, compute averages on subdivision elements, not using weight function :param (int,int) div: number of cells for the gaussian grid (smooth mode only) :param (float,float) margin: x,y margins around bounding box for data (smooth mode only) :param float/callable radius: Fallback value for radius (for raw plotting) for non-spherical particles or contacts; if a callable, receives body/interaction and returns radius :return: dictionary Returned dictionary always containing keys 'type' (one of 'rawScalar','rawVector','smoothScalar','smoothVector', depending on value of smooth and on return value from extractor), 'x', 'y', 'bbox'. Raw data further contains 'radii'. Scalar fields contain 'val' (value from *extractor*), vector fields have 'valX' and 'valY' (2 components returned by the *extractor*). """ from minieigen import Vector3 from woo.dem import Sphere xx, yy, dd1, dd2, rr = [], [], [], [], [] nDim = 0 objects = scene.dem.con if con else scene.dem.par for b in objects: if not con and (len(b.shape.nodes) != 1 or (onlyDynamic and b.blocked == 'xyzXYZ')): continue xy, d = flattener(b), extractor(b) if xy == None or d == None or math.isnan(d): continue if nDim == 0: nDim = 1 if isinstance(d, float) else 2 if nDim == 1: dd1.append(d) elif len(d) == 2: dd1.append(d[0]) dd2.append(d[1]) elif len(d) == 3: d1, d2 = flattener.planar(b, Vector3(d)) dd1.append(d1) dd2.append(d2) else: raise RuntimeError( "Extractor must return float or 2 or 3 (not %d) floats" % nDim) if stDev == None: # radii are needed in the raw mode exclusively if nDim == 1 and math.isnan(d): rr.append(nan) # add NaN if value was also invalid else: if not con and isinstance(b.shape, Sphere): r = b.shape.radius else: r = (radius(b) if callable(radius) else radius) rr.append(r) xx.append(xy[0]) yy.append(xy[1]) if stDev == None: bbox = (min(xx), min(yy)), (max(xx), max(yy)) if nDim == 1: return { 'type': 'rawScalar', 'x': xx, 'y': yy, 'val': dd1, 'radii': rr, 'bbox': bbox } else: return { 'type': 'rawVector', 'x': xx, 'y': yy, 'valX': dd1, 'valY': dd2, 'radii': rr, 'bbox': bbox } from woo.WeightedAverage2d import GaussAverage import numpy if min(len(xx), len(yy) == 0): raise RuntimeError( 'Nothing to plot (extractor did not return any values)') lo, hi = (min(xx), min(yy)), (max(xx), max(yy)) llo = lo[0] - margin[0], lo[1] - margin[1] hhi = hi[0] + margin[0], hi[1] + margin[1] ga = GaussAverage(llo, hhi, div, stDev, relThreshold) ga2 = GaussAverage(llo, hhi, div, stDev, relThreshold) for i in range(0, len(xx)): ga.add(dd1[i], (xx[i], yy[i])) if nDim > 1: ga2.add(dd2[i], (xx[i], yy[i])) step = [(hhi[i] - llo[i]) / float(div[i]) for i in [0, 1]] xxx, yyy = [ numpy.arange(llo[i] + .5 * step[i], hhi[i], step[i]) for i in [0, 1] ] ddd = numpy.zeros((len(yyy), len(xxx)), float) ddd2 = numpy.zeros((len(yyy), len(xxx)), float) # set the type of average we are going to use if perArea == 0: def compAvg(gauss, coord, cellCoord): return float(gauss.avg(coord)) elif perArea == 1: def compAvg(gauss, coord, cellCoord): return gauss.avgPerUnitArea(coord) elif perArea == 2: def compAvg(gauss, coord, cellCoord): s = gauss.cellSum(cellCoord) return (s / gauss.cellArea) if s > 0 else nan elif perArea == 3: def compAvg(gauss, coord, cellCoord): s = gauss.cellSum(cellCoord) return s if s > 0 else nan else: raise RuntimeError( 'Invalid value of *perArea*, must be one of 0,1,2,3.') # for cx in range(0, div[0]): for cy in range(0, div[1]): ddd[cy, cx] = compAvg(ga, (xxx[cx], yyy[cy]), (cx, cy)) if nDim > 1: ddd2[cy, cx] = compAvg(ga2, (xxx[cx], yyy[cy]), (cx, cy)) if nDim == 1: return { 'type': 'smoothScalar', 'x': xxx, 'y': yyy, 'val': ddd, 'bbox': (llo, hhi), 'perArea': perArea, 'grid': ga } else: return { 'type': 'smoothVector', 'x': xxx, 'y': yyy, 'valX': ddd, 'valY': ddd2, 'bbox': (llo, hhi), 'grid': ga, 'grid2': ga2 }
""""these are color vectors converted from hex color codes, used for coloring bodies in subdomains. This removes the dependecy from the colorsys module as, we had issues with the same when running MPI tests in the pipeline """ from minieigen import Vector3 #128 colors! col = [ Vector3(0, 0, 0), Vector3(1, 1, 0), Vector3(0.10980392156862745, 0.9019607843137255, 1), Vector3(1, 0.20392156862745098, 1), Vector3(1, 0.2901960784313726, 0.27450980392156865), Vector3(0, 0.5372549019607843, 0.2549019607843137), Vector3(0, 0.43529411764705883, 0.6509803921568628), Vector3(0.6392156862745098, 0, 0.34901960784313724), Vector3(1, 0.8588235294117647, 0.8980392156862745), Vector3(0.47843137254901963, 0.28627450980392155, 0), Vector3(0, 0, 0.6509803921568628), Vector3(0.38823529411764707, 1, 0.6745098039215687), Vector3(0.7176470588235294, 0.592156862745098, 0.3843137254901961), Vector3(0, 0.30196078431372547, 0.2627450980392157), Vector3(0.5607843137254902, 0.6901960784313725, 1), Vector3(0.6, 0.49019607843137253, 0.5294117647058824), Vector3(0.35294117647058826, 0, 0.027450980392156862), Vector3(0.5019607843137255, 0.5882352941176471, 0.5764705882352941), Vector3(0.996078431372549, 1, 0.9019607843137255), Vector3(0.10588235294117647, 0.26666666666666666, 0), Vector3(0.30980392156862746, 0.7764705882352941, 0.00392156862745098), Vector3(0.23137254901960785, 0.36470588235294116, 1), Vector3(0.2901960784313726, 0.23137254901960785, 0.3254901960784314), Vector3(1, 0.1843137254901961, 0.5019607843137255), Vector3(0.3803921568627451, 0.3803921568627451, 0.35294117647058826),