def vec_dihedral(v1,v2,v3,v4):
'''
* Usage
v1 = np.array([0,0,0])
v2 = np.array([1,1,1]) #
v3 = np.array([2,2,2]) # The v2--v3 bond is rotated around
v4 = np.array([3,3,3])
phi = vec_dihedral(v1,v2,v3)
* Input
v1,v2,v3 - 1D Numpy array
Vectors must have the same number of elements
* Output
Floating point number which is the angle in degrees
* Description
v1,v2,v3,v4 refers to the vector of Cartesian coordinates describing each point
vec1, vec2, etc... refers to the vector between 2 points, e.g, vec1 = v1-v2
'''
a1 = v2-v1
a2 = v3-v2
a3 = v4-v3
v1 = np.cross(a1, a2)
v2 = np.cross(a2, a3)
v1 = v1 / (v1 * v1).sum()**0.5
v2 = v2 / (v2 * v2).sum()**0.5
porm = np.sign((v1 * a3).sum())
rad = np.arccos((v1*v2).sum() / ((v1**2).sum() * (v2**2).sum())**0.5)
angle = porm * rad
if np.isnan(angle):
angle = 1.0
return math.degrees(angle)
def getDihedralAngle(self,dihedral):
"""Calculates the dihedral angle of a given dihedral.
Parameters
----------
dihedral : Dihedral
Dihedral Object that one wishes to calculate the dihedral angle of.
Returns
-------
float
The current dihedral angle of the Dihedral object
"""
atom1 = self.getAtomByID(dihedral.atom1)
atom2 = self.getAtomByID(dihedral.atom2)
atom3 = self.getAtomByID(dihedral.atom3)
atom4 = self.getAtomByID(dihedral.atom4)
b1 = atom2.position-atom1.position
b2 = atom3.position-atom2.position
b3 = atom4.position-atom3.position
b2norm = b2/np.linalg.norm(b2)
n1 = np.cross(b1,b2)/np.linalg.norm(np.cross(b1,b2))
n2 = np.cross(b2,b3)/np.linalg.norm(np.cross(b2,b3))
m1 = np.cross(n1,n2)
angle = atan2(np.dot(m1,b2norm),np.dot(n1,n2))
angle=angle%(2*pi)
return angle
def get_new_cell(self):
"""Returns new basis vectors"""
a = np.sqrt(self.a)
b = np.sqrt(self.b)
c = np.sqrt(self.c)
ad = self.atoms.cell[0] / np.linalg.norm(self.atoms.cell[0])
Z = np.cross(self.atoms.cell[0], self.atoms.cell[1])
Z /= np.linalg.norm(Z)
X = ad - np.dot(ad, Z) * Z
X /= np.linalg.norm(X)
Y = np.cross(Z, X)
alpha = np.arccos(self.x / (2 * b * c))
beta = np.arccos(self.y / (2 * a * c))
gamma = np.arccos(self.z / (2 * a * b))
va = a * np.array([1, 0, 0])
vb = b * np.array([np.cos(gamma), np.sin(gamma), 0])
cx = np.cos(beta)
cy = (np.cos(alpha) - np.cos(beta) * np.cos(gamma)) \
/ np.sin(gamma)
cz = np.sqrt(1. - cx * cx - cy * cy)
vc = c * np.array([cx, cy, cz])
abc = np.vstack((va, vb, vc))
T = np.vstack((X, Y, Z))
return np.dot(abc, T)
def get_orientation_from_lines(self, v0, v1):
"""
Takes in two vectors, v0 and v1, (which are KNOWN to be in the same plane) and finds
the normal, and creates a rotation matrix from v0, v1, and the normal; then converts this
rotation matrix to a quaternion
"""
v0, v1 = np.array(v0), np.array(v1)
v0 = v0 / np.linalg.norm(v0)
v1 = v1 / np.linalg.norm(v1)
n = np.cross(v0, v1)
parallel = average_vectors(v0, v1)
parallel = parallel / np.linalg.norm(parallel)
third = np.cross(n, parallel)
third = third / np.linalg.norm(third)
#n = n / np.linalg.norm(n)
#v1 = np.cross(n, v0)
#v1 = v1 / np.linalg.norm(v1)
#rotMat = np.vstack((n, v1, v0)).T
rotMat = np.vstack((parallel, third, n)).T
matrix = rotMat
tbRot = tfx.tb_angles(matrix).matrix
#tbRot = self.rotate(-90, "yaw", tbRot) #FIXME: get correct yaw pitch roll values
#tbRot = self.rotate(60, "pitch", tbRot)
tbRot = self.rotate(180, "roll", tbRot)
quat = tfx.tb_angles(tbRot).quaternion
return list(quat)
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
and the equations of a table its resting on, and the general direction of the robot
Fit a barrell drill
'''
if not drillPoints.GetNumberOfPoints():
return
vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])
if not drillBarrelPoints.GetNumberOfPoints():
return
# fit line to drill barrel points
linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)
if np.dot(lineDirection, forwardDirection) < 0:
lineDirection = -lineDirection
vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)
pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')
dists = np.dot(pts-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
p1 = projectPointToPlane(p1, plane_origin, plane_normal)
p2 = projectPointToPlane(p2, plane_origin, plane_normal)
d = DebugData()
d.addSphere(p1, radius=0.01)
d.addSphere(p2, radius=0.01)
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)
drillToBasePoint = np.array([-0.07, 0.0 , -0.12])
zaxis = plane_normal
xaxis = lineDirection
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
t = getTransformFromAxes(xaxis, yaxis, zaxis)
t.PreMultiply()
t.Translate(-drillToBasePoint)
t.PostMultiply()
t.Translate(p1)
return t
def __init__(self, joint):
GenericObject.__init__(self)
children_joints = joint.get_children_joints()
for child in children_joints:
child.init_local_transformation()
p = child.localTransformation[:3,3]
translation = p*0.5
height = numpy.linalg.norm(p)
if height < 1e-6:
continue
radius = 0.005
new_y = p/numpy.linalg.norm(p)
new_x = numpy.cross(new_y, numpy.array([1,0,0]))
new_x_norm = numpy.linalg.norm(new_x)
# if new_y is colinear to numpy.array([1,0,0])
# recompute new_x
if new_x_norm < 1e-6:
new_x = numpy.cross(new_y, numpy.array([0,1,0]))
new_x_norm = numpy.linalg.norm(new_x)
new_x /= new_x_norm
new_z = numpy.cross(new_x, new_y)
trans_T = numpy.eye(4)
trans_T[:3,0] = new_x
trans_T[:3,1] = new_y
trans_T[:3,2] = new_z
try:
angle, direc, point = tf.rotation_from_matrix(trans_T)
except:
logger.exception("Error in creating link {0}-{1}".format(joint.name,
child.name))
else:
rotation = [direc[0], direc[1], direc[2], angle]
link = Shape()
link.enabled = False
link.appearance.material.diffuseColor = [0,0.5,1]
link.geometry = Cylinder()
link.geometry.height = height
link.geometry.radius = radius
link.rotation = rotation
link.translation = translation
link.init()
self.add_child(link)
motor = Shape()
motor.enabled = False
self.add_child(motor)
motor.appearance.material.diffuseColor = [1,0,0]
if joint.jointAxis in ["X","x","Y","y","z","Z"]:
motor.geometry = Cylinder()
motor.geometry.height = 0.005
motor.geometry.radius = 0.02
if joint.jointAxis in ["X","x"]:
motor.rotation = [0, 0, 1, 1.5708]
elif joint.jointAxis in ["Z","z"]:
motor.rotation = [0, 1, 0, 1.5708]
else:
motor.geometry = Sphere()
motor.geometry.radius = 0.02
def rotation_matrix(a1, a2, b1, b2):
"""Returns a rotation matrix that rotates the vectors *a1* in the
direction of *a2* and *b1* in the direction of *b2*.
In the case that the angle between *a2* and *b2* is not the same
as between *a1* and *b1*, a proper rotation matrix will anyway be
constructed by first rotate *b2* in the *b1*, *b2* plane.
"""
a1 = np.asarray(a1, dtype=float) / np.linalg.norm(a1)
b1 = np.asarray(b1, dtype=float) / np.linalg.norm(b1)
c1 = np.cross(a1, b1)
c1 /= np.linalg.norm(c1) # clean out rounding errors...
a2 = np.asarray(a2, dtype=float) / np.linalg.norm(a2)
b2 = np.asarray(b2, dtype=float) / np.linalg.norm(b2)
c2 = np.cross(a2, b2)
c2 /= np.linalg.norm(c2) # clean out rounding errors...
# Calculate rotated *b2*
theta = np.arccos(np.dot(a2, b2)) - np.arccos(np.dot(a1, b1))
b3 = np.sin(theta) * a2 + np.cos(theta) * b2
b3 /= np.linalg.norm(b3) # clean out rounding errors...
A1 = np.array([a1, b1, c1])
A2 = np.array([a2, b3, c2])
R = np.linalg.solve(A1, A2).T
return R
def test_A(self):
cid0 = CORD2R()
Lx = 2.
Ly = 0.
Lz = 3.
Fy = 1.
origin = array([-Lx, 0., -Lz])
z_axis = origin + array([0., 0., 1.])
xz_plane = origin + array([1., 0., 1.])
rid = 0
data = [1, rid] + list(origin) + list(z_axis) + list(xz_plane)
Fxyz = [0., -Fy, 0.]
Mxyz = [0., 0., 0.]
cid_new = CORD2R(data=data)
model = None
Fxyz_local, Mxyz_local = TransformLoadWRT(Fxyz, Mxyz, cid0, cid_new,
model, is_cid_int=False)
r = array([Lx, Ly, Lz])
F = array([0., -Fy, 0.])
M = cross(r, F)
self.assertTrue(array_equal(Fxyz_local, F)), "expected=%s actual=%s" % (F, Fxyz_local)
self.assertTrue(array_equal(Mxyz_local, cross(r, F))), "expected=%s actual=%s" % (M, Mxyz_local)
def SegIntersect(A1, A2, B1, B2):
"""The function returns the intersection or the points of closest approach if lines are skewed.
If lines are parallel, NaN is returned.
INPUT:
A1 -float(3,n), [x,y,z;nsegments] cordinates of 1st point(s) of 1st segment(s)
A2 -float(3,n), [x,y,z;nsegments] cordinates of 2nd point(s) of 1st segment(s)
B1 -float(3,n), [x,y,z;nsegments] cordinates of 1st point(s) of 2nd segment(s)
B2 -float(3,n), [x,y,z;nsegments] cordinates of 2nd point(s) of 2nd segment(s)
OUTPUT:
A0 -float(3,n), [x,y,z;nsegments] coordinates of intersection point (=B0) or closet point to 2nd line on 1st segment,
B0 -float(3,n), [x,y,z;nsegments] coordinates of intersection point (=A0) or closet point to 1st line on 2nd segment,
OR -NaN
"""
# reshape A1..B2 in case they have 1 dimension only
A1 = A1.reshape(3, -1)
A2 = A2.reshape(3, -1)
B1 = B1.reshape(3, -1)
B2 = B2.reshape(3, -1)
vec = np.cross(A2 - A1, B2 - B1, 0, 0, 0)
nA = np.sum(np.cross(B2 - B1, A1 - B1, 0, 0, 0) * vec, axis=0) * np.ones(A1.shape[1])
nB = np.sum(np.cross(A2 - A1, A1 - B1, 0, 0, 0) * vec, axis=0) * np.ones(A1.shape[1])
d = np.sum(vec ** 2, axis=0) * np.ones(A1.shape[1])
A0 = np.ones(A1.shape) * np.NaN
B0 = A0.copy()
idx = np.nonzero(d)[0]
A0[:, idx] = A1[:, idx] + (nA[idx] / d[idx]) * (A2[:, idx] - A1[:, idx])
B0[:, idx] = B1[:, idx] + (nB[idx] / d[idx]) * (B2[:, idx] - B1[:, idx])
return A0, B0
def get_double_alignment_matrix(vp1, vp2):
'''
Align two sets of two vectors onto each other.
@param vp1: A pair of two vectors.
@param vp2: Another pair of two vectors.
'''
angle1 = vec_angle(vp1[0], vp1[1])
angle2 = vec_angle(vp2[0], vp2[1])
nt.assert_allclose(angle1, angle2, rtol=1e-7, atol=1e-7)
# Align the first two segments
mat1 = get_alignment_matrix(vp1[0], vp2[0])
# See where the second segment of the second set ends up
# after the first alignment
new_vp2_1 = np.dot(mat1, vp2[1])
comp1 = np.cross(vp1[1], vp1[0])
#comp1 = np.cross(vp1[0], vp1[1])
comp2 = np.cross(vp1[0], comp1) # should be along the plane of vp1[0] and vp1[1]
basis1 = create_orthonormal_basis(normalize(vp1[0]), normalize(comp2))
rej2 = change_basis(new_vp2_1, basis1, standard_basis)
angle = m.atan2(rej2[2], rej2[1])
mat2 = rotation_matrix(vp1[0], angle)
#return np.dot(mat1, mat2)
return np.dot(mat2, mat1)
def build(lattice, basis, layers, tol):
surf = lattice.copy()
scaled = solve(basis.T, surf.get_scaled_positions().T).T
scaled -= np.floor(scaled + tol)
surf.set_scaled_positions(scaled)
surf.set_cell(np.dot(basis, surf.cell), scale_atoms=True)
surf *= (1, 1, layers)
a1, a2, a3 = surf.cell
surf.set_cell([a1, a2,
np.cross(a1, a2) * np.dot(a3, np.cross(a1, a2)) /
norm(np.cross(a1, a2))**2])
# Change unit cell to have the x-axis parallel with a surface vector
# and z perpendicular to the surface:
a1, a2, a3 = surf.cell
surf.set_cell([(norm(a1), 0, 0),
(np.dot(a1, a2) / norm(a1),
np.sqrt(norm(a2)**2 - (np.dot(a1, a2) / norm(a1))**2), 0),
(0, 0, norm(a3))],
scale_atoms=True)
surf.pbc = (True, True, False)
# Move atoms into the unit cell:
scaled = surf.get_scaled_positions()
scaled[:, :2] %= 1
surf.set_scaled_positions(scaled)
return surf
def getCurvePoints(self,P0,P1,P2,R,n=20):
"""Generates a curve. P0,P1 and P2 define a corner and R defines the radius of the
tangential circle element. n gives the number of point to approximate the curve.
P1 is the corner itself and P0 and P2 give the tangents.
"""
P0=np.array(P0)
P1=np.array(P1)
P2=np.array(P2)
o1=(P0-P1)/np.sqrt(np.dot(P0-P1,P0-P1))
o2=(P2-P1)/np.sqrt(np.dot(P2-P1,P2-P1))
if np.arcsin(np.cross(o1,o2)) > 0:
a=1.
b=-1.
else:
a=-1.
b=1.
v1=R*np.dot(np.array([[0.,b],[a,0.]]),o1)
v2=R*np.dot(np.array([[0.,a],[b,0.]]),o2)
dv=v2-v1
a=np.array([[o1[0],-o2[0]],[o1[1],-o2[1]]])
b=dv
x=np.linalg.solve(a,b)
circleCenter= P1+x[0]*o1+v1
angle = np.arcsin(np.cross(v2/R,v1/R))
points=[]
for i in range(n+1):
x=-i*angle/n
rot = np.array([[np.cos(x),-np.sin(x)],
[np.sin(x),np.cos(x)]])
points.append(circleCenter+np.dot(rot,-v1))
return points
def lookAt(eye, center, up):
ret = numpy.eye(4, dtype=numpy.float32)
Z = numpy.array(eye, numpy.float32) - numpy.array(center, numpy.float32)
Z = normalize(Z)
Y = numpy.array(up, numpy.float32)
X = numpy.cross(Y, Z)
Y = numpy.cross(Z, X)
X = normalize(X)
Y = normalize(Y)
ret[0][0] = X[0]
ret[1][0] = X[1]
ret[2][0] = X[2]
ret[3][0] = -numpy.dot(X, eye)
ret[0][1] = Y[0]
ret[1][1] = Y[1]
ret[2][1] = Y[2]
ret[3][1] = -numpy.dot(Y, eye)
ret[0][2] = Z[0]
ret[1][2] = Z[1]
ret[2][2] = Z[2]
ret[3][2] = -numpy.dot(Z, eye)
ret[0][3] = 0
ret[1][3] = 0
ret[2][3] = 0
ret[3][3] = 1.0
return ret
def TEME_to_ITRF(jd_ut1, rTEME, vTEME, xp=0.0, yp=0.0):
"""Convert TEME position and velocity into standard ITRS coordinates.
This converts a position and velocity vector in the idiosyncratic
True Equator Mean Equinox (TEME) frame of reference used by the SGP4
theory into vectors into the more standard ITRS frame of reference.
The velocity should be provided in units per day, not per second.
From AIAA 2006-6753 Appendix C.
"""
theta, theta_dot = theta_GMST1982(jd_ut1)
zero = theta_dot * 0.0
angular_velocity = array([zero, zero, -theta_dot])
R = rot_z(-theta)
if len(rTEME.shape) == 1:
rPEF = (R).dot(rTEME)
vPEF = (R).dot(vTEME) + cross(angular_velocity, rPEF)
else:
rPEF = einsum('ij...,j...->i...', R, rTEME)
vPEF = einsum('ij...,j...->i...', R, vTEME) + cross(
angular_velocity, rPEF, 0, 0).T
if xp == 0.0 and yp == 0.0:
rITRF = rPEF
vITRF = vPEF
else:
W = (rot_x(yp)).dot(rot_y(xp))
rITRF = (W).dot(rPEF)
vITRF = (W).dot(vPEF)
return rITRF, vITRF
def my_solid_angle(center, coords):
"""
Helper method to calculate the solid angle of a set of coords from the
center.
Args:
center:
Center to measure solid angle from.
coords:
List of coords to determine solid angle.
Returns:
The solid angle.
"""
o = np.array(center)
r = [np.array(c) - o for c in coords]
r.append(r[0])
n = [np.cross(r[i + 1], r[i]) for i in range(len(r) - 1)]
n.append(np.cross(r[1], r[0]))
phi = 0.0
for i in range(len(n) - 1):
try:
value = math.acos(-np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1])))
except ValueError:
mycos = -np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1]))
if 0.999999999999 < mycos < 1.000000000001:
value = math.acos(1.0)
elif -0.999999999999 > mycos > -1.000000000001:
value = math.acos(-1.0)
else:
raise SolidAngleError(mycos)
phi += value
return phi + (3 - len(r)) * math.pi
def calculateNormals(self):
# Numpy magic!
# First, reset the normals
self._normals = numpy.zeros((self._vertex_count, 3), dtype=numpy.float32)
if self.hasIndices():
for face in self._indices[0:self._face_count]:
#print(self._vertices[face[0]])
#print(self._vertices[face[1]])
#print(self._vertices[face[2]])
self._normals[face[0]] = numpy.cross(self._vertices[face[0]] - self._vertices[face[1]], self._vertices[face[0]] - self._vertices[face[2]])
length = numpy.linalg.norm(self._normals[face[0]])
self._normals[face[0]] /= length
self._normals[face[1]] = self._normals[face[0]]
self._normals[face[2]] = self._normals[face[0]]
else: #Old way of doing it, asuming that each face has 3 unique verts
# Then, take the cross product of each pair of vectors formed from a set of three vertices.
# The [] operator on a numpy array returns itself a numpy array. The slicing syntax is [begin:end:step],
# so in this case we perform the cross over a two arrays. The first array is built from the difference
# between every second item in the array starting at two and every third item in the array starting at
# zero. The second array is built from the difference between every third item in the array starting at
# two and every third item in the array starting at zero. The cross operation then returns an array of
# the normals of each set of three vertices.
n = numpy.cross(self._vertices[1::3] - self._vertices[::3], self._vertices[2::3] - self._vertices[::3])
# We then calculate the length for each normal and perform normalization on the normals.
l = numpy.linalg.norm(n, axis=1)
n[:, 0] /= l
n[:, 1] /= l
n[:, 2] /= l
# Finally, we store the normals per vertex, with each face normal being repeated three times, once for
# every vertex.
self._normals = n.repeat(3, axis=0)
def __init__(self, p1, p2, p3):
# initialize points
self.p1 = np.array(p1)
self.p2 = np.array(p2)
self.p3 = np.array(p3)
# set vectors form p1 to p2 and from p1 to p3
self.v12 = self.p2 - self.p1
self.v13 = self.p3 - self.p1
self.v12 = self.v12 / np.linalg.norm(self.v12)
# calculate the normal to v12 and v13
self.n = np.cross(self.v12, self.v13)
self.n = self.n / np.linalg.norm(self.n)
# calculate corresponding axes of the plane, having p1 as base
# axisX = v12 / |v12|
self.axisX = self.v12 / np.linalg.norm(self.v12)
self.axisY = np.cross(self.n, self.axisX)
#self.axisY = self.axisY / np.linalg.norm(self.axisY)
self.rot = np.matrix([np.array(self.axisX), np.array(self.axisY),
np.array(self.n)])
self.q = [ 0,0,0,0 ]
self.q[3] = np.sqrt(1 + self.rot[0,0] + self.rot[1,1] + self.rot[2,2]) \
/ 2
self.q[0] = (self.rot[2,1] - self.rot[1,2]) / (4 * self.q[3])
self.q[1] = (self.rot[0,2] - self.rot[2,0]) / (4 * self.q[3])
self.q[2] = (self.rot[1,0] - self.rot[0,1]) / (4 * self.q[3])
self.scale = np.linalg.norm(self.p2 - self.p1)
def test_line_modelND_estimate():
# generate original data without noise
model0 = LineModelND()
model0.params = (np.array([0,0,0], dtype='float'),
np.array([1,1,1], dtype='float')/np.sqrt(3))
# we scale the unit vector with a factor 10 when generating points on the
# line in order to compensate for the scale of the random noise
data0 = (model0.params[0] +
10 * np.arange(-100,100)[...,np.newaxis] * model0.params[1])
# add gaussian noise to data
random_state = np.random.RandomState(1234)
data = data0 + random_state.normal(size=data0.shape)
# estimate parameters of noisy data
model_est = LineModelND()
model_est.estimate(data)
# test whether estimated parameters are correct
# we use the following geometric property: two aligned vectors have
# a cross-product equal to zero
# test if direction vectors are aligned
assert_almost_equal(np.linalg.norm(np.cross(model0.params[1],
model_est.params[1])), 0, 1)
# test if origins are aligned with the direction
a = model_est.params[0] - model0.params[0]
if np.linalg.norm(a) > 0:
a /= np.linalg.norm(a)
assert_almost_equal(np.linalg.norm(np.cross(model0.params[1], a)), 0, 1)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def __init__(self, position, direction, N=720, M=486,
scale=1,
orthographic=False):
super(Camera, self).__init__(position)
self._orth = orthographic
self.z_p = 1 #focal length
self.up = Vector(0, 1, 0)
self.direction = direction
#centre of the image screen
self.c = self.position + self.z_p * self.direction
#horizontal screen direction
self.u_x = (np.cross(self.direction, self.up) /
norm(np.cross(self.direction, self.up)))
#vertical screen direction
self.u_z = - self.direction
#normal to the viewscreen
self.u_y = np.cross(self.u_z, self.u_x)
self.m = int(M * scale)
self.n = int(N * scale)
factor = 0.5
self.hight = (1)*factor
self.width = (16/9.0)*factor
#vertical distance between pixels
self.dydi = self.hight / float(self.m)
#horizontal distance between pixels
self.dxdi = self.width / float(self.n)
def render_constraints(self):
phys = self.sim.phys
glDisable(GL_LIGHTING)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LINE_SMOOTH)
glLineWidth(1.0)
#glBegin(GL_LINES)
glColor3f(1,0,0)
for i in range(phys.n_planes):
p = phys.plane_pts[i]
nn = phys.plane_norms[i]
n = [nn[i] for i in range(3)]
# Make an orthonormal basis in the plane
v1 = numpy.cross([0,0,1],n)
if la.norm(v1)<1e-6:
v1 = numpy.cross([0,1,0],n)
v2 = numpy.cross(v1,n)
glBegin(GL_LINE_STRIP)
glVertex3f(p[0], p[1], p[2])
#glVertex3f(p[0]+n[0]*5, p[1]+n[1]*5, p[2]+n[2]*5)
glVertex3f(p[0]+v1[0]*5, p[1]+v1[1]*5, p[2]+v1[2]*5)
glVertex3f(p[0]+(v1[0]+v2[0])*5, p[1]+(v1[1]+v2[1])*5, p[2]+(v1[2]+v2[2])*5)
glVertex3f(p[0]+v2[0]*5, p[1]+v2[1]*5, p[2]+v2[2]*5)
glVertex3f(p[0], p[1], p[2])
glEnd()
#glEnd()
glEnable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
def rotation_angle(helix_vector, axis_vector, rotation_vector):
reference_vector = np.cross(np.cross(helix_vector, axis_vector), helix_vector)
second_reference_vector = np.cross(axis_vector, helix_vector)
screw_angle = mdamath.angle(reference_vector, rotation_vector)
alt_screw_angle = mdamath.angle(second_reference_vector, rotation_vector)
updown = np.cross(reference_vector, rotation_vector)
if not (np.pi < screw_angle < 3 * np.pi / 4):
if screw_angle < np.pi / 4 and alt_screw_angle < np.pi / 2:
screw_angle = np.pi / 2 - alt_screw_angle
elif screw_angle < np.pi / 4 and alt_screw_angle > np.pi / 2:
screw_angle = alt_screw_angle - np.pi / 2
elif screw_angle > 3 * np.pi / 4 and alt_screw_angle < np.pi / 2:
screw_angle = np.pi / 2 + alt_screw_angle
elif screw_angle > 3 * np.pi / 4 and alt_screw_angle > np.pi / 2:
screw_angle = 3 * np.pi / 2 - alt_screw_angle
else:
logger.debug("Big Screw Up: screw_angle=%g degrees", np.rad2deg(screw_angle))
if mdamath.norm(updown) == 0:
logger.warn("PROBLEM (vector is at 0 or 180)")
helix_dot_rehelix = mdamath.angle(updown, helix_vector)
#if ( helix_dot_rehelix < np.pi/2 and helix_dot_rehelix >= 0 )or helix_dot_rehelix <-np.pi/2:
if (-np.pi / 2 < helix_dot_rehelix < np.pi / 2) or (helix_dot_rehelix > 3 * np.pi / 2):
screw_angle = -screw_angle
return screw_angle
def get_neuromag_transform(lpa, rpa, nasion):
"""Creates a transformation matrix from RAS to Neuromag-like space
Resets the origin to mid-distance of peri-auricular points with nasion
passing through y-axis.
(mne manual, pg. 97)
Parameters
----------
lpa : numpy.array, shape = (1, 3)
Left peri-auricular point coordinate.
rpa : numpy.array, shape = (1, 3)
Right peri-auricular point coordinate.
nasion : numpy.array, shape = (1, 3)
Nasion point coordinate.
Returns
-------
trans : numpy.array, shape = (3, 3)
Transformation matrix to Neuromag-like space.
"""
origin = (lpa + rpa) / 2
nasion = nasion - origin
lpa = lpa - origin
rpa = rpa - origin
axes = np.empty((3, 3))
axes[1] = nasion / linalg.norm(nasion)
axes[2] = np.cross(axes[1], lpa - rpa)
axes[2] /= linalg.norm(axes[2])
axes[0] = np.cross(axes[1], axes[2])
trans = linalg.inv(axes)
return trans
def _dihedral(xyz, indices, out=None):
"""Compute the dihedral angles of traj for the atom indices in indices.
Parameters
----------
xyz : np.ndarray, shape=(num_frames, num_atoms, 3), dtype=float
The XYZ coordinates of a trajectory
indices : np.ndarray, shape=(num_dihedrals, 4), dtype=int
Atom indices to compute dihedrals.
Returns
-------
dih : np.ndarray, shape=(num_dihedrals), dtype=float
dih[i,j] gives the dihedral angle at traj[i] correponding to indices[j].
"""
x0 = xyz[:, indices[:, 0]]
x1 = xyz[:, indices[:, 1]]
x2 = xyz[:, indices[:, 2]]
x3 = xyz[:, indices[:, 3]]
b1 = x1 - x0
b2 = x2 - x1
b3 = x3 - x2
c1 = np.cross(b2, b3)
c2 = np.cross(b1, b2)
p1 = (b1 * c1).sum(-1)
p1 *= (b2 * b2).sum(-1) ** 0.5
p2 = (c1 * c2).sum(-1)
return np.arctan2(p1, p2, out)
def _facenorm_cross_edge(self):
ppts = self.ppts
fnorms = self.face_normals
fe12 = np.cross(fnorms, ppts[:,1] - ppts[:,0])
fe23 = np.cross(fnorms, ppts[:,2] - ppts[:,1])
fe31 = np.cross(fnorms, ppts[:,0] - ppts[:,2])
return fe12, fe23, fe31
def add(self, args):
dL, nL, stdL, dR, nR, stdR = args
rL = np.cross(np.array([0,0,1]), nL)
sL = np.cross(rL, nL)
RL = np.array([rL, sL, nL])
rR = np.cross(np.array([0,0,1]), nR)
sR = np.cross(rR, nR)
RR = np.array([rR, sR, nR])
self.addPlane(RL, dL*nL)
self.addPlane(RR, dR*nR)
self.ax.plot([0,50], [0,0], [0,0], linewidth=2.0, color='red')
self.ax.plot([0,0], [0,0], [0,50], linewidth=2.0, color='green')
self.ax.plot([0,0], [0,50], [0,0], linewidth=2.0, color='blue')
self.ax.set_xlabel('X')
self.ax.set_ylabel('Z')
self.ax.set_zlabel('Y')
self.ax.text(-100,0,0, str(round(stdL, 5)), fontsize=15)
self.ax.text(100,0,0, str(round(stdR, 5)), fontsize=15)
self.ax.set_xlim(-150, 150)
self.ax.set_ylim(0, 400)
self.ax.set_zlim(-150, 150)
self.ax.invert_xaxis()
self.ax.invert_yaxis()
self.ax.invert_zaxis()
self.canvas.draw()
self.Layout()
def trimField(self):
trimStarts = self.trimOutline.starts
trimVectors = self.trimOutline.vectors
fieldStarts = self.design.starts
fieldVectors = self.design.vectors
trimLen = len(self.trimOutline)
fieldLen = len(self.design)
Q_Less_P = fieldStarts - trimStarts.reshape(trimLen,1,2)
denom = np.cross(trimVectors, fieldVectors.reshape(fieldLen,1,2))
all_t = np.cross(Q_Less_P, trimVectors.reshape(trimLen,1,2)).transpose()/denom
all_u = np.cross(Q_Less_P, fieldVectors).transpose()/denom
for t, u, line in zip(all_t, all_u, self.design):
if not self.trimOutline.lineOutsideBoundingBox(line):
pointSet = set([line.start])
t = t[(0 <= u) & (u <= 1) & (0 <= t) & (t <= 1)]
pointSet |= set(Point(line.start.x + line.vector[c.X]*value,
line.start.y+line.vector[c.Y]*value)
for value in t)
pointSet.add(line.end)
pointList = sorted(list(pointSet))
pointVectors = np.array([point.normalVector for point in pointList])
""" Calculation for midPoints from here:
http://stackoverflow.com/questions/23855976/middle-point-of-each-pair-of-an-numpy-array
"""
midPoints = (pointVectors[1:] + pointVectors[:-1])/2.0
for i in range(len(midPoints)):
if self.trimOutline.isInside(midPoints[i]):
self.append(Line(pointList[i], pointList[i+1]))
def view_transformation(E, R, V):
n = (E - R)
## new
# n /= mod(n)
# u = np.cross(V,n)
# u /= mod(u)
# v = np.cross(n,u)
# Mr = np.diag([1.]*4)
# Mt = np.diag([1.]*4)
# Mr[:3,:3] = u,v,n
# Mt[:3,-1] = -E
## end new
## old
n = n / mod(n)
u = np.cross(V, n)
u = u / mod(u)
v = np.cross(n, u)
Mr = [[u[0],u[1],u[2],0],
[v[0],v[1],v[2],0],
[n[0],n[1],n[2],0],
[0, 0, 0, 1],
]
#
Mt = [[1, 0, 0, -E[0]],
[0, 1, 0, -E[1]],
[0, 0, 1, -E[2]],
[0, 0, 0, 1]]
## end old
return np.dot(Mr, Mt)
def getSurfaceVectors(face, camera):
a = np.array([face[0][0],face[1][0], face[2][0]]).T
#a = np.array(np.dot(camera.P,a))[0]
b = np.array([face[0][1],face[1][1], face[2][1]]).T
#b = np.array(np.dot(camera.P,b))[0]
c = np.array([face[0][2],face[1][2], face[2][2]]).T
#c = np.array(np.dot(camera.P,c))[0]
a = np.array([a[0], a[1],a[2]])
b = np.array([b[0], b[1],b[2]])
c = np.array([c[0], c[1],c[2]])
#vector between two face points
v_ba = np.array([a[0]-b[0],a[1]-b[1],a[2]-b[2]])
v_bc = np.array([c[0]-b[0],c[1]-b[1],c[2]-b[2]])
#surface center
cent = ((v_ba+v_bc)/2) + b
#normalize norm --- ghurt is working here.
norm = np.cross(v_ba,v_bc)/np.linalg.norm(np.cross(v_ba,v_bc))
#surface center and norm
return (cent, norm)
def dihedral(vec1,vec2,vec3,vec4):
"""
Returns a float value for the dihedral angle between
the four vectors. They define the bond for which the
torsion is calculated (~) as:
V1 - V2 ~ V3 - V4
The vectors vec1 .. vec4 can be array objects, lists or tuples of length
three containing floats.
For Scientific.geometry.Vector objects the behavior is different
on Windows and Linux. Therefore, the latter is not a featured input type
even though it may work.
If the dihedral angle cant be calculated (because vectors are collinear),
the function raises a DihedralGeometryError
"""
# create array instances.
v1,v2,v3,v4 =create_vectors(vec1,vec2,vec3,vec4)
all_vecs = [v1,v2,v3,v4]
# rule out that two of the atoms are identical
# except the first and last, which may be.
for i in range(len(all_vecs)-1):
for j in range(i+1,len(all_vecs)):
if i>0 or j<3: # exclude the (1,4) pair
equals = all_vecs[i]==all_vecs[j]
if equals.all():
raise DihedralGeometryError(\
"Vectors #%i and #%i may not be identical!"%(i,j))
# calculate vectors representing bonds
v12 = v2-v1
v23 = v3-v2
v34 = v4-v3
# calculate vectors perpendicular to the bonds
normal1 = cross(v12,v23)
normal2 = cross(v23,v34)
# check for linearity
if norm(normal1) == 0 or norm(normal2)== 0:
raise DihedralGeometryError(\
"Vectors are in one line; cannot calculate normals!")
# normalize them to length 1.0
normal1 = normal1/norm(normal1)
normal2 = normal2/norm(normal2)
# calculate torsion and convert to degrees
torsion = angle(normal1,normal2) * 180.0/pi
# take into account the determinant
# (the determinant is a scalar value distinguishing
# between clockwise and counter-clockwise torsion.
if scalar(normal1,v34) >= 0:
return torsion
else:
torsion = 360-torsion
if torsion == 360: torsion = 0.0
return torsion
def cross(v1, v2):
return np.cross(v1, v2)
def test_static_plate_quad_point_load(plot=False):
nx = 7
ny = 7
# geometry
a = 3
b = 7
h = 0.005 # m
# material
E = 200e9
nu = 0.3
plate = read_isotropic(thickness=h, E=E, nu=nu, calc_scf=True)
xtmp = np.linspace(0, a, nx)
ytmp = np.linspace(0, b, ny)
xmesh, ymesh = np.meshgrid(xtmp, ytmp)
ncoords = np.vstack((xmesh.T.flatten(), ymesh.T.flatten())).T
x = ncoords[:, 0]
y = ncoords[:, 1]
nids = 1 + np.arange(ncoords.shape[0])
nid_pos = dict(zip(nids, np.arange(len(nids))))
nids_mesh = nids.reshape(nx, ny)
n1s = nids_mesh[:-1, :-1].flatten()
n2s = nids_mesh[1:, :-1].flatten()
n3s = nids_mesh[1:, 1:].flatten()
n4s = nids_mesh[:-1, 1:].flatten()
# creating global stiffness matrix
K = np.zeros((DOF * nx * ny, DOF * nx * ny))
# creating elements and populating global stiffness
quads = []
for n1, n2, n3, n4 in zip(n1s, n2s, n3s, n4s):
pos1 = nid_pos[n1]
pos2 = nid_pos[n2]
pos3 = nid_pos[n3]
pos4 = nid_pos[n4]
r1 = ncoords[pos1]
r2 = ncoords[pos2]
r3 = ncoords[pos3]
normal = np.cross(r2 - r1, r3 - r2)
assert normal > 0 # guaranteeing that all elements have CCW positive normal
quad = Quad4R()
quad.n1 = n1
quad.n2 = n2
quad.n3 = n3
quad.n4 = n4
quad.scf13 = plate.scf_k13
quad.scf23 = plate.scf_k23
quad.h = h
quad.ABDE = plate.ABDE
update_K(quad, nid_pos, ncoords, K)
quads.append(quad)
print('elements created')
# applying boundary conditions
# simply supported
bk = np.zeros(K.shape[0], dtype=bool) #array to store known DOFs
check = np.isclose(x, 0.) | np.isclose(x, a) | np.isclose(
y, 0) | np.isclose(y, b)
bk[2::DOF] = check
# eliminating all u,v displacements
bk[0::DOF] = True
bk[1::DOF] = True
bu = ~bk # same as np.logical_not, defining unknown DOFs
# external force vector for point load at center
f = np.zeros(K.shape[0])
fmid = 1.
# force at center node
check = np.isclose(x, a / 2) & np.isclose(y, b / 2)
f[2::DOF][check] = fmid
assert f.sum() == fmid
# sub-matrices corresponding to unknown DOFs
Kuu = K[bu, :][:, bu]
fu = f[bu]
# solving static problem
uu = solve(Kuu, fu)
# vector u containing displacements for all DOFs
u = np.zeros(K.shape[0])
u[bu] = uu
w = u[2::DOF].reshape(nx, ny).T
# obtained with bfsplate2d element, nx=ny=29
wmax_ref = 6.594931610258557e-05
assert np.isclose(wmax_ref, w.max(), rtol=0.02)
if plot:
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
plt.gca().set_aspect('equal')
levels = np.linspace(w.min(), w.max(), 300)
plt.contourf(xmesh, ymesh, w, levels=levels)
plt.colorbar()
plt.show()
def get_rt_mat(origin, Pa, Pb, Pc):
# define the vector for new axis
# the x y z axis
vab = np.subtract(Pb, Pa)
vac = np.subtract(Pc, Pa)
vz = np.cross(vab, vac)
# the origin
vtrans = np.negative(origin)
# the normalized new x y z vector in old axis represent
ex = vab / np.sqrt(np.dot(vab, vab))
ez = vz / np.sqrt(np.dot(vz, vz))
vy = np.cross(vz, ex)
ey = vy / np.sqrt(np.dot(vy, vy))
# print "the normalized x y z vector:"
# print ex
# print ey
# print ez
# Make the rot. mat.
matA = np.array([ex, ey, ez])
# print "mat. A before transpose (xyz in row represent:"
# print matA
matA = matA.transpose()
# print "mat. A after transpose, the coordinate axis (xyz in col):"
# print matA
matR = np.linalg.inv(matA)
# print "after inv, the real rotation matrix (in row):"
# print matR
# try to make 4v rot. mat.
ex = np.append(matR[:, 0], 0)
ey = np.append(matR[:, 1], 0)
ez = np.append(matR[:, 2], 0)
trans = np.array([0.0, 0.0, 0.0, 1.0])
matR4v = np.array([ex, ey, ez, trans])
matR4v = matR4v.transpose()
# print "final 4v rot. mat.:"
# print matR4v
# try to make 4v trans. mat.
matT4v = np.array([[1.0, 0.0, 0.0, vtrans[0]], [0.0, 1.0, 0.0, vtrans[1]],
[0.0, 0.0, 1.0, vtrans[2]], [0.0, 0.0, 0.0, 1.0]])
# print "final 4v trans. mat.:"
# print matT4v
# try to get the final 4v mat. ( R * T mat.)
move4v = np.dot(matR4v, matT4v)
# print "final 4v R*T mat.:"
# print move4v
# for checking by users
a = np.dot(move4v, pto4v(Pa))
b = np.dot(move4v, pto4v(Pb))
c = np.dot(move4v, pto4v(Pc))
# print "the input Pa Pb Pc in new axis (shown in order: new old):"
# print Pa, a.reshape(1,4)
# print Pb, b.reshape(1,4)
# print Pc, c.reshape(1,4)
return move4v
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# Frame C is located at the contact point, but with the world frame
# orientation.
if force_norm < 1e-6:
X_CGeom = tf.identity_matrix()
else:
# Rotates [0,1,0] to contact_force/force_norm.
angle_axis = np.cross(
np.array([0, 1, 0]),
contact_info.contact_force() / force_norm)
X_CGeom = tf.rotation_matrix(
np.arcsin(np.linalg.norm(angle_axis)), angle_axis)
X_WC = tf.translation_matrix(contact_info.contact_point())
cvis.set_transform(X_WC @ X_CGeom)
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
def rodrigues_rotate(vect, axis, phi):
z_axis = np.array([0,0,1])
vect_rotated = vect * np.cos(phi)
vect_rotated += np.cross(axis, vect) * np.sin(phi)
vect_rotated += axis * np.dot(z_axis, vect) * (1 - np.cos(phi))
return vect_rotated
def rotate_mat(axis, radian):
return linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian))
# print(boundary_points)
# [[-37.264586548075904, -4.818972663825464, 27.865702430922006],
# [-37.2642637125735, -14.216115063345738, 24.446095053032607],
# [-39.764781970840225, -12.735559826177374, 20.377266643063948],
# [-39.76510480634263, -3.3384174266571005, 23.796874020953346]]
# Obtaining the centroid of the module
centroid = np.array(centroidDB.getCentroid(437526553))
# print(centroid) # [-8.77723, 24.1215, -38.5147]
# Normal vector obtain via cross vector between two vectors from centroid to two end points on a boundary
vec1_on_module = centroid - np.array(
[boundary_points[0][1], boundary_points[0][2], boundary_points[0][0]])
vec2_on_module = centroid - np.array(
[boundary_points[1][1], boundary_points[1][2], boundary_points[1][0]])
norm_vec = np.cross(vec1_on_module, vec2_on_module)
norm_vec = norm_vec / np.linalg.norm(norm_vec)
def h(t):
return sdlmath.get_track_point(1.1325262784957886, -1.224777340888977,
1.841907024383545,
-0.000797238782979548,
-0.0006373987998813391,
3.2855265140533447, -1, t)
# # Debugging track drawing
# points = np.array([ h(np.pi / 100. * i) for i in range(100) ])
# ax_xy = pickle.load(file('/nfs-7/userdata/phchang/detector_layout_matplotlib_pickle/detxy.pickle'))
# ax_xy.scatter(points[0:100,0], points[0:100,1])
# plt.savefig("detxy.pdf")
def provideJ(self):
nj = 3 * self.n
Jab = np.zeros(shape=(nj, ), dtype=np.float)
Jib = np.zeros(shape=(nj, ), dtype=np.int)
Jjb = np.zeros(shape=(nj, ), dtype=np.int)
Jas = np.zeros(shape=(nj, ), dtype=np.float)
Jis = np.zeros(shape=(nj, ), dtype=np.int)
Jjs = np.zeros(shape=(nj, ), dtype=np.int)
r_b = np.zeros(shape=(3, ), dtype=np.int)
r_s = np.zeros(shape=(3, ), dtype=np.int)
Bx = np.zeros(shape=(
3,
3,
), dtype=np.int)
Sx = np.zeros(shape=(
3,
3,
), dtype=np.int)
cross = np.zeros(shape=(3, ), dtype=np.int)
#ddist_cross = np.zeros(shape=(3,), dtype = np.int)
dcross_drb = np.zeros(shape=(
3,
3,
), dtype=np.int)
dcross_drs = np.zeros(shape=(
3,
3,
), dtype=np.int)
dLOS_dx = np.zeros(shape=(3, ), dtype=np.int)
dLOS_drs = np.zeros(shape=(3, ), dtype=np.int)
dLOS_drb = np.zeros(shape=(3, ), dtype=np.int)
for i in range(self.n):
r_b = self.r_e2b_I[:3, i]
r_s = self.r_e2s_I[:3, i]
Bx = crossMatrix(r_b)
Sx = crossMatrix(-r_s)
dot = np.dot(r_b, r_s)
cross = np.cross(r_b, r_s)
dist = np.sqrt(np.dot(cross, cross))
if dot >= 0.0:
dLOS_drb[:] = 0.0
dLOS_drs[:] = 0.0
elif dist <= self.r1:
dLOS_drb[:] = 0.0
dLOS_drs[:] = 0.0
elif dist >= self.r2:
dLOS_drb[:] = 0.0
dLOS_drs[:] = 0.0
else:
x = (dist - self.r1) / (self.r2 - self.r1)
#LOS = 3*x**2 - 2*x**3
ddist_dcross = cross / dist
dcross_drb = Sx
dcross_drs = Bx
dx_ddist = 1.0 / (self.r2 - self.r1)
dLOS_dx = 6 * x - 6 * x**2
dLOS_drb = dLOS_dx * dx_ddist * np.dot(ddist_dcross,
dcross_drb)
dLOS_drs = dLOS_dx * dx_ddist * np.dot(ddist_dcross,
dcross_drs)
for k in range(3):
iJ = i * 3 + k
Jab[iJ] = dLOS_drb[k]
Jib[iJ] = i
Jjb[iJ] = (i) * 6 + k
Jas[iJ] = dLOS_drs[k]
Jis[iJ] = i
Jjs[iJ] = (i) * 3 + k
self.Jb = scipy.sparse.csc_matrix((Jab, (Jib, Jjb)),
shape=(self.n, 6 * self.n))
self.Js = scipy.sparse.csc_matrix((Jas, (Jis, Jjs)),
shape=(self.n, 3 * self.n))
self.JbT = self.Jb.transpose()
self.JsT = self.Js.transpose()
def vcross(a, b):
return np.cross(a, b)
def getRotationMatrix(self, mol1):
"""
return a matrix which turns this molecule to be another, the same kind of molecule
Note: self and the para. molecule must be the same molecule or chiral isomers
"""
mol0 = copy.deepcopy(self)
ZERO = 1e-10
matrix = np.identity(3)
rotatedAxis = []
for i in range(len(mol0.atomDic)-1):
origin1 = mol0.atomDic[0].coordinate
origin2 = mol1.atomDic[0].coordinate
x1 = mol0.atomDic[i+1].coordinate-origin1
x2 = mol1.atomDic[i+1].coordinate-origin2
for axis in rotatedAxis:
x1 = x1-x1.dot(axis)*axis
x2 = x2-x2.dot(axis)*axis
x1_model = np.linalg.norm(x1)
x2_model = np.linalg.norm(x2)
if x1_model<ZERO : continue
else:
if len(rotatedAxis)<2:
rotatedAxis.append(x2/x2_model)
v = np.cross(x1,x2)
v_model = np.linalg.norm(v)
if v_model <ZERO:
if len(rotatedAxis)==1:
if np.linalg.norm(x1/x1_model-x2/x2_model)<ZERO:
pass
else:
matrix = -matrix
mol0.rotate(-1*np.eye(3))
elif len(rotatedAxis)==2:
if np.linalg.norm(x1/x1_model-x2/x2_model) <ZERO:
pass
else:
matrix = rotationMatrix(np.pi,rotatedAxis[0]).dot(matrix)
mol0.rotate(rotationMatrix(np.pi,rotatedAxis[0]))
else:
v = v/v_model
alpha = np.arccos(x1.dot(x2)/np.linalg.norm(x1)/np.linalg.norm(x2))
matrix = rotationMatrix(alpha,v).dot(matrix)
mol0.rotate(matrix)
else:
if np.linalg.norm(x1/x1_model-x2/x2_model) <ZERO:
pass
else:
normal = np.cross(rotatedAxis[0],rotatedAxis[1])
normal = normal/np.linalg.norm(normal)
ax = np.cross(normal,np.array([0,0,1]))
if np.linalg.norm(ax) <ZERO:
matrix = np.array([[1,0,0],
[0,1,0],[0,0,-1]]).dot(matrix)
else:
ax = ax/np.linalg.norm(ax)
alpha = np.arccos(normal.dot(np.array([0,0,1])))
transform = rotationMatrix(alpha,ax)
matrix = np.linalg.inv(transform).dot(np.array([[1,0,0],
[0,1,0],[0,0,-1]]).dot(transform.dot(matrix)))
break
matrix[(np.abs(matrix)<ZERO)] = 0
return matrix
def smooth(points,
radius,
points_per_degree=POINTS_PER_DEGREE,
already_package_format=False):
"""Return a list of smoothed points constructed by adding points to change the given corners into circular arcs.
At each corner, the original point is replaced by points that form circular arcs that are tangent to the original
straight sections. Because this process replaces all of the interior points, the returned path will not contain any
of the given points except for the start and end.
If the given radius is too large compared to the length of the straight sections there is no way to make this
work, and the results will be ugly. **No warning is currently given when this happens, so you need to inspect
your design.** The given radius should be smaller than about half the length of the shortest straight section. If
several points lie on the same line, the redundant ones are removed.
:param iterable[indexable] points: a list of points forming the outline of the path to smooth.
:param float radius: the radius of the circular arcs used to connect the straight segments.
:param float points_per_degree: the number of points per degree of arc; the default of 1 is usually enough.
:param bool already_package_format: if True, skip the conversion of the points to package format (used internally
by :class:`SmoothedSegment` to avoid double-conversion).
:return: four lists with length equal to the number of bends (i.e., two less than the number of given points)
that contain, for each bend, (1) a list of points in that bend, (2) the bend angle in radians, (3) the
corner point from the original list, and (4) the vector offset of the bend arc center relative to the
corner point.
:rtype: tuple[list]
"""
bends = list()
angles = list()
corners = list()
offsets = list()
if not already_package_format:
points = to_point_list(points)
for before, current, after in zip(points[:-2], points[1:-1], points[2:]):
before_to_current = current - before
current_to_after = after - current
# The angle at which the path bends at the current point, in (-pi, pi)
bend_angle = np.angle(
np.inner(before_to_current, current_to_after) +
1j * np.cross(before_to_current, current_to_after))
if np.abs(
bend_angle
) > 0: # If the three points are co-linear then drop the current point
# The distance from the corner point to the arc center
h = radius / np.cos(bend_angle / 2)
# The absolute angle of the arc center point, in (-pi, pi)
theta = (np.arctan2(before_to_current[1], before_to_current[0]) +
bend_angle / 2 + np.sign(bend_angle) * np.pi / 2)
# The offset of the arc center relative to the corner
offset = h * np.array([np.cos(theta), np.sin(theta)])
# The absolute angles of the new points (at least two), using the absolute center as origin
arc_angles = (theta + np.pi + np.linspace(
-bend_angle / 2, bend_angle / 2,
int(
np.ceil(
np.degrees(np.abs(bend_angle)) * points_per_degree) +
1)))
bend = [
current + offset +
radius * np.array([np.cos(phi), np.sin(phi)])
for phi in arc_angles
]
bends.append(bend)
angles.append(bend_angle)
corners.append(current)
offsets.append(offset)
return bends, angles, corners, offsets
def __init__(self,corner1,corner2,corner3,corner4=None,maskInside=True,coordinates=None,name=None):
"""Generate a box mask with side corner1, corner 2, corner 3 and use corner 4 to define height.
args:
corner1 (3D point): First corner
corner2 (3D point): Second corner
corner2 (3D point): Third corner
kwargs:
corner4 (3D point): Fourth corner used to define height of box. If corner4 is not set corner1, corner2, corner3 are assumed to be maximal positions.
maskInside (bool): If true, points inside is masked otherwise outside (default True)
coordinates (list of str): List containing names of attributes on point object. If None, use x,y (default None)
"""
super(boxMask,self).__init__(coordinates=coordinates,maskInside=maskInside,name=name)
self.corner1 = np.array(corner1,dtype=float)
self.corner2 = np.array(corner2,dtype=float)
self.corner3 = np.array(corner3,dtype=float)
self.corner4 = np.array(corner4,dtype=float)
if corner4 is None: # use provided corners and find bounding box
X,Y,Z = [[np.min(x),np.max(x)] for x in np.array([corner1,corner2,corner3]).T]
c1 = np.array([X[0],Y[0],Z[0]])
c2 = np.array([X[1],Y[0],Z[0]])
c3 = np.array([X[0],Y[1],Z[0]])
c4 = np.array([X[1],Y[1],Z[0]])
c5 = np.array([X[0],Y[0],Z[1]])
c6 = np.array([X[1],Y[0],Z[1]])
c7 = np.array([X[0],Y[1],Z[1]])
c8 = np.array([X[1],Y[1],Z[1]])
self.length = X[1]-X[0]
self.width = Y[1]-Y[0]
self.height = Z[1]-Z[0]
self.planeNormal = np.array([0,0,1])#np.cross(edge1,edge2)
#planeNormal *= 1.0/np.linalg.norm(planeNormal)
#self.rotationMatrix = RotationMatrix3D(0.0,n=None,deg=False)
else:
c1 = np.array(corner1,dtype=float)
c2 = np.array(corner2,dtype=float)
# Start by finding common plane for c1, c2, and c3
edge1 = c2 - c1 # Defined as line between first and second corner
edge2 = corner3 - c1 # Defined as line between first and second corner
self.length = np.linalg.norm(edge1)
edge1 *= 1.0/self.length
edge2 *= 1.0/np.linalg.norm(edge2)
planeNormal = np.cross(edge1,edge2)
self.planeNormal = planeNormal/np.linalg.norm(planeNormal)
cross = np.cross(edge1,self.planeNormal)
width = np.dot(cross,corner3 - c1) # distance from c3 orthogonally on edge1
widthSign = np.sign(width)
self.width = np.abs(width)
# calculate corner point for c3 and c4
c4 = c2+widthSign*cross*self.width
c3 = c4-c2+c1 # Find corresponding c4
# find cube
height = np.dot(self.planeNormal,corner4-c1)
heightSign = np.sign(height)
self.height = np.abs(height)
c5 = c1+heightSign*self.planeNormal*self.height
c6 = c5+c2-c1
c7 = c5+c3-c1
c8 = c5+c4-c1
self.corners = np.array([c1,c2,c3,c4,c5,c6,c7,c8])
self.center = np.array([np.mean(X) for X in self.corners.T]).reshape(1,-1)
# Create rotation matrix.
# rotate planeNormal to lie along x,z plane (https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d)
v = np.cross(self.planeNormal,np.array([0.0,1.0,0.0]))
Vs = np.zeros((3,3))
Vs[0,1] = -v[2]
Vs[0,2] = +v[1]
Vs[1,2] = -v[0]
Vs[1,0] = -Vs[0,1]
Vs[0,2] = -Vs[0,2]
Vs[2,1] = -Vs[1,2]
c = np.dot(self.planeNormal,np.array([0.0,1.0,0.0]))
rot1 = np.eye(3)+Vs+np.dot(Vs,Vs)*1.0/(1+c)
c1r1,c2r1,c3r1,c4r1,c5r1,c6r1,c7r1,c8r1 = [np.dot(rot1,(x-self.center).T) for x in [c1,c2,c3,c4,c5,c6,c7,c8]]
# rotate around y into plane using edge 1 (c2-c1)
edge = c2r1-c1r1
theta = np.arctan2(edge[2],edge[0])
rot2 = RotationMatrix3D(theta,n=[0,1,0],deg=False)
self.rotationMatrix = np.einsum('ij,jk->ik',rot2,rot1)
def laplacian(M, mode=None, normalized=True):
"""Return the Laplacian of the weigth matrix."""
if mode == 'cotan':
n = len(M.v)
W_ij = np.empty(0)
I = np.empty(0, np.int32)
J = np.empty(0, np.int32)
for i1, i2, i3 in [(0, 1, 2), (1, 2, 0), (2, 0, 1)]:
vi1 = M.f[:, i1]
vi2 = M.f[:, i2]
vi3 = M.f[:, i3]
u = M.v[vi2] - M.v[vi1]
v = M.v[vi3] - M.v[vi1]
veclen = lambda vectors: np.sqrt(np.sum(vectors**2, axis=-1))
cotan = -(u * v).sum(axis=1) / veclen(np.cross(u, v))
#if normalized:
# cotan /= np.sum(cotan)
W_ij = np.append(W_ij, 0.5 * cotan)
I = np.append(I, vi2)
J = np.append(J, vi3)
W_ij = np.append(W_ij, 0.5 * cotan)
I = np.append(I, vi3)
J = np.append(J, vi2)
L = scipy.sparse.csr_matrix((W_ij, (I, J)), shape=(n, n))
d = -L * np.ones(n)
if not normalized:
L = scipy.sparse.spdiags(d, 0, n, n) - L
else:
d += np.spacing(np.array(0, L.dtype))
d = 1 / np.sqrt(d)
D = scipy.sparse.diags(d, 0)
I = scipy.sparse.identity(d.size, dtype=L.dtype)
L = I - D * L * D
#val = np.repeat(d, L.getnnz(axis=1))
#L.data /= val
L = L.tocsr()
print(L)
assert type(L) is scipy.sparse.csr.csr_matrix
return L
else:
W = get_vert_connectivity(M)
# Degree matrix.
d = W.sum(axis=0)
# Laplacian matrix.
if not normalized:
D = scipy.sparse.diags(d.A.squeeze(), 0)
L = D - W
else:
d += np.spacing(np.array(0, W.dtype))
d = 1 / np.sqrt(d)
D = scipy.sparse.diags(d.A.squeeze(), 0)
I = scipy.sparse.identity(d.size, dtype=W.dtype)
L = I - D * W * D
# assert np.abs(L - L.T).mean() < 1e-9
assert type(L) is scipy.sparse.csr.csr_matrix
return L
def angular_acceleration(self,inputs, omega, I, L, b, k): tau = self.torques(inputs, L, b, k); omegad = np.dot(np.linalg.inv(I), (tau - np.cross(omega.flatten(), np.dot(I, omega).flatten()))); return self.lineToColumn(omegad)
def setupJoints(self):
"""
Evaluate symbolic expressions for joint locations and store them in self.locations.
Joint locations are specified symbolically in the *Joints list in the beginning of the
rig_*.py files (e.g. ArmJoints in rig_arm.py).
"""
for (key, typ, data) in self.joints:
if typ == 'j':
loc = mh2proxy.calcJointPos(self.mesh, data)
self.locations[key] = loc
self.locations[data] = loc
elif typ == 'v':
v = int(data)
self.locations[key] = self.mesh.coord[v]
elif typ == 'x':
self.locations[key] = np.array(
(float(data[0]), float(data[2]), -float(data[1])))
elif typ == 'vo':
v = int(data[0])
offset = np.array(
(float(data[1]), float(data[3]), -float(data[2])))
self.locations[key] = self.mesh.coord[v] + offset
elif typ == 'vl':
((k1, v1), (k2, v2)) = data
loc1 = self.mesh.coord[int(v1)]
loc2 = self.mesh.coord[int(v2)]
self.locations[key] = k1 * loc1 + k2 * loc2
elif typ == 'f':
(raw, head, tail, offs) = data
rloc = self.locations[raw]
hloc = self.locations[head]
tloc = self.locations[tail]
vec = tloc - hloc
vraw = rloc - hloc
x = np.dot(vec, vraw) / np.dot(vec, vec)
self.locations[key] = hloc + x * vec + np.array(offs)
elif typ == 'b':
self.locations[key] = self.locations[data]
elif typ == 'p':
x = self.locations[data[0]]
y = self.locations[data[1]]
z = self.locations[data[2]]
self.locations[key] = np.array((x[0], y[1], z[2]))
elif typ == 'vz':
v = int(data[0])
z = self.mesh.coord[v][2]
loc = self.locations[data[1]]
self.locations[key] = np.array((loc[0], loc[1], z))
elif typ == 'X':
r = self.locations[data[0]]
(x, y, z) = data[1]
r1 = np.array([float(x), float(y), float(z)])
self.locations[key] = np.cross(r, r1)
elif typ == 'l':
((k1, joint1), (k2, joint2)) = data
self.locations[key] = k1 * self.locations[
joint1] + k2 * self.locations[joint2]
elif typ == 'o':
(joint, offsSym) = data
if type(offsSym) == str:
offs = self.locations[offsSym]
else:
offs = np.array(offsSym)
self.locations[key] = self.locations[joint] + offs
else:
raise NameError("Unknown %s" % typ)
return
def crossProduct(self, rhs):
return matrix(np.cross(np.transpose(self.__m),
np.transpose(rhs.__m))).transpose()
def get_extended_system(system, radial_cutoff, centers=None, return_cell_indices=False):
"""Used to create a periodically extended system. If centers are not
specified, simply takes returns the original system multiplied by an
integer amount of times in each direction to cover the radial cutoff. If
centers are provided, returns the exact atoms that are within the given
radial cutoff from the given centers.
Args:
original_system (ase.Atoms): The original periodic system to duplicate.
radial_cutoff (float): The radial cutoff to use in constructing the
extended system.
centers (np.ndarray): Array of xyz-coordinates from which the distance
is calculated. If provided, these centers are used to calculate the
exact distance and only atoms within the radial cutoff from these
centers are returned.
return_cell_indices (boolean): Whether to return an array of cell
indices for each atom in the extended system.
Returns:
ase.Atoms | tuple: If return_cell_indices is False, returns the new
extended system. Else returns a tuple containing the new extended
system as the first entry and the index of the periodically repeated
cell for each atom as the second entry.
"""
# Determine the upper limit of how many copies we need in each cell vector
# direction. We take as many copies as needed to reach the radial cutoff.
# Notice that we need to use vectors that are perpendicular to the cell
# vectors to ensure that the correct atoms are included for non-cubic cells.
cell = np.array(system.get_cell())
a1, a2, a3 = cell[0], cell[1], cell[2]
b1 = np.cross(a2, a3, axis=0)
b2 = np.cross(a3, a1, axis=0)
b3 = np.cross(a1, a2, axis=0)
p1 = np.dot(a1, b1) / np.dot(b1, b1) * b1 # Projections onto perpendicular vectors
p2 = np.dot(a2, b2) / np.dot(b2, b2) * b2
p3 = np.dot(a3, b3) / np.dot(b3, b3) * b3
xyz_arr = np.linalg.norm(np.array([p1, p2, p3]), axis=1)
cell_images = np.ceil(radial_cutoff/xyz_arr)
nx = int(cell_images[0])
ny = int(cell_images[1])
nz = int(cell_images[2])
n_copies_axis = np.array([nx, ny, nz], dtype=int)
# If no centers are given, and the cell indices are not requested, simply
# return the multiplied system. This is much faster.
if centers is None and not return_cell_indices:
n_atoms = len(system)
n_rep = np.product(2*n_copies_axis+1) # Number of repeated copies
ext_pos = np.tile(system.get_positions(), (n_rep, 1))
# Calculate the extended system positions so that the original cell
# stays in place: both in space and in index
i_curr = 0
for m0 in np.append(np.arange(0, nx+1), np.arange(-nx, 0)):
for m1 in np.append(np.arange(0, ny+1), np.arange(-ny, 0)):
for m2 in np.append(np.arange(0, nz+1), np.arange(-nz, 0)):
ext_pos[i_curr:i_curr+n_atoms] += np.dot((m0, m1, m2), cell)
i_curr += n_atoms
ext_symbols = np.tile(system.get_atomic_numbers(), n_rep)
extended_system = Atoms(
positions=ext_pos,
symbols=ext_symbols,
)
return extended_system
# If centers are given and/or cell indices are needed, the process is done
# one cell at a time to keep track of the cell inded and the distances.
# This is a bit slower.
else:
# We need to specify that the relative positions should not be wrapped.
# Otherwise the repeated systems may overlap with the positions taken
# with get_positions()
relative_pos = np.array(system.get_scaled_positions(wrap=False))
numbers = np.array(system.numbers)
cartesian_pos = np.array(system.get_positions())
# Create copies of the cell but keep track of the atoms in the
# original cell
num_extended = []
pos_extended = []
num_extended.append(numbers)
pos_extended.append(cartesian_pos)
a = np.array([1, 0, 0])
b = np.array([0, 1, 0])
c = np.array([0, 0, 1])
cell_indices = [np.zeros((len(system), 3), dtype=int)]
for i in range(-n_copies_axis[0], n_copies_axis[0]+1):
for j in range(-n_copies_axis[1], n_copies_axis[1]+1):
for k in range(-n_copies_axis[2], n_copies_axis[2]+1):
if i == 0 and j == 0 and k == 0:
continue
# Calculate the positions of the copied atoms and filter
# out the atoms that are farther away than the given
# cutoff.
num_copy = np.array(numbers)
pos_copy = np.array(relative_pos)
pos_shifted = pos_copy-i*a-j*b-k*c
pos_copy_cartesian = np.dot(pos_shifted, cell)
# Only distances to the atoms within the interaction limit
# are considered.
distances = cdist(pos_copy_cartesian, centers)
weight_mask = distances < radial_cutoff
# Create a boolean mask that says if the atom is within the
# range from at least one atom in the original cell
valids_mask = np.any(weight_mask, axis=1)
if np.any(valids_mask):
valid_pos = pos_copy_cartesian[valids_mask]
valid_num = num_copy[valids_mask]
valid_ind = np.tile(np.array([i, j, k], dtype=int), (len(valid_num), 1))
pos_extended.append(valid_pos)
num_extended.append(valid_num)
cell_indices.append(valid_ind)
pos_extended = np.concatenate(pos_extended)
num_extended = np.concatenate(num_extended)
cell_indices = np.vstack(cell_indices)
extended_system = Atoms(
positions=pos_extended,
numbers=num_extended,
cell=cell,
pbc=False
)
return extended_system, cell_indices
def get_new_axis_x(delline):
# Need CONTCAR and KPOINTS
rf = open('CONTCAR', 'r')
lines = rf.readlines()
rf.close()
# Convert lattice constants in real space to inverted space
real_lattice_x = np.array([
abs(float(lines[2].split()[0])),
abs(float(lines[2].split()[1])),
abs(float(lines[2].split()[2]))
])
real_lattice_y = np.array([
abs(float(lines[3].split()[0])),
abs(float(lines[3].split()[1])),
abs(float(lines[3].split()[2]))
])
real_lattice_z = np.array([
abs(float(lines[4].split()[0])),
abs(float(lines[4].split()[1])),
abs(float(lines[4].split()[2]))
])
volume_xyz = np.dot(np.cross(real_lattice_x, real_lattice_y),
real_lattice_z)
down_lattice_x = 2 * math.pi * np.cross(real_lattice_y,
real_lattice_z) / volume_xyz
down_lattice_y = 2 * math.pi * np.cross(real_lattice_x,
real_lattice_z) / volume_xyz
down_lattice_z = 2 * math.pi * np.cross(real_lattice_x,
real_lattice_y) / volume_xyz
rf = open('KPOINTS', 'r')
kpt_lines = rf.readlines()
rf.close()
def down_distance_calculation_kpt(line_i, line_next_i):
# Calculate distance of points in KPOINTS file
dot_x = float(kpt_lines[line_i].split()[0]) - float(
kpt_lines[line_next_i].split()[0])
dot_y = float(kpt_lines[line_i].split()[1]) - float(
kpt_lines[line_next_i].split()[1])
dot_z = float(kpt_lines[line_i].split()[2]) - float(
kpt_lines[line_next_i].split()[2])
dot_i = np.array([dot_x, dot_y, dot_z])
down_dot_x = np.dot(
dot_i, [down_lattice_x[0], down_lattice_y[0], down_lattice_z[0]])
down_dot_y = np.dot(
dot_i, [down_lattice_x[1], down_lattice_y[1], down_lattice_z[1]])
down_dot_z = np.dot(
dot_i, [down_lattice_x[2], down_lattice_y[2], down_lattice_z[2]])
down_dot_i = np.array([down_dot_x, down_dot_y, down_dot_z])
distance = math.sqrt(np.dot(down_dot_i, down_dot_i))
return distance
# Making coordinate axes after proportional compression
new_axis_x = []
compression_radio = []
insert_point = int(kpt_lines[1].split()[0]) - 1
num_axis_x = float(0)
new_axis_x.append(num_axis_x)
for i in range(0, len(delline) + 1):
k = i * 3 + 4
compression_radio.append(down_distance_calculation_kpt(k, k + 1))
for j in range(0, insert_point):
num_axis_x = num_axis_x + down_distance_calculation_kpt(k, k + 1)
new_axis_x.append(num_axis_x)
# Get the K-path point density of each segment
int_compress_radio = []
for i in compression_radio:
int_compress_radio.append(int(max(compression_radio) // i))
return new_axis_x, int_compress_radio, insert_point
def monte_carlo():
elevation = 45 * deg
cam_dist = 0.2
cam_dist_xy = np.cos(elevation) * cam_dist * 0.5
diameter = 0.01
view_diameter_ratio = 10
fov = np.arctan(diameter * view_diameter_ratio / 2 / cam_dist) * 2
resolution = 200
K = get_cam_K(resolution, fov)
hole_world = np.array((0, 0, 0)).reshape(3, 1)
peg_world = np.array((0, 0.005, 0.01)).reshape(3, 1)
sigma_px = 1
sigma_K = 2
sigma_t_p = 0.01
max_angle_t = 2 * deg
N = 200
alpha = .5 / N**0.1
pt_size = 10
print('fov', fov * 180 / np.pi)
thetas = [np.pi / n for n in (2, 4, 8)]
fig, ax = plt.subplots(1, 1, figsize=(5, 5))
ax.set_xlim(0, resolution)
ax.set_ylim(0, resolution)
center = (resolution / 2, resolution / 2)
ax.add_artist(plt.Circle(center, resolution / view_diameter_ratio / 2))
ax.scatter(*(sigma_px * np.random.randn(N, 2) + center).T,
s=pt_size,
alpha=alpha,
lw=0,
c='r',
zorder=3)
ax.set_aspect(1)
ax.set_xlabel('u [px]')
ax.set_ylabel('v [px]')
plt.tight_layout()
fig, axs = plt.subplots(1, 3, figsize=(15, 5))
text = 'elevation: {:.2f} deg'.format(elevation / deg)
text += ', c2w unc.: ({:.2f} deg, {:.2f} mm)'.format(
max_angle_t / deg, sigma_t_p * 1e3)
text += ', vision unc.: {:.2f}'.format(sigma_px)
text += ', K unc.: {:.2f}'.format(sigma_K)
fig.suptitle(text)
for i, (theta, ax) in enumerate(zip(thetas, axs.reshape(-1))):
cams_t_world = [
get_cam_t_hole(cam_dist, azimuth, elevation)
for azimuth in (0, theta)
]
holes = []
pegs = []
for _ in range(N):
Ks_ = K.copy(), K.copy()
for K_ in Ks_:
K_[:2] += np.random.randn(2, 3) * sigma_K
c2ws_ = [
c2w @ noise_transform(sigma_t_p, max_angle_t)
for c2w in cams_t_world
]
for p, ls in zip((hole_world, peg_world), (holes, pegs)):
p_cams = [c2w.inv() @ p for c2w in c2ws_]
p_imgs_ = [
cam_to_image(p_cam, K) + np.random.randn(2, 1) * sigma_px
for p_cam in p_cams
]
p_cams_ = [
image_to_cam(p_img_, K_)
for p_img_, K_ in zip(p_imgs_, Ks_)
]
p_lines = [
line_eq((c2w @ p_cam_)[:2, 0], c2w.t[:2])
for c2w, p_cam_ in zip(cams_t_world, p_cams_)
]
p_ = np.cross(*p_lines)
p_ = p_[:2] / p_[2]
ls.append(p_)
holes = np.array(holes)
pegs = np.array(pegs)
for ps, name in zip((holes, pegs, pegs - holes),
('hole', 'peg', 'direction')):
ax.scatter(*ps.T, alpha=alpha, s=pt_size, lw=0, label=name)
# plt.hexbin(*ps.T, extent=(-hole_distance, hole_distance, -hole_distance, hole_distance), cmap=c, alpha=0.3)
a_t, b_t = cams_t_world[0].t[:2], cams_t_world[1].t[:2]
ax.scatter([a_t[0], b_t[0]], [a_t[1], b_t[1]], label='cams')
ax.grid()
ax.add_artist(
plt.Circle(hole_world[:2, 0],
diameter / 2,
fill=False,
color='blue'))
ax.add_artist(
plt.Circle(peg_world[:2, 0],
diameter / 2,
fill=False,
color='darkorange'))
ax.set_aspect(1)
ax.set_xlim(-cam_dist_xy, cam_dist_xy)
ax.set_ylim(-cam_dist_xy, cam_dist_xy)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('azimuth diff: {:.2f} deg'.format(theta / deg))
ax.legend()
plt.tight_layout()
plt.show()
def __place_edge_1node(arg, unit_cell, edge_coord, node_1_coord,
v11_nbors_for_e1):
edge = __edge_properties(arg)
connection_site_edge = edge[0]
distance_connection_site = edge[1]
angle_connection_site_pair = edge[2]
connectivity = edge[3]
elements = edge[4]
element_coord = edge[5]
bb_elements = []
bb_frac_coordinates = []
bb_connectivity = []
for j in range(len(edge_coord)):
node_1_vector = []
node_2_vector = []
##Get coordinates of neighboring nodes to find connection vectors
node_1_vector.append(node_1_coord[v11_nbors_for_e1[j][0]])
node_2_vector.append(node_1_coord[v11_nbors_for_e1[j][1]])
node_1_vector = np.asarray(
node_1_vector) # fract. coord. of neigboring nodes
node_2_vector = np.asarray(node_2_vector)
edge_node_1_vector = []
for i in range(len(node_1_vector)):
diffa = node_1_vector[i][0] - edge_coord[j][0]
diffb = node_1_vector[i][1] - edge_coord[j][1]
diffc = node_1_vector[i][2] - edge_coord[j][2]
### PERIODIC BOUNDARY CONDITIONS
if diffa > 0.5:
node_1_vector[i][0] = node_1_vector[i][0] - 1
elif diffa < -0.5:
node_1_vector[i][0] = node_1_vector[i][0] + 1
if diffb > 0.5:
node_1_vector[i][1] = node_1_vector[i][1] - 1
elif diffb < -0.5:
node_1_vector[i][1] = node_1_vector[i][1] + 1
if diffc > 0.5:
node_1_vector[i][2] = node_1_vector[i][2] - 1
elif diffc < -0.5:
node_1_vector[i][2] = node_1_vector[i][2] + 1
edge_node_1_vector.append(node_1_vector[i] - edge_coord[j])
edge_node_1_vector = np.asarray(
edge_node_1_vector
) ## fract vectors edge to node adjusted for PBCs
node_1_vector_real = []
for i in range(len(edge_node_1_vector)):
vector_1_real = np.dot(np.transpose(unit_cell),
edge_node_1_vector[i])
node_1_vector_real.append(vector_1_real)
node_1_vector_real = np.asarray(
node_1_vector_real
) # real (not fractional) coord vector of network
edge_node_2_vector = []
for i in range(len(node_2_vector)):
diffa = node_2_vector[i][0] - edge_coord[j][0]
diffb = node_2_vector[i][1] - edge_coord[j][1]
diffc = node_2_vector[i][2] - edge_coord[j][2]
### PERIODIC BOUNDARY CONDITIONS
if diffa > 0.5:
node_2_vector[i][0] = node_2_vector[i][0] - 1
elif diffa < -0.5:
node_2_vector[i][0] = node_2_vector[i][0] + 1
if diffb > 0.5:
node_2_vector[i][1] = node_2_vector[i][1] - 1
elif diffb < -0.5:
node_2_vector[i][1] = node_2_vector[i][1] + 1
if diffc > 0.5:
node_2_vector[i][2] = node_2_vector[i][2] - 1
elif diffc < -0.5:
node_2_vector[i][2] = node_2_vector[i][2] + 1
edge_node_2_vector.append(node_2_vector[i] - edge_coord[j])
edge_node_2_vector = np.asarray(
edge_node_2_vector
) ## fract vectors from edge to node 2 adjusted for PBCs
node_2_vector_real = []
for i in range(len(edge_node_2_vector)):
vector_2_real = np.dot(np.transpose(unit_cell),
edge_node_2_vector[i])
node_2_vector_real.append(vector_2_real)
node_2_vector_real = np.asarray(
node_2_vector_real
) # real (not fractional) coord vector of network
edge_coord_real = np.dot(
np.transpose(unit_cell),
edge_coord[j]) # real coord of network edge (centroid of edge)
norm_node_1_vector_real = []
for i in range(len(node_1_vector_real)):
norm = node_1_vector_real[i] / np.linalg.norm(
node_1_vector_real[i])
norm_node_1_vector_real.append(norm)
norm_node_1_vector_real = np.asarray(
norm_node_1_vector_real) # normalized network vectors
norm_node_2_vector_real = []
for i in range(len(node_2_vector_real)):
norm = node_2_vector_real[i] / np.linalg.norm(
node_2_vector_real[i])
norm_node_2_vector_real.append(norm)
norm_node_2_vector_real = np.asarray(
norm_node_2_vector_real) # normalized network vectors
connection_edge = []
connection_1 = norm_node_1_vector_real[0] * distance_connection_site[0]
connection_2 = norm_node_2_vector_real[0] * distance_connection_site[
1] ##coordinates to where edge connection sites should be placed
connection_edge.append(connection_1)
connection_edge.append(connection_2)
connection_edge.append(np.cross(connection_1, connection_2))
connection_edge = np.asarray(connection_edge)
connection_site = []
connection_site.append(connection_site_edge[0])
connection_site.append(connection_site_edge[1])
connection_site.append(
np.cross(connection_site_edge[0], connection_site_edge[1]))
connection_site = np.asarray(connection_site)
##Calculate transformation matrix, using quaternions, to map building block vectors onto network vectors
tfmatrix = superimposition_matrix(np.transpose(connection_site),
np.transpose(connection_edge),
usesvd=False)
connection_site_plusone = np.append(
connection_site, np.ones([len(connection_site), 1]),
1) # add a column of ones for dimension agreement
tf_connection_site = []
for i in range(len(connection_site)):
new_sites = np.dot(
tfmatrix, connection_site_plusone[i]
) #apply transformation matrix to each building block vector
tf_connection_site.append(new_sites)
tf_connection_site = np.asarray(
tf_connection_site
) #coordinates of building block connection sites, mapped onto network node sites
tf_connection_site = tf_connection_site[:, :
-1] #remove the column of ones, to obtain final set of coordinates
###Apply transformation matrix to all atoms in building block
element_coord_plusone = np.append(element_coord,
np.ones([len(element_coord), 1]), 1)
tf_element_coord = []
for i in range(len(element_coord)):
new_element = np.dot(tfmatrix, element_coord_plusone[i])
tf_element_coord.append(new_element)
tf_element_coord = np.asarray(tf_element_coord)
tf_element_coord = tf_element_coord[:, :-1]
tf_frac_element_coord = []
for i in range(len(tf_element_coord)):
frac_element_coord = np.dot(np.transpose(np.linalg.inv(unit_cell)),
tf_element_coord[i])
frac_element_coord = frac_element_coord + edge_coord[j]
tf_frac_element_coord.append(frac_element_coord)
tf_frac_element_coord = np.asarray(
tf_frac_element_coord) #edge after transformation, in frac coords
bb_elements.append(elements)
bb_frac_coordinates.append(tf_frac_element_coord)
bb_connectivity.append(connectivity)
bb_elements = np.asarray(bb_elements)
bb_frac_coordinates = np.asarray(bb_frac_coordinates)
bb_connectivity = np.asarray(bb_connectivity)
return bb_elements, bb_frac_coordinates, bb_connectivity
def drawHyrosphere(hyrosphere):
#glTranslatef(hyrosphere.position[0],hyrosphere.position[1], hyrosphere.position[2])
U = hyrosphere.U
A = U * np.sqrt(1.0 / 8.0)
b1 = np.cross(A[0, :], A[1, :])
b2 = np.cross(A[1, :], A[2, :])
b3 = np.cross(A[2, :], A[3, :])
b4 = np.cross(A[3, :], A[0, :])
b1 = b1 / np.linalg.norm(b1) * hyrosphere.t_len / 2.0
b2 = b2 / np.linalg.norm(b2) * hyrosphere.t_len / 2.0
b3 = b3 / np.linalg.norm(b3) * hyrosphere.t_len / 2.0
b4 = b4 / np.linalg.norm(b4) * hyrosphere.t_len / 2.0
b1 = np.dot(rotate_vec(U[0, :], hyrosphere.phi[0]), b1)
b2 = np.dot(rotate_vec(U[1, :], hyrosphere.phi[1]), b2)
b3 = np.dot(rotate_vec(U[2, :], hyrosphere.phi[2]), b3)
b4 = np.dot(rotate_vec(U[3, :], hyrosphere.phi[3]), b4)
B = np.array([b1, b2, b3, b4])
R = A + B
zeros = np.zeros(3)
drawLines(start=[zeros, zeros, zeros, zeros],
end=A,
color=(1, 0, 0, 0.5),
position=(0, 0, 0, 0))
drawLines(start=A, end=R, color=(1, 0, 0, 0.5), position=(0, 0, 0, 0))
#drawCylinders([zeros,zeros,zeros, zeros],A,radius=0.02, color=(1,0,0,0.5))
#drawCylinders(A,R,radius=0.02, color=(1,0,0,0.5))
drawArrow(np.zeros(3),
hyrosphere.velocity,
radius=0.02,
color=(1, 0, 0, 0.5))
drawCircle(B[0], A[0], A[0], color=(0, 0, 0.7, 0.3))
drawCircle(B[1], A[1], A[1], color=(0, 0, 0.7, 0.3))
drawCircle(B[2], A[2], A[2], color=(0, 0, 0.7, 0.3))
drawCircle(B[3], A[3], A[3], color=(0, 0, 0.7, 0.3))
drawSphere(center=(0, 0, 0),
radius=hyrosphere.t_len * np.sqrt(3.0 / 8.0),
colors=(90.0 / 256, 1.0, 39.0 / 256, 0.3))
drawSphere(center=R[0],
radius=hyrosphere.t_len * np.sqrt(3.0 / 8.0) / 20,
colors=(0, 0, 0, 0.2))
drawSphere(center=R[1],
radius=hyrosphere.t_len * np.sqrt(3.0 / 8.0) / 20,
colors=(0, 0, 0, 0.2))
drawSphere(center=R[2],
radius=hyrosphere.t_len * np.sqrt(3.0 / 8.0) / 20,
colors=(0, 0, 0, 0.2))
drawSphere(center=R[3],
radius=hyrosphere.t_len * np.sqrt(3.0 / 8.0) / 20,
colors=(0, 0, 0, 0.2))
#os_vec = np.asarray([0.0,0.0,-hyrosphere.radius])
#drawArrow(os_vec, os_vec + hyrosphere.Omega , radius=0.02, color=(1,0,0,0.5), nseg=10, mseg=10)
text = "position : {0:.2f} {1:.2f} {2:.2f}".format(hyrosphere.position[0],
hyrosphere.position[1],
hyrosphere.position[2])
drawText(position=(-1.0, 0.0, 1.0), textString=text)
text = "velocity : {0:.2f} {1:.2f} {2:.2f}".format(hyrosphere.velocity[0],
hyrosphere.velocity[1],
hyrosphere.velocity[2])
drawText(position=(-1.0, 0.0, 0.8), textString=text)
def specific_angular_momentum(r, v):
return np.cross(r, v)
def PlotSet(self, ds, p, dp, speed, force, R, amin, amax,
color=None):
if color is not None:
self.COLOR_REACHABLE_SET_LAST= color
tsamples= 15
[qnext,dtmp,utmp,tend] = params.ForwardSimulate(p, dp, speed, ds, force)
print "TEND:",tend,qnext
tstart = 0.0
Ndim = p.shape[0]
poly = []
### A all possible control input combination
A = np.vstack((amin,amax)).T
A = np.array(np.meshgrid(*A)).T.reshape(-1,amin.shape[0])
M_time = A.shape[0]
for dt in self.expspace(tstart,tend,tsamples):
dt2 = dt*dt*0.5
if self.PLOT_TOTAL_REACHABLE_SET:
M_speed = 5
speedvec = np.linspace(0,speed,M_speed)
else:
M_speed = 1
speedvec=[speed]
q = np.zeros((Ndim,M_speed*M_time))
for k in range(0,M_speed):
for i in range(0,M_time):
control = np.dot(R,A[i])
q[:,i+k*M_time] = p + dt*speedvec[k]*dp + dt2 * force + dt2 * control
self.add_points( q.T )
dt = tend
dt2 = dt*dt*0.5
qnext = p+dt*speed*dp+dt2*force
pnext = p+ds*dp/np.linalg.norm(dp)
self.arrow_head_size = np.linalg.norm(dt2*force)/5
self.hw = 0.5*self.arrow_head_size
self.lw = 0.2*self.arrow_head_size
self.image.scatter(p[0], p[1], p[3], color='k',
s=self.point_size)
self.image.scatter(pnext[0],pnext[1],pnext[3], 'og', color='g',
s=self.point_size)
self.image.scatter(qnext[0],qnext[1],qnext[3], 'ok',
s=self.point_size)
[qcontrol,qtmp,utmp,dtmp] = params.ForwardSimulate(p, dp, speed, ds, force)
dq = qcontrol - pnext
dnp = dp/np.linalg.norm(dp)
wmove = dq - np.dot(dq,dnp)*dnp
ds = np.linalg.norm(p-pnext)
if self.PLOT_PROJECTION_ONTO_REACHABLE_SET:
self.PlotPathSegment( pnext, qcontrol)
self.image.scatter(qcontrol[0],qcontrol[1],qcontrol[3], 'or',
color='r',
s=self.point_size)
text_offset_t = ds/10.0
text_offset_y = -ds/10.0
text_offset_x = -ds/5.0
if self.PLOT_SHOW_POINT_LABELS:
self.image.text(p[0]-text_offset_x, p[1]-text_offset_y, p[3]-text_offset_t,
"$p(s)$",
color='black',
fontsize=self.fs)
self.image.text(pnext[0]-text_offset_x, pnext[1]-text_offset_y, pnext[3]-text_offset_t,
"$p(s+\\Delta s)$",
color='black',
fontsize=self.fs)
dori = np.zeros((Ndim))
dori[0:2] = np.dot(Rz(p[3]),ex)[0:2]
dori[0:2] = 0.5*(dori[0:2]/np.linalg.norm(dori[0:2]))
dori[3] = p[3]
pathnext = p + 1.5*ds*dp/np.linalg.norm(dp)
pathlast = p - 0.2*ds*dp/np.linalg.norm(dp)
self.PlotPathSegment( pathlast, pathnext)
#arrow0 = self.PlotArrow(p, dt*s*dori, self.orientation_color)
arrow1 = self.PlotArrow(p+dt2*force, dt*speed*dp, self.tangent_color, ls='dashed')
arrow1 = self.PlotArrow(p, dt*speed*dp, self.tangent_color)
arrow2 = self.PlotArrow(p, dt2*force, self.force_color)
#arrow0 = self.PlotArrow(qnext, dt*s*dori, self.orientation_color)
arrow2 = self.PlotArrow(p+dt*speed*dp, dt2*force, self.force_color)
plt.legend([arrow1,arrow2,],
['Velocity Displacement','Wrench Displacement',],
fontsize=self.fs,
loc=self.loc)
self.origin = p
self.tangent = dp
#draw sphere
if self.PLOT_SPHERE_SEGMENT:
ts1 = acos( np.dot(dp[0:2],ex[0:2])/np.linalg.norm(dp[0:2]))
ts2 = acos( np.dot((qnext-p)[0:2],ex[0:2])/np.linalg.norm((qnext-p)[0:2]))
toffset = pi/16
toffsetz = pi/4
ori1 = np.cross(ex[0:2],(dp)[0:2])
ori2 = np.cross(ex[0:2],(qnext-p)[0:2])
if ori1 < 0:
ts1 *= -1
if ori2 < 0:
ts2 *= -1
if ts1 > ts2:
tlimU = ts1+toffset
tlimL = ts2-2*toffset
else:
tlimL = ts1-toffset
tlimU = ts2+2*toffset
u, v = np.mgrid[tlimL:tlimU:10j, 0+toffsetz:np.pi/2+toffsetz/4:10j]
x=ds*np.cos(u)*np.sin(v) + p[0]
y=ds*np.sin(u)*np.sin(v) + p[1]
z=ds*np.cos(v) + p[3]
self.image.plot_surface( x, y, z, rstride=2, cstride=2,
color='c', alpha=0.3, linewidth=2,
edgecolors=np.array((0.5,0.5,0.5,0.5)))
self.PlotPoly()
self.PlotPrettify()
def drawCylinder(start, end, radius, color, close=True, nseg=20, mseg=40):
if np.linalg.norm(start) > 10e-4:
d = (end - start)
p = np.cross(start, d)
else:
d = end
p = np.cross([1.0] * 3, d)
p = p / np.linalg.norm(p)
p = p * radius
points = []
curr_point = p
alpha = 2.0 * np.pi / nseg
for i in range(0, nseg):
points.append(curr_point)
curr_point = np.dot(rotate_vec(d, alpha), curr_point)
layers = []
curr_pos = start
step = d / (mseg - 1)
for i in range(0, mseg):
layer = [point + start + i * step for point in points]
layers.append(layer)
quads = []
for i in range(0, mseg - 1):
for j in range(0, nseg - 1):
quads.append([
layers[i][j], layers[i + 1][j], layers[i + 1][j + 1],
layers[i][j + 1]
])
join_layer = [
layers[mseg - 1][0], layers[mseg - 1][nseg - 1],
layers[0][nseg - 1], layers[0][0]
]
quads.append(join_layer)
glPushMatrix()
glBegin(GL_QUADS)
glColor4f(color[0], color[1], color[2], color[3])
for quad in quads:
glVertex3fv(quad[0])
glVertex3fv(quad[1])
glVertex3fv(quad[2])
glVertex3fv(quad[3])
glEnd()
if close:
glBegin(GL_POLYGON)
for point in layers[0]:
glVertex3fv(point)
glEnd()
glBegin(GL_POLYGON)
for point in layers[mseg - 1]:
glVertex3fv(point)
glEnd()
glPopMatrix()
def homography(points, last_points):
stat = 1
try:
x1c = points[0, 0, 0]
y1c = points[0, 0, 1]
x2c = points[1, 0, 0]
y2c = points[1, 0, 1]
x3c = points[2, 0, 0]
y3c = points[2, 0, 1]
x4c = points[3, 0, 0]
y4c = points[3, 0, 1]
if len(points) > 4:
fail = points[12, 0, 0]
if cv2.contourArea(points) > 1658880:
fail = points[12, 0, 0]
except:
x1c = last_points[0, 0, 0]
y1c = last_points[0, 0, 1]
x2c = last_points[1, 0, 0]
y2c = last_points[1, 0, 1]
x3c = last_points[2, 0, 0]
y3c = last_points[2, 0, 1]
x4c = last_points[3, 0, 0]
y4c = last_points[3, 0, 1]
stat = 0
srcPoints = np.array([[x1c, y1c], [x2c, y2c], [x3c, y3c], [x4c, y4c]])
dstPoints = np.array([[ref_scale, 0], [ref_scale, ref_scale],
[0, ref_scale], [0, 0]])
x1w = ref_scale
y1w = 0
x2w = ref_scale
y2w = ref_scale
x3w = 0
y3w = ref_scale
x4w = 0
y4w = 0
p1 = [x1w, y1w, 0, 1]
p2 = [x2w, y2w, 0, 1]
p3 = [x3w, y3w, 0, 1]
p4 = [x4w, y4w, 0, 1]
l1 = [x1w, y1w, 1, 0, 0, 0, -x1c * x1w, -x1c * y1w, -x1c]
l2 = [0, 0, 0, x1w, y1w, 1, -y1c * x1w, -y1c * y1w, -y1c]
l3 = [x2w, y2w, 1, 0, 0, 0, -x2c * x2w, -x2c * y2w, -x2c]
l4 = [0, 0, 0, x2w, y2w, 1, -y2c * x2w, -y2c * y2w, -y2c]
l5 = [x3w, y3w, 1, 0, 0, 0, -x3c * x3w, -x3c * y3w, -x3c]
l6 = [0, 0, 0, x3w, y3w, 1, -y3c * x3w, -y3c * y3w, -y3c]
l7 = [x4w, y4w, 1, 0, 0, 0, -x4c * x4w, -x4c * y4w, -x4c]
l8 = [0, 0, 0, x4w, y4w, 1, -y4c * x4w, -y4c * y4w, -y4c]
A = np.vstack([l1, l2, l3, l4, l5, l6, l7, l8])
U, S, V = np.linalg.svd(A)
V = V[8]
h = V / V[8]
H = np.reshape(h, (3, 3))
R = np.dot(np.linalg.inv(K), H)
c1 = R[:, 0]
c2 = R[:, 1]
c3 = R[:, 2]
h1, h2, h3 = H
h1 = h1.reshape(3, 1)
h2 = h2.reshape(3, 1)
one = np.dot(np.linalg.inv(K), h1)
two = np.dot(np.linalg.inv(K), h2)
top = (np.linalg.norm(c1) + np.linalg.norm(c2))
lam = (top / 2)**(-1)
Bt = lam * R
if np.linalg.det(Bt) < 0:
Bt = Bt * (-1)
B = Bt
b1, b2, b3 = B.T
b1 = b1.reshape(1, 3)
b2 = b2.reshape(1, 3)
b3 = b3.reshape(1, 3)
r1 = b1
r2 = b2
r3 = np.cross(r1, r2)
t = b3
T = np.concatenate((r1.T, r2.T, r3.T, t.T), axis=1)
P = np.matmul(K, T)
return P, H, stat
def runFilter(self, accMeas, gyroMeas, otherMeas, dT):
#spool up after initialization
if (self.timeConstant < self.tcTarget):
self.timeConstant += .0001 * (self.tcTarget -
self.timeConstant) + .001
# integrate angular velocity to get
omega = gyroMeas - self.gyroBias
# get the magnitude of the angular velocity
magOmega = np.linalg.norm(omega)
# recover the axis around which it's spinning
axis = omega / (magOmega + 1e-13)
# calculate the angle through which it has spun
angle = magOmega * dT
# make a quaternion
deltaAttPredict = AQ.quatFromAxisAngle(axis, angle)
# integrate attitude
self.attitudeEstimate = deltaAttPredict * self.attitudeEstimate
# unpack attitude into Eulers
r, p, y = self.attitudeEstimate.asEuler
# Do we have magnetometer information?
for ii in otherMeas:
if (ii[0] == 'mag'):
# rotate magnetometer reading into earth reference frame
magReadingInEarth = np.dot(self.attitudeEstimate.asRotMat.T,
ii[1])
# expected magnetometer reading.
# Declination can be determined if you know your GPS coordinates.
# The code in Arducopter does this.
#earthMagReading = np.array([[.48407,.12519,.8660254]]).T # corresponds to 14.5deg east, 60deg downward inclination
earthMagReading = ii[
2] # corresponds to 14.5deg east, 60deg downward inclination
# the z-component of the cross product of these two quantities is proportional to the sine of the yaw error,
# for small angles, sin(x) ~= x
yawErr = np.cross(magReadingInEarth.T, earthMagReading.T)
# update yaw
y = y + 180. / np.pi * yawErr[0][2]
# do first slerp for yaw
updateQuat = AQ.Quaternion(np.array([r, p, y]))
newAtt = AQ.slerp(self.attitudeEstimate, updateQuat,
min(20 * dT / (self.timeConstant + dT), .5))
# process accel data (if magnitude of acceleration is around 1g)
r, p, y = newAtt.asEuler
if (abs(np.linalg.norm(accMeas) - grav) < .5):
# incorporate accelerometer data
#updateQuat = attFromAccel(y,accMeas)
deltaA = np.cross((newAtt.asRotMat * (-gravity)).T, accMeas.T)
dMagAcc = np.linalg.norm(deltaA)
if (dMagAcc != 0):
axisAcc = (1. / dMagAcc) * deltaA
else:
axisAcc = np.array([[1., 0., 0.]])
updateQuat = AQ.quatFromAxisAngle(axisAcc, dMagAcc)
else:
# rebuild attitude estimate. May have no change, but 'y' may have magnetometer info incorporated into it, which doesn't care if we're accelerating.
updateQuat = AQ.Quaternion(np.array([r, p, y]))
# slerp between integrated and measured attitude
newAtt = AQ.slerp(newAtt, updateQuat, dT / (self.timeConstant + dT))
# calculate difference between measurement and integrated for bias calc
errorQuat = (newAtt * self.attitudeEstimate.inv()).q
# calculate bias derivative
# if we think of a quaternion in terms of the axis-angle representation of a rotation, then
# dividing the vector component of a quaternion by its scalar component gives a vector proportional
# to tan(angle/2). For small angles this is pretty close to angle/2.
biasDot = -np.dot(
np.array([[self.Kbias, 0, 0], [0, self.Kbias, 0],
[0, 0, self.Kbias]]), (1. / errorQuat[3]) *
np.array([[errorQuat[0]], [errorQuat[1]], [errorQuat[2]]]))
# Update gyro bias
self.gyroBias = self.gyroBias + dT * biasDot
# update attitude
self.attitudeEstimate = newAtt
return [self.attitudeEstimate, self.gyroBias]
def M(axis, theta):
return expm(np.cross(np.eye(3), axis / norm(axis) * theta))
def check_for_stop_signs(self, waypoints, closest_index, goal_index):
"""Checks for a stop sign that is intervening the goal path.
Checks for a stop sign that is intervening the goal path. Returns a new
goal index (the current goal index is obstructed by a stop line), and a
boolean flag indicating if a stop sign obstruction was found.
args:
waypoints: current waypoints to track. (global frame)
length and speed in m and m/s.
(includes speed to track at each x,y location.)
format: [[x0, y0, v0],
[x1, y1, v1],
...
[xn, yn, vn]]
example:
waypoints[2][1]:
returns the 3rd waypoint's y position
waypoints[5]:
returns [x5, y5, v5] (6th waypoint)
closest_index: index of the waypoint which is closest to the vehicle.
i.e. waypoints[closest_index] gives the waypoint closest to the vehicle.
goal_index (current): Current goal index for the vehicle to reach
i.e. waypoints[goal_index] gives the goal waypoint
variables to set:
[goal_index (updated), stop_sign_found]:
goal_index (updated): Updated goal index for the vehicle to reach
i.e. waypoints[goal_index] gives the goal waypoint
stop_sign_found: Boolean flag for whether a stop sign was found or not
"""
for i in range(closest_index, goal_index):
# Check to see if path segment crosses any of the stop lines.
intersect_flag = False
for stopsign_fence in self._stopsign_fences:
wp_1 = np.array(waypoints[i][0:2])
wp_2 = np.array(waypoints[i + 1][0:2])
s_1 = np.array(stopsign_fence[0:2])
s_2 = np.array(stopsign_fence[2:4])
v1 = np.subtract(wp_2, wp_1)
v2 = np.subtract(s_1, wp_2)
sign_1 = np.sign(np.cross(v1, v2))
v2 = np.subtract(s_2, wp_2)
sign_2 = np.sign(np.cross(v1, v2))
v1 = np.subtract(s_2, s_1)
v2 = np.subtract(wp_1, s_2)
sign_3 = np.sign(np.cross(v1, v2))
v2 = np.subtract(wp_2, s_2)
sign_4 = np.sign(np.cross(v1, v2))
# Check if the line segments intersect.
if (sign_1 != sign_2) and (sign_3 != sign_4):
intersect_flag = True
# Check if the collinearity cases hold.
if (sign_1 == 0) and pointOnSegment(wp_1, s_1, wp_2):
intersect_flag = True
if (sign_2 == 0) and pointOnSegment(wp_1, s_2, wp_2):
intersect_flag = True
if (sign_3 == 0) and pointOnSegment(s_1, wp_1, s_2):
intersect_flag = True
if (sign_4 == 0) and pointOnSegment(s_1, wp_2, s_2):
intersect_flag = True
# If there is an intersection with a stop line, update
# the goal state to stop before the goal line.
if intersect_flag:
goal_index = i
return goal_index, True
return goal_index, False
def test_fixedjoint():
# Define the rod for testing
# Some rod properties. We need them for constructer, they are not used.
normal1 = np.array([0.0, 1.0, 0.0])
direction = np.array([1.0, 0.0, 0.0])
normal2 = np.array([0.0, 0.0, 1.0])
direction2 = np.array([1.0 / np.sqrt(2), 1.0 / np.sqrt(2), 0.0])
# direction2 = np.array([0.,1.0,0.])
base_length = 1
base_radius = 0.2
density = 1
nu = 0.1
# Youngs Modulus [Pa]
youngs_modulus = 1e6
# poisson ratio
poisson_ratio = 0.5
shear_modulus = youngs_modulus / (poisson_ratio + 1.0)
# Origin of the rod
origin1 = np.array([0.0, 0.0, 0.0])
origin2 = np.array([1.0, 0.0, 0.0])
# Number of elements
n = 2
# create rod classes
rod1 = CosseratRod.straight_rod(
n,
origin1,
direction,
normal1,
base_length,
base_radius,
density,
nu,
youngs_modulus,
shear_modulus=shear_modulus,
)
rod2 = CosseratRod.straight_rod(
n,
origin2,
direction2,
normal2,
base_length,
base_radius,
density,
nu,
youngs_modulus,
shear_modulus=shear_modulus,
)
# Rod velocity
v1 = np.array([-1, 0, 0]).reshape(3, 1)
v2 = v1 * -1
rod1.velocity_collection = np.tile(v1, (1, n + 1))
rod2.velocity_collection = np.tile(v2, (1, n + 1))
# Stiffness between points
k = 1e8
kt = 1e6
# Damping between two points
nu = 1
# Rod indexes
rod1_index = -1
rod2_index = 0
# Compute the free joint forces
distance = (rod2.position_collection[..., rod2_index] -
rod1.position_collection[..., rod1_index])
end_distance = np.linalg.norm(distance)
if end_distance == 0:
end_distance = 1
elasticforce = k * distance
relative_vel = (rod2.velocity_collection[..., rod2_index] -
rod1.velocity_collection[..., rod1_index])
normal_relative_vel = np.dot(relative_vel, distance) / end_distance
dampingforce = nu * normal_relative_vel * distance / end_distance
contactforce = elasticforce - dampingforce
fxjt = FixedJoint(k, nu, kt)
fxjt.apply_forces(rod1, rod1_index, rod2, rod2_index)
fxjt.apply_torques(rod1, rod1_index, rod2, rod2_index)
assert_allclose(rod1.external_forces[..., rod1_index],
contactforce,
atol=Tolerance.atol())
assert_allclose(rod2.external_forces[..., rod2_index],
-1 * contactforce,
atol=Tolerance.atol())
linkdirection = (rod2.position_collection[..., rod2_index + 1] -
rod2.position_collection[..., rod2_index])
positiondiff = (rod1.position_collection[..., rod1_index] -
rod1.position_collection[..., rod1_index - 1])
tangent = positiondiff / np.sqrt(np.dot(positiondiff, positiondiff))
# rod 2 has to be alligned with rod 1
check1 = (rod1.position_collection[..., rod1_index] +
rod2.rest_lengths[rod2_index] * tangent)
check2 = rod2.position_collection[..., rod2_index + 1]
forcedirection = -kt * (check2 - check1)
torque = np.cross(linkdirection, forcedirection)
torque_rod1 = -rod1.director_collection[..., rod1_index] @ torque
torque_rod2 = rod2.director_collection[..., rod2_index] @ torque
assert_allclose(rod1.external_torques[..., rod1_index],
torque_rod1,
atol=Tolerance.atol())
assert_allclose(rod2.external_torques[..., rod2_index],
torque_rod2,
atol=Tolerance.atol())
ax.set_ylim(-.25, .25)
'''
#plot bloch sphere
# Make data
elev = 10.0
rot = 80.0 / 180 * np.pi
u = np.linspace(0, 2 * np.pi, 100)
v = np.linspace(0, np.pi, 100)
x = 1. * np.outer(np.cos(u), np.sin(v))
y = 1. * np.outer(np.sin(u), np.sin(v))
z = 1. * np.outer(np.ones(np.size(u)), np.cos(v))
#plot the grid lines
a = np.array([-np.sin(elev / 180 * np.pi), 0, np.cos(elev / 180 * np.pi)])
b = np.array([0, 1, 0])
b = +np.cross(a, b) + a * np.dot(a, b)
ax.plot(np.sin(u), np.cos(u), 0, color='k', linestyle='dashed', alpha=0.05)
horiz_front = np.linspace(0, np.pi, 100)
ax.plot(np.sin(horiz_front),
np.cos(horiz_front),
0,
color='k',
linestyle='dashed',
alpha=0.05)
vert_front = np.linspace(np.pi / 2, 3 * np.pi / 2, 100)
ax.plot(a[0] * np.sin(u) + b[0] * np.cos(u),
b[1] * np.cos(u),
a[2] * np.sin(u) + b[2] * np.cos(u),
color='k',
linestyle='dashed',
alpha=0.05)