def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
>>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
True
"""
q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True)
nq = numpy.dot(q, q)
if nq < _EPS:
return numpy.identity(4)
q *= math.sqrt(2.0 / nq)
q = numpy.outer(q, q)
return numpy.array((
(1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0),
( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0),
( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
( 0.0, 0.0, 0.0, 1.0)
), dtype=numpy.float64)
python类identity()的实例源码
transformations.py 文件源码
项目：Neural-Networks-for-Inverse-Kinematics
作者: paramrajpura
项目源码
文件源码
阅读 45
收藏 0
点赞 0
评论 0
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2, numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
transformations.py 文件源码
项目：Neural-Networks-for-Inverse-Kinematics
作者: paramrajpura
项目源码
文件源码
阅读 26
收藏 0
点赞 0
评论 0
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> numpy.allclose(M, numpy.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
True
"""
q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
n = numpy.dot(q, q)
if n < _EPS:
return numpy.identity(4)
q *= math.sqrt(2.0 / n)
q = numpy.outer(q, q)
return numpy.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.0
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2., numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2, numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
def orthogonalization_matrix(lengths, angles):
"""Return orthogonalization matrix for crystallographic cell coordinates.
Angles are expected in degrees.
The de-orthogonalization matrix is the inverse.
>>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
>>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
True
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
>>> numpy.allclose(numpy.sum(O), 43.063229)
True
"""
a, b, c = lengths
angles = numpy.radians(angles)
sina, sinb, _ = numpy.sin(angles)
cosa, cosb, cosg = numpy.cos(angles)
co = (cosa * cosb - cosg) / (sina * sinb)
return numpy.array([
[ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0],
[-a*sinb*co, b*sina, 0.0, 0.0],
[ a*cosb, b*cosa, c, 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> numpy.allclose(M, numpy.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
True
"""
q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
n = numpy.dot(q, q)
if n < _EPS:
return numpy.identity(4)
q *= math.sqrt(2.0 / n)
q = numpy.outer(q, q)
return numpy.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def test_basic(self):
import numpy.linalg as linalg
A = np.array([[1., 2.], [3., 4.]])
mA = matrix(A)
B = np.identity(2)
for i in range(6):
assert_(np.allclose((mA ** i).A, B))
B = np.dot(B, A)
Ainv = linalg.inv(A)
B = np.identity(2)
for i in range(6):
assert_(np.allclose((mA ** -i).A, B))
B = np.dot(B, Ainv)
assert_(np.allclose((mA * mA).A, np.dot(A, A)))
assert_(np.allclose((mA + mA).A, (A + A)))
assert_(np.allclose((3*mA).A, (3*A)))
mA2 = matrix(A)
mA2 *= 3
assert_(np.allclose(mA2.A, 3*A))
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2, numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
def orthogonalization_matrix(lengths, angles):
"""Return orthogonalization matrix for crystallographic cell coordinates.
Angles are expected in degrees.
The de-orthogonalization matrix is the inverse.
>>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
>>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
True
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
>>> numpy.allclose(numpy.sum(O), 43.063229)
True
"""
a, b, c = lengths
angles = numpy.radians(angles)
sina, sinb, _ = numpy.sin(angles)
cosa, cosb, cosg = numpy.cos(angles)
co = (cosa * cosb - cosg) / (sina * sinb)
return numpy.array([
[ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0],
[-a*sinb*co, b*sina, 0.0, 0.0],
[ a*cosb, b*cosa, c, 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> numpy.allclose(M, numpy.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
True
"""
q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
n = numpy.dot(q, q)
if n < _EPS:
return numpy.identity(4)
q *= math.sqrt(2.0 / n)
q = numpy.outer(q, q)
return numpy.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def svm(x, y):
"""
classification SVM
Minimize
1/2 * w^T w
subject to
y_n (w^T x_n + b) >= 1
"""
weights_total = len(x[0])
I_n = np.identity(weights_total-1)
P_int = np.vstack(([np.zeros(weights_total-1)], I_n))
zeros = np.array([np.zeros(weights_total)]).T
P = np.hstack((zeros, P_int))
q = np.zeros(weights_total)
G = -1 * vec_to_dia(y).dot(x)
h = -1 * np.ones(len(y))
matrix_arg = [ matrix(x) for x in [P,q,G,h] ]
sol = solvers.qp(*matrix_arg)
return np.array(sol['x']).flatten()
def lookat(eye, center, up):
f = normalize(center - eye)
s = normalize(cross(f, up))
u = cross(s, f)
res = identity()
res[0][0] = s[0]
res[1][0] = s[1]
res[2][0] = s[2]
res[0][1] = u[0]
res[1][1] = u[1]
res[2][1] = u[2]
res[0][2] = -f[0]
res[1][2] = -f[1]
res[2][2] = -f[2]
res[3][0] = -dot(s, eye)
res[3][1] = -dot(u, eye)
res[3][2] = -dot(f, eye)
return res.T
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2, numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
def orthogonalization_matrix(lengths, angles):
"""Return orthogonalization matrix for crystallographic cell coordinates.
Angles are expected in degrees.
The de-orthogonalization matrix is the inverse.
>>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
>>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
True
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
>>> numpy.allclose(numpy.sum(O), 43.063229)
True
"""
a, b, c = lengths
angles = numpy.radians(angles)
sina, sinb, _ = numpy.sin(angles)
cosa, cosb, cosg = numpy.cos(angles)
co = (cosa * cosb - cosg) / (sina * sinb)
return numpy.array([
[ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0],
[-a*sinb*co, b*sina, 0.0, 0.0],
[ a*cosb, b*cosa, c, 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def quaternion_matrix(quaternion):
"""Return homogeneous rotation matrix from quaternion.
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
>>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
True
>>> M = quaternion_matrix([1, 0, 0, 0])
>>> numpy.allclose(M, numpy.identity(4))
True
>>> M = quaternion_matrix([0, 1, 0, 0])
>>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
True
"""
q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
n = numpy.dot(q, q)
if n < _EPS:
return numpy.identity(4)
q *= math.sqrt(2.0 / n)
q = numpy.outer(q, q)
return numpy.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def calc_rotation_matrix(q1, q2, ref_q1, ref_q2):
ref_nv = np.cross(ref_q1, ref_q2)
q_nv = np.cross(q1, q2)
if min(norm(ref_nv), norm(q_nv)) == 0.: # avoid 0 degree including angle
return np.identity(3)
axis = np.cross(ref_nv, q_nv)
angle = rad2deg(acos(ref_nv.dot(q_nv) / (norm(ref_nv) * norm(q_nv))))
R1 = axis_angle_to_rotation_matrix(axis, angle)
rot_ref_q1, rot_ref_q2 = R1.dot(ref_q1), R1.dot(ref_q2) # rotate ref_q1,2 plane to q1,2 plane
cos1 = max(min(q1.dot(rot_ref_q1) / (norm(rot_ref_q1) * norm(q1)), 1.), -1.) # avoid math domain error
cos2 = max(min(q2.dot(rot_ref_q2) / (norm(rot_ref_q2) * norm(q2)), 1.), -1.)
angle1 = rad2deg(acos(cos1))
angle2 = rad2deg(acos(cos2))
angle = (angle1 + angle2) / 2.
axis = np.cross(rot_ref_q1, q1)
R2 = axis_angle_to_rotation_matrix(axis, angle)
R = R2.dot(R1)
return R
def __init__(self, posterior, scale, nsims, initials,
cov_matrix=None, thinning=2, warm_up_period=True, model_object=None, quiet_progress=False):
self.posterior = posterior
self.scale = scale
self.nsims = (1+warm_up_period)*nsims*thinning
self.initials = initials
self.param_no = self.initials.shape[0]
self.phi = np.zeros([self.nsims, self.param_no])
self.phi[0] = self.initials # point from which to start the Metropolis-Hasting algorithm
self.quiet_progress = quiet_progress
if cov_matrix is None:
self.cov_matrix = np.identity(self.param_no) * np.abs(self.initials)
else:
self.cov_matrix = cov_matrix
self.thinning = thinning
self.warm_up_period = warm_up_period
if model_object is not None:
self.model = model_object
def _ss_matrices(self, beta):
""" Creates the state space matrices required
Parameters
----------
beta : np.array
Contains untransformed starting values for latent variables
Returns
----------
T, Z, R, Q, H : np.array
State space matrices used in KFS algorithm
"""
T = np.identity(self.z_no-1)
H = np.identity(1)*self.latent_variables.z_list[0].prior.transform(beta[0])
Z = self.X
R = np.identity(self.z_no-1)
Q = np.identity(self.z_no-1)
for i in range(0,self.z_no-1):
Q[i][i] = self.latent_variables.z_list[i+1].prior.transform(beta[i+1])
return T, Z, R, Q, H
def _ss_matrices(self,beta):
""" Creates the state space matrices required
Parameters
----------
beta : np.array
Contains untransformed starting values for latent variables
Returns
----------
T, Z, R, Q, H : np.array
State space matrices used in KFS algorithm
"""
T = np.identity(self.z_no-1)
H = np.identity(1)*self.latent_variables.z_list[0].prior.transform(beta[0])
Z = self.X
R = np.identity(self.z_no-1)
Q = np.identity(self.z_no-1)
for i in range(0,self.z_no-1):
Q[i][i] = self.latent_variables.z_list[i+1].prior.transform(beta[i+1])
return T, Z, R, Q, H
def _ss_matrices(self,beta):
""" Creates the state space matrices required
Parameters
----------
beta : np.array
Contains untransformed starting values for latent_variables
Returns
----------
T, Z, R, Q, H : np.array
State space matrices used in KFS algorithm
"""
T = np.identity(1)
R = np.identity(1)
Z = np.identity(1)
H = np.identity(1)*self.latent_variables.z_list[0].prior.transform(beta[0])
Q = np.identity(1)*self.latent_variables.z_list[1].prior.transform(beta[1])
return T, Z, R, Q, H
def _ss_matrices(self, beta):
""" Creates the state space matrices required
Parameters
----------
beta : np.array
Contains untransformed starting values for latent variables
Returns
----------
T, Z, R, Q : np.array
State space matrices used in KFS algorithm
"""
T = np.identity(1)
R = np.identity(1)
Z = np.identity(1)
Q = np.identity(1)*self.latent_variables.z_list[0].prior.transform(beta[0])
return T, Z, R, Q
def _ss_matrices(self,beta):
""" Creates the state space matrices required
Parameters
----------
beta : np.array
Contains untransformed starting values for latent variables
Returns
----------
T, Z, R, Q : np.array
State space matrices used in KFS algorithm
"""
T = np.identity(self.state_no)
Z = self.X
R = np.identity(self.state_no)
Q = np.identity(self.state_no)
for i in range(0,self.state_no):
Q[i][i] = self.latent_variables.z_list[i].prior.transform(beta[i])
return T, Z, R, Q
def setup_awg_channels(physicalChannels):
translators = {}
for chan in physicalChannels:
translators[chan.instrument] = import_module('QGL.drivers.' + chan.translator)
data = {awg: translator.get_empty_channel_set()
for awg, translator in translators.items()}
for name, awg in data.items():
for chan in awg.keys():
awg[chan] = {
'linkList': [],
'wfLib': {},
'correctionT': np.identity(2)
}
data[name]['translator'] = translators[name]
data[name]['seqFileExt'] = translators[name].get_seq_file_extension()
return data
def reflection_matrix(point, normal):
"""Return matrix to mirror at plane defined by point and normal vector.
>>> v0 = numpy.random.random(4) - 0.5
>>> v0[3] = 1.0
>>> v1 = numpy.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> numpy.allclose(2., numpy.trace(R))
True
>>> numpy.allclose(v0, numpy.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> numpy.allclose(v2, numpy.dot(R, v3))
True
"""
normal = unit_vector(normal[:3])
M = numpy.identity(4)
M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
return M
def dexp(v):
x = np.linalg.norm(v)
a = trig.sinox(x)
b = trig.cosox2(x)
c = trig.sinox3(x)
I = np.identity(3)
S = skew(v)
W = v * v.transpose()
return a*I + b*S + c*W
def dlog(v):
x = np.linalg.norm(v)
y = trig.specialFun4(x)
z = trig.specialFun2(x)
I = np.identity(3)
S = skew(v)
W = v * v.transpose()
return y*I - 0.5*S + z*W
def mt(v):
r = v[0:3]
t = v[3:6]
x = np.linalg.norm(r)
b = trig.cosox2(x)
c = trig.sinox3(x)
g = trig.specialFun1(x)
h = trig.specialFun3(x)
I = np.identity(3)
return b*quat.skew(t) + c*(r*t.transpose() + t*r.transpose()) + v3.dot(r,t)*((c - b) * I + g*quat.skew(r) + h*r*r.transpose())
def get_embeddings(self, *args):
return np.identity(self.output_size, np.float32)
def fit(self, paths):
featmat = np.concatenate([self._features(path) for path in paths])
returns = np.concatenate([path["returns"] for path in paths])
n_col = featmat.shape[1]
lamb = 2.0
self.coeffs = np.linalg.lstsq(featmat.T.dot(featmat) + lamb * np.identity(n_col), featmat.T.dot(returns))[0]
