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),