def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
a = joint_angle
x = self.links[joint_name][0]
y = self.links[joint_name][1]
z = self.links[joint_name][2]
c = cos(a)
s = sin(a)
matrix_x = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0],
[x, y, z, 1]])
matrix_y = matrix([[0, 1, 0, 0], [0, c, -s, 0], [-s, 0, c, 0],
[x, y, z, 1]])
matrix_z = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0],
[x, y, z, 1]])
if 'Roll' in joint_name: #x
return matrix_x
elif 'Pitch' in joint_name: #y
return matrix_y
else:
return matrix_z
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = math.sin(joint_angle)
c = math.cos(joint_angle)
r_x = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]])
r_z = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
r_y = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]])
if "Yaw" in joint_name:
T *= r_z
if "Pitch" in joint_name:
T *= r_y
if "Roll" in joint_name:
T *= r_x
T[0, 3] = self.jointLength[joint_name][0]
T[1, 3] = self.jointLength[joint_name][1]
T[2, 3] = self.jointLength[joint_name][2]
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
R_x = identity(4)
R_y = identity(4)
R_z = identity(4)
sin_angle = sin(joint_angle)
cos_angle = cos(joint_angle)
x = self.link_offsets[joint_name][0]
y = self.link_offsets[joint_name][1]
z = self.link_offsets[joint_name][2]
if 'Roll' in joint_name:
R_x = matrix([[1, 0, 0, 0], [0, cos_angle, -sin_angle, 0],
[0, sin_angle, cos_angle, 0], [x, y, z, 1]])
elif 'Pitch' in joint_name:
R_y = matrix([[cos_angle, 0, sin_angle, 0], [0, 1, 0, 0],
[-sin_angle, 0, cos_angle, 0], [x, y, z, 1]])
elif 'Yaw' in joint_name:
R_z = matrix([[cos_angle, sin_angle, 0, 0],
[-sin_angle, cos_angle, 0, 0], [0, 0, 1, 0],
[x, y, z, 1]])
T = R_x * R_y * R_z
return T
def test_basic(self):
# test basic construction
T = Transform()
assert_allclose(T, np.matrix(np.eye(4)))
# test initializing with rotation matrix
T = Transform(Rpitch(pi / 4))
N = np.eye(4)
N[:3, :3] = Rpitch(pi / 4)
assert_allclose(T, N)
# test initializing with position vector
pos = np.matrix([5, 2, 4]).T
T = Transform(p=pos)
N = np.eye(4)
N[:3, 3] = pos
assert_allclose(T, N)
# test initializing with both rotation and position vector
R = Ryaw(5)
p = np.matrix('3;2;4')
T = Transform(R, p)
N = np.eye(4)
N[:3, :3] = R
N[:3, 3] = p
assert_allclose(T, N)
def update_pos(self, pose, absolute=False):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# if we have absolute (i.e. QR-code) data, and it's far from our current
# estimate, then assume we're lost and reset the EKF.
if absolute:
# we can intelligently mod the incoming pose angle to be close
# to the current estimate - note that the ordered if statements
# mean that if we are super-lost, we will randomly add some pi
# but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2,2] < 10:
while pose.theta - self.state[2,0] > pi:
pose.theta -= tau
while self.state[2,0] - pose.theta > pi:
pose.theta += tau
dx = self.state[0,0]-pose.x
dy = self.state[1,0]-pose.y
dt = fmod(self.state[2,0]-pose.theta,2*pi)
if dx*dx + dy*dy > 0.1 or abs(dt) > 0.3:
self.state = column_vector(pose.x, pose.y, pose.theta)
self.covariance = matrix(pose.cov).reshape(3, 3)
return
else:
pose.x += self.state[0,0]
pose.y += self.state[1,0]
while pose.theta > pi:
pose.theta -= tau
while pose.theta < -pi:
pose.theta += tau
pose.theta += self.state[2,0]
y = column_vector(pose.x, pose.y, pose.theta)
rospy.loginfo("Updating with position %s",y)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def LagkJointMomentsFromMRAP(H, K=0, L=1):
"""
Returns the lag-L joint moments of a marked rational
arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP to check
K : int, optional
The dimension of the matrix of joint moments to
compute. If K=0, the MxM joint moments will be
computed. The default value is 0
L : int, optional
The lag at which the joint moments are computed.
The default value is 1
prec : double, optional
Numerical precision to check if the input is valid.
The default value is 1e-14
Returns
-------
Nm : list/cell of matrices of shape(K+1,K+1), length(N)
The matrices containing the lag-L joint moments,
starting from moment 0.
"""
if butools.checkInput and not CheckMRAPRepresentation(H):
raise Exception(
"LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!"
)
if K == 0:
K = H[0].shape[0] - 1
M = len(H) - 1
H0 = H[0]
sumH = ml.zeros(H[0].shape)
for i in range(M):
sumH += H[i + 1]
H0i = la.inv(-H0)
P = H0i * sumH
pi = DRPSolve(P)
Pw = ml.eye(H0.shape[0])
H0p = [ml.matrix(Pw)]
for i in range(1, K + 1):
Pw *= i * H0i
H0p.append(ml.matrix(Pw))
Pl = la.matrix_power(P, L - 1)
Nm = []
for m in range(M):
Nmm = ml.zeros((K + 1, K + 1))
for i in range(K + 1):
for j in range(K + 1):
Nmm[i, j] = np.sum(pi * H0p[i] * H0i * H[m + 1] * Pl * H0p[j])
Nm.append(ml.matrix(Nmm))
return Nm
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
c = cos(joint_angle)
s = sin(joint_angle)
if (joint_name == 'LHipYawPitch' or joint_name == 'LHipYawPitch'):
T1 = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
T2 = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0],
[0, 0, 0, 1]])
T = dot(T2, T1)
#print("1")
elif ('Roll' in joint_name):
T = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0],
[0, 0, 0, 1]])
#print("2")
elif ('Pitch' in joint_name):
T = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0],
[0, 0, 0, 1]])
#print("3")
elif ('Yaw' in joint_name):
T = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
#print("4")
#print(T)
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = matrix((4, 4))
# YOUR CODE HERE
if joint_name.endswith("Pitch"):
#X
T = matrix([[1, 0, 0, 0],
[0,
math.cos(joint_angle), -math.sin(joint_angle), 0],
[0, math.sin(joint_angle),
math.cos(joint_angle), 0], [0, 0, 0, 1]])
elif joint_name.endswith("Yaw"):
#Y
T = matrix([[math.cos(joint_angle), 0,
math.sin(joint_angle), 0], [0, 1, 0, 0],
[-math.sin(joint_angle), 0,
math.cos(joint_angle), 0], [0, 0, 0, 1]])
elif joint_name.endswith("Roll"):
#Z
T = matrix([[math.cos(joint_angle), -math.sin(joint_angle), 0, 0],
[math.sin(joint_angle),
math.cos(joint_angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
else:
print "Error!!"
print joint_name
return T
def update_pos(self, pose, absolute=False):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# if we have absolute (i.e. QR-code) data, and it's far from our current
# estimate, then assume we're lost and reset the EKF.
if absolute:
# we can intelligently mod the incoming pose angle to be close
# to the current estimate - note that the ordered if statements
# mean that if we are super-lost, we will randomly add some pi
# but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2, 2] < 10:
while pose.theta - self.state[2, 0] > pi:
pose.theta -= tau
while self.state[2, 0] - pose.theta > pi:
pose.theta += tau
dx = self.state[0, 0] - pose.x
dy = self.state[1, 0] - pose.y
dt = fmod(self.state[2, 0] - pose.theta, 2 * pi)
if dx * dx + dy * dy > 0.1 or abs(dt) > 0.3:
self.state = column_vector(pose.x, pose.y, pose.theta)
self.covariance = matrix(pose.cov).reshape(3, 3)
return
else:
pose.x += self.state[0, 0]
pose.y += self.state[1, 0]
while pose.theta > pi:
pose.theta -= tau
while pose.theta < -pi:
pose.theta += tau
pose.theta += self.state[2, 0]
y = column_vector(pose.x, pose.y, pose.theta)
rospy.loginfo("Updating with position %s", y)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def local_trans(self, joint_name, joint_angle, translation):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = sin(joint_angle)
c = cos(joint_angle)
x, y, z = translation
if 'Yaw' in joint_name:
T = matrix([[c, -s, 0.0, x], [s, c, 0.0, y], [0.0, 0.0, 1.0, z],
[0.0, 0.0, 0.0, 1.0]])
if 'Roll' in joint_name:
T = matrix([[1.0, 0.0, 0.0, x], [0.0, c, -s, y], [0.0, s, c, z],
[0.0, 0.0, 0.0, 1.0]])
if 'Pitch' in joint_name:
T = matrix([[c, 0.0, s, x], [0.0, 1.0, 0.0, y], [-s, 0.0, c, z],
[0.0, 0.0, 0.0, 1.0]])
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = np.zeros([4, 4])
sin_vec = sin(joint_angle)
cos_vec = cos(joint_angle)
# Differ between the three possible movements angles (Roll, Pitch and Yaw)
if 'Roll' in joint_name:
T = matrix([[1, 0, 0, 0], [0, cos_vec, -sin_vec, 0],
[0, sin_vec, cos_vec, 0], [0, 0, 0, 1]])
if 'Pitch' in joint_name:
T = matrix([[cos_vec, 0, sin_vec, 0], [0, 1, 0, 0],
[-sin_vec, 0, cos_vec, 0], [0, 0, 0, 1]])
if 'Yaw' in joint_name:
T = matrix([[cos_vec, -sin_vec, 0, 0], [sin_vec, cos_vec, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
T[3, 0:3] = self.len_joint[joint_name]
return T
def test_mult_simple(self):
tempa = [[1,0,0],[0,1,0],[0,0,1]]
self.a = matlib.matrix(tempa)
self.b = matlib.matrix(tempa)
self.c = matlib.matrix(tempa)
self.d = multiplierMatrice(self.a, self.b)
self.assertTrue(numpy.array_equal(self.c, self.d))
def loglikelihood(self, obs):
# compute the log likelihood given a sequence of obs
t_mat = npmat.matrix(self.t_mat)
# use alpha
alpha = npmat.matrix((self.pi_vec * self.z_mat[obs[0], :])[:,np.newaxis])
# print('self.z_mat[obs[0], :]: '+ str(self.z_mat[obs[0], :]))
# print('self.pi_vec: '+ str(self.pi_vec))
# print('self.pi_vec * self.z_mat[obs[0], :]: '+ str(self.pi_vec * self.z_mat[obs[0], :]))
# print('alpha: '+ str(alpha))
log_coef = 0
for i in range(1,len(obs)):
alpha = npmat.matrix(((t_mat * alpha).getA()[:,0] * self.z_mat[obs[i], :])[:,np.newaxis])
asum = alpha.sum()
alpha /= asum
log_coef += np.log(asum)
alpha_ll = log_coef + np.log(alpha.sum())
# use beta
if False:
beta = npmat.ones((t_mat.shape[0], 1))
log_coef = 0
for i in range(len(obs)-2,-1,-1):
# print('self.z_mat[obs[i+1], :]: ' + str(self.z_mat[obs[i+1], :]))
# print('beta.getA()[0]: ' + str(beta.getA()[:,0]))
# print('self.z_mat[obs[i+1], :] * beta.getA()[:,0]: ' + str(self.z_mat[obs[i+1], :] * beta.getA()[:,0]))
# print('npmat.matrix(result): ' + str(npmat.matrix((self.z_mat[obs[i+1], :] * beta.getA()[:,0])[:,np.newaxis])))
# print('t_mat.getT(): ' + str(t_mat.getT()))
beta = t_mat.getT() * npmat.matrix((self.z_mat[obs[i+1], :] * beta.getA()[:,0])[:,np.newaxis])
bsum = beta.sum()
beta /= bsum
log_coef += np.log(bsum)
beta_ll = log_coef + np.log(np.sum(beta.getA()[0] * self.pi_vec * self.z_mat[obs[0],:]))
# print('alpha_ll: ' + str(alpha_ll))
# print('beta_ll: ' + str(beta_ll))
return alpha_ll
def test_euler(self):
"""Test axis-angle and euler representation conversions."""
q1 = Quaternion([0, 0, 0, 0], np.float64)
#q1.from_euler(0, 0, 0)
#print(q1)
#self.assertEqual(q1, Quaternion([1,0,0,0]))
for i in range(100):
(phi, theta, psi) = self.rand_euler()
q1.from_euler(phi, theta, psi)
q_phi = np.arctan2(q1[0] * q1[2] + q1[1] * q1[3],
-(q1[1] * q1[2] - q1[0] * q1[3]))
q_theta = np.arccos(-q1[0]**2 - q1[1]**2 + q1[2]**2 + q1[3]**2)
q_psi = np.arctan2(q1[0] * q1[2] - q1[1] * q1[3],
q1[1] * q1[2] + q1[0] * q1[3])
# Convert between different conventions
if q_phi < 0:
q_phi += 2 * np.pi
#if q_theta < 0:
#q_theta += 2 * np.pi
if q_psi < 0:
q_psi += 2 * np.pi
r1 = m.matrix([phi, theta, psi])
r1q = m.matrix([q_phi, q_theta, q_psi])
#print(r1- r1q)
self.assertTrue(np.linalg.norm(r1 - r1q) < 1e-10)
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
offsets = { # x,y,z
'HeadYaw': [0, 0, 0],
'HeadPitch': [0, 0, 126.5],
'LShoulderPitch': [0, 0, 0],
'LShoulderRoll': [105, 15, 0],
'LElbowYaw': [0, 0, 0],
'LElbowRoll': [55.95, 0, 0],
'ShoulderOffset': [0, 98, 100],
'LHipYawPitch': [0, 0, 0],
'LHipRoll': [0, 0, 0],
'LHipPitch': [0, 0, -100],
'LKneePitch': [0, 0, -102.9],
'LAnklePitch': [0, 0, 0],
'LAnkleRoll': [0, 0, 0],
'HipOffset': [0, 0, -85]
}
# 0 = x, 1 = y, 2 = z
if joint_name[0] == "R":
coords = offsets["L"+(joint_name[1:])]
coords[1] = -1 * coords[1]
# invert y for right leg
else:
coords = offsets[joint_name]
# movements: roll - x, pitch - y, yaw - z
s = numpy.sin(joint_angle)
c = numpy.cos(joint_angle)
Rx = matrix([[1, 0, 0, 0],
[0, c, -s, 0],
[0, s, c, 0],
[coords[0], coords[1], coords[2], 1]])
Ry = matrix([[c, 0, s, 0],
[0, 1, 0, 0],
[-s, 0, c, 0],
[coords[0], coords[1], coords[2], 1]])
Rz = matrix([[c, s, 0, 0],
[-s, c, 0, 0],
[0, 0, 1, 0],
[coords[0], coords[1], coords[2], 1]])
if joint_name.endswith("YawPitch"):
T = Ry / 2 + Rz / 2
elif joint_name.endswith("Roll"):
T = Rx
elif joint_name.endswith("Pitch"):
T = Ry
elif joint_name.endswith("Yaw"):
T = Rz
return T
def fetch_pids_ttol(pnts: MatrixVector, psys: PanelSystem, ztol: float=0.01, ttol: float=0.1):
shp = pnts.shape
pnts = pnts.reshape((-1, 1))
numpnt = pnts.shape[0]
numpnl = len(psys.pnls)
pidm = zeros((1, numpnl), dtype=int)
wintm = zeros((numpnt, numpnl), dtype=bool)
abszm = zeros((numpnt, numpnl), dtype=float)
for pnl in psys.pnls.values():
pidm[0, pnl.ind] = pnl.pid
wintm[:, pnl.ind], abszm[:, pnl.ind] = pnl.within_and_absz_ttol(pnts[:, 0], ttol=ttol)
abszm[wintm is False] = float('inf')
minm = argmin(abszm, axis=1)
minm = array(minm).flatten()
pidm = array(pidm).flatten()
pids = pidm[minm]
pids = matrix([pids], dtype=int).transpose()
indp = arange(numpnt)
minz = array(abszm[indp, minm]).flatten()
minz = matrix([minz], dtype=float).transpose()
chkz = minz < ztol
pids[logical_not(chkz)] = 0
pids = pids.reshape(shp)
chkz = chkz.reshape(shp)
return pids, chkz
def testzyz_fk_ik(self):
R = np.matrix([[1., -0., 0.], [0., 1., 0.], [-0., 0., 1.]])
self.assertTrue(np.allclose(zyz_euler_angle(0, 0, 0), R))
self.assertTrue(
np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
R = np.matrix([[0.70710678, -0.70710678, 0.],
[0.70710678, 0.70710678, 0.], [-0., 0., 1.]])
self.assertTrue(np.allclose(zyz_euler_angle(math.pi / 4, 0, 0), R))
self.assertTrue(
np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
R = np.matrix([[0.5, -0., 0.8660254], [0., 1., 0.],
[-0.8660254, 0., 0.5]])
self.assertTrue(np.allclose(zyz_euler_angle(0, math.pi / 3, 0), R))
self.assertTrue(
np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
R = np.matrix([[0.5, -0.5,
0.70710678], [0.70710678, 0.70710678, 0.],
[-0.5, 0.5, 0.70710678]])
self.assertTrue(
np.allclose(zyz_euler_angle(0, math.pi / 4, math.pi / 4), R))
self.assertTrue(
np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
R = np.matrix([[-0.55034481, -0.70029646, 0.45464871],
[0.70029646, -0.09064712, 0.70807342],
[-0.45464871, 0.70807342, 0.54030231]])
self.assertTrue(np.allclose(zyz_euler_angle(1, 1, 1), R))
self.assertTrue(
np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
def SamplesFromDPH(a, A, k):
"""
Generates random samples from a discrete phase-type
distribution.
Parameters
----------
alpha : matrix, shape (1,M)
The initial probability vector of the discrete phase-
type distribution.
A : matrix, shape (M,M)
The transition probability matrix of the discrete phase-
type distribution.
K : integer
The number of samples to generate.
prec : double, optional
Numerical precision to check if the input phase-type
distribution is valid. The default value is 1e-14.
Returns
-------
x : vector, length(K)
The vector of random samples
"""
if butools.checkInput and not CheckDPHRepresentation(a, A):
raise Exception(
"SamplesFromDPH: input is not a valid DPH representation!")
# auxilary variables
a = a.A.flatten()
N = len(a)
cummInitial = np.cumsum(a)
logp = np.log(np.diag(A))
sojourn = 1.0 / (1.0 - np.diag(A))
nextpr = ml.matrix(np.diag(sojourn)) * A
nextpr = nextpr - ml.matrix(np.diag(np.diag(nextpr)))
nextpr = np.hstack((nextpr, 1.0 - np.sum(nextpr, 1)))
nextpr = np.cumsum(nextpr, 1)
x = np.zeros(k)
for n in range(k):
time = 0
# draw initial distribution
r = rand()
state = 0
while cummInitial[state] <= r:
state += 1
# play state transitions
while state < N:
time += 1 + int(np.log(rand()) / logp[state])
r = rand()
nstate = 0
while nextpr[state, nstate] <= r:
nstate += 1
state = nstate
x[n] = time
return x
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
for chain_joints in self.chains.values():
T = identity(4)
for joint in chain_joints:
T1 = identity(4)
if joint == "HeadYaw":
T1 = matrix([[1, 0.0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0.1265], [0, 0, 0, 1]])
elif joint == "RShoulderPitch":
T1 = matrix([[1, 0, 0, 0], [0, 1, 0, -0.098],
[0, 0, 1, 0.100], [0, 0, 0, 1]])
elif joint == "LShoulderPitch":
T1 = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0.098],
[0, 0, 1, 0.100], [0, 0, 0, 1]])
elif joint == "LHipYawPitch":
T1 = matrix([[1, 0, 0, 0], [0, 1, 0, 0.050],
[0, 0, 1, -0.085], [0, 0, 0, 1]])
elif joint == "RHipYawPitch":
T1 = matrix([[1, 0, 0, 0], [0, 1, 0, -0.050],
[0, 0, 1, -0.085], [0, 0, 0, 1]])
angle = joints[joint]
Tl = self.local_trans(joint, angle)
T = T1 * T * Tl
self.transforms[joint] = T
"""
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = math.sin(joint_angle)
c = math.cos(joint_angle)
xo, yo, zo = self.jointoffs[joint_name]
#x
if (joint_name.find('Roll') > 0):
T = matrix([[1, 0, 0, xo], [0, c, -s, yo], [0, s, c, zo],
[0, 0, 0, 1]])
#y
elif (joint_name.find('Pitch') > 0):
T = matrix([[c, 0, s, xo], [0, 1, 0, yo], [-s, 0, c, zo],
[0, 0, 0, 1]])
#z
elif (joint_name.find('Yaw') > 0):
T = matrix([[c, -s, 0, xo], [s, c, 0, yo], [0, 0, 1, zo],
[0, 0, 0, 1]])
else:
print("Invalid joint_name")
return T
def optimize(bd_con, init_val=None):
# bd_con: boundary conditions - (k0, x1, y1, theta1, k1)
# init_val: (p1, p2, sg)
if init_val is None or init_val[2] < 0:
init_val = ((2 * bd_con[0] + bd_con[4]) / 3,
(bd_con[0] + 2 * bd_con[4]) / 3,
np.sqrt(bd_con[1]**2 + bd_con[2]**2) +
5 * min(np.abs(bd_con[3]), np.abs(2 * np.pi - bd_con[3])))
# revise the parameters
init_val = list(init_val)
init_val[0] = max(min(init_val[0], 0.2), -0.2)
init_val[1] = max(min(init_val[1], 0.2), -0.2)
init_val[2] = max(min(init_val[2], 1000.), 1.)
# print('IterTimes: {0},P = {1}'.format(0, (init_val[0], init_val[1], init_val[2])))
q_g = matlib.matrix([[bd_con[1]], [bd_con[2]], [bd_con[3]]])
p = (bd_con[0], init_val[0], init_val[1], bd_con[4], init_val[2])
r = (__a(p), __b(p), __c(p), __d(p))
pp = matlib.matrix([[init_val[0]], [init_val[1]], [init_val[2]]])
dq = matlib.matrix([[1.], [1.], [1.]])
eps1, eps2 = 1.e-4, 1.e-6
times = 0
while (np.abs(dq[0, 0]) > eps1 or np.abs(dq[1, 0]) > eps1
or np.abs(dq[2, 0]) > eps2) and times <= 100:
times += 1
J = __Jacobian(p, r)
# print('J={0}'.format(J))
theta_p = __theta(p[4], r)
x_p, y_p = __xy_calc(p[4], r)
# print('q = {0}'.format((x_p,y_p,theta_p)))
dq = q_g - matlib.matrix([[x_p], [y_p], [theta_p]])
pp += J**-1 * dq
# revise the parameters
pp[0, 0] = max(min(pp[0, 0], 0.2), -0.2)
pp[1, 0] = max(min(pp[1, 0], 0.2), -0.2)
pp[2, 0] = max(min(pp[2, 0], 1000.), 1.)
#
# if pp[0,0] > 0.2:
# pp[0,0] = 0.2
# elif pp[0,0] < -0.2:
# pp[0,0] = -0.2
# if pp[1,0] > 0.2:
# pp[1,0] = 0.2
# elif pp[1,0] < -0.2:
# pp[1,0] = -0.2
# if pp[2,0] < 1.:
# pp[2,0] = 1.
# elif pp[2,0] > 1000.:
# pp[2,0] = 1000.
p = (bd_con[0], pp[0, 0], pp[1, 0], bd_con[4], pp[2, 0])
# print('p={0}'.format(p))
r = (__a(p), __b(p), __c(p), __d(p))
# print('r={0}'.format(r))
# print('IterTimes: {0},P = {1}'.format(times, (p[1],p[2],p[4])))
# print('IterTimes: {0}'.format(times))
if times > 100:
# pp = matlib.matrix([[-1.],[-1.],[-1.]])
return None
else:
return pp[0, 0], pp[1, 0], pp[2, 0]
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
#get sin and cos of joint angle
sin = math.sin(joint_angle)
cos = math.cos(joint_angle)
#get link values for joint
joint_links = (0.0, 0.0, 0.0)
if joint_name in self.links:
joint_links = self.links[joint_name]
#Check if X,Y or Z and set T according to 3D forward kinematics
if (joint_name.endswith("Roll")):
T = matrix([[1, 0, 0, 0], [0, cos, -sin, 0], [0, sin, cos, 0],
[joint_links[0], joint_links[1], joint_links[2], 1]])
if (joint_name.endswith("Pitch")):
T = matrix([[cos, 0, sin, 0], [0, 1, 0, 0], [-sin, 0, cos, 0],
[joint_links[0], joint_links[1], joint_links[2], 1]])
if (joint_name.endswith("Yaw")):
T = matrix([[cos, sin, 0, 0], [-sin, cos, 0, 0], [0, 0, 1, 0],
[joint_links[0], joint_links[1], joint_links[2], 1]])
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = math.sin(joint_angle)
c = math.cos(joint_angle)
if joint_name.endswith("Roll"):
#X
T = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0],
[0, 0, 0, 1]])
elif joint_name.endswith("Pitch"):
#Y
T = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0],
[0, 0, 0, 1]])
elif joint_name.endswith("Yaw"):
#Z
T = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
else:
print "Error!!"
print joint_name
T[0, 3] = self.jointLengths[joint_name][0]
T[1, 3] = self.jointLengths[joint_name][1]
T[2, 3] = self.jointLengths[joint_name][2]
return T
def test_euler(self): """Test axis-angle and euler representation conversions.""" q1 = Quaternion([0,0,0,0], np.float64) #q1.from_euler(0, 0, 0) #print(q1) #self.assertEqual(q1, Quaternion([1,0,0,0])) for i in range(100): (phi, theta, psi) = self.rand_euler() q1.from_euler(phi, theta, psi) q_phi = np.arctan2(q1[0]*q1[2] + q1[1]*q1[3],-(q1[1]*q1[2]-q1[0]*q1[3])) q_theta = np.arccos(-q1[0]**2 -q1[1]**2 + q1[2]**2 + q1[3]**2) q_psi = np.arctan2(q1[0]*q1[2] - q1[1]*q1[3], q1[1]*q1[2] + q1[0]*q1[3]) # Convert between different conventions if q_phi < 0: q_phi += 2 * np.pi #if q_theta < 0: #q_theta += 2 * np.pi if q_psi < 0: q_psi += 2 * np.pi r1 = m.matrix([phi, theta, psi]) r1q = m.matrix([q_phi, q_theta, q_psi]) #print(r1- r1q) self.assertTrue(np.linalg.norm(r1 - r1q) < 1e-10)
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
a = math.sin(joint_angle)
b = math.cos(joint_angle)
if joint_name.endswith("Roll"):
T = np.dot(
T,
matrix([[1, 0, 0, 0], [0, b, -a, 0], [0, a, b, 0],
[0, 0, 0, 1]]))
elif joint_name.endswith("Pitch"):
T = np.dot(
T,
matrix([[b, 0, a, 0], [0, 1, 0, 0], [-a, 0, b, 0],
[0, 0, 0, 1]]))
elif joint_name.endswith("Yaw"):
T = np.dot(
T,
matrix([[b, a, 0, 0], [-a, b, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
T[0, 3] = self.jointLength[joint_name][0]
T[1, 3] = self.jointLength[joint_name][1]
T[2, 3] = self.jointLength[joint_name][2]
return T
def SamplesFromDPH (a,A,k):
"""
Generates random samples from a discrete phase-type
distribution.
Parameters
----------
alpha : matrix, shape (1,M)
The initial probability vector of the discrete phase-
type distribution.
A : matrix, shape (M,M)
The transition probability matrix of the discrete phase-
type distribution.
K : integer
The number of samples to generate.
prec : double, optional
Numerical precision to check if the input phase-type
distribution is valid. The default value is 1e-14.
Returns
-------
x : vector, length(K)
The vector of random samples
"""
if butools.checkInput and not CheckDPHRepresentation(a,A):
raise Exception("SamplesFromDPH: input is not a valid DPH representation!")
# auxilary variables
a = a.A.flatten()
N = len(a)
cummInitial = np.cumsum(a)
logp = np.log(np.diag(A));
sojourn = 1.0/(1.0-np.diag(A))
nextpr = ml.matrix(np.diag(sojourn))*A
nextpr = nextpr - ml.matrix(np.diag(np.diag(nextpr)))
nextpr = np.hstack((nextpr, 1.0-np.sum(nextpr,1)))
nextpr = np.cumsum(nextpr,1)
x = np.zeros(k)
for n in range(k):
time = 0
# draw initial distribution
r = rand()
state = 0
while cummInitial[state]<=r:
state+=1
# play state transitions
while state<N:
time += 1 + int(np.log(rand()) / logp[state])
r = rand()
nstate = 0
while nextpr[state,nstate]<=r:
nstate += 1
state = nstate
x[n] = time
return x
def em_step(num_states, num_obs, pi_vec, z_mat, t_mat, obs_all):
# given initial parameters, run em from the obs
# obs are 2d with each row being a trajectory
# returns estimated observation and transition matrices
num_steps = obs_all.shape[1]
t_mat = npmat.matrix(t_mat)
gamma_sum = np.zeros((num_states,))
xi_sum = np.zeros((num_states, num_states))
obs_sum = np.zeros((num_obs, num_states))
for t in range(obs_all.shape[0]):
obs = obs_all[t, :]
# compute alpha
alpha = npmat.zeros((num_states, num_steps))
alpha[:, 0] = npmat.matrix((pi_vec * z_mat[obs[0], :])[:, np.newaxis])
for i in range(1, len(obs)):
alpha[:, i] = npmat.matrix(((t_mat * alpha[:, i - 1]).getA()[:, 0] * z_mat[obs[i], :])[:, np.newaxis])
asum = alpha[:, i].sum()
alpha[:, i] /= asum
# print('alpha\n' + str(alpha))
# compute beta
beta = npmat.zeros((num_states, num_steps))
beta[:, num_steps - 1] = npmat.ones((num_states, 1))
for i in range(len(obs) - 2, -1, -1):
beta[:, i] = t_mat.getT() * npmat.matrix(
(z_mat[obs[i + 1], :] * beta[:, i + 1].getA()[:, 0])[:, np.newaxis])
bsum = beta[:, i].sum()
beta[:, i] /= bsum
# print(beta)
# compute gamma
gamma = alpha.getA() * beta.getA()
gamma = gamma / (gamma.sum(axis=0)[np.newaxis, :])
# print(gamma)
# compute xi (transposed from the paper)
xi = np.zeros((num_states, num_states, num_steps - 1))
for i in range(num_steps - 1):
xi[:, :, i] = np.transpose(alpha[:, i].getA()) * t_mat.getA() * (z_mat[obs[i + 1], :] *
beta[:, i + 1].getA()[0])[:, np.newaxis]
xsum = xi[:, :, i].sum()
xi[:, :, i] /= xsum
# print('xi[:,:,i]' + str(xi[:,:,i]))
# print('np.transpose(alpha[:,i].getA()): ' + str(np.transpose(alpha[:,i].getA())))
# print('t_mat: ' + str(t_mat))
# print('result of *: ' + str(np.transpose(alpha[:,i].getA()) * t_mat.getA()))
# add to the sums
gamma_sum += gamma.sum(axis=1)
xi_sum += xi.sum(axis=2)
for z in range(num_obs):
obs_sum[z, :] += gamma[:, obs == z].sum(axis=1)
# print('gamma_sum\n' + str(gamma_sum))
# print('xi_sum\n' + str(xi_sum))
# print('obs_sum\n' + str(obs_sum))
# finally compute estimates
est_z_mat = obs_sum / obs_sum.sum(axis=0)[np.newaxis, :]
est_t_mat = xi_sum / xi_sum.sum(axis=0)[np.newaxis, :]
return est_z_mat, est_t_mat;
def test_state_values():
# StateValues tests
sv = StateValues(X)
assert (sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5)
assert (sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5)
assert (sv[nm.matrix('1 1 1; 0 0 0; 0 0 0')] == 1)
assert (sv[nm.matrix('0 1 0; 0 1 0; 0 1 0')] == 1)
assert (sv[nm.matrix('0 0 2; 0 2 0; 2 0 0')] == 0)
def test_mult_dimensions(self):
tempa = [[1,1],[2,2],[3,3]]
tempb = [[3,3,3],[2,2,2]]
tempc = [[5,5,5],[10,10,10],[15, 15, 15]]
self.a = matlib.matrix(tempa)
self.b = matlib.matrix(tempb)
self.c = matlib.matrix(tempc)
self.assertTrue(numpy.array_equal(multiplierMatrice(self.a, self.b), self.c))
def test_state_values():
# StateValues tests
sv = StateValues(X)
assert(sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5)
assert(sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5)
assert(sv[nm.matrix('1 1 1; 0 0 0; 0 0 0')] == 1)
assert(sv[nm.matrix('0 1 0; 0 1 0; 0 1 0')] == 1)
assert(sv[nm.matrix('0 0 2; 0 2 0; 2 0 0')] == 0)
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
# YOUR CODE HERE
s = sin(joint_angle)
c = cos(joint_angle)
#set rotation
#Z-axis
if (joint_name[-3:] == 'Yaw'):
T = matrix([[c, -s, 0, 0],
[s, c, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
#X-axis
elif (joint_name[-4:] == 'Roll'):
T = matrix([[1, 0 , 0, 0],
[0, c, -s, 0],
[0, s, c, 0],
[0, 0, 0, 1]])
#Y-axis
elif (joint_name[-5:] == 'Pitch'):
T = matrix([[c, 0 , -s, 0],
[0, 1, 0, 0],
[s, 0, c, 0],
[0, 0, 0, 1]])
#set X,Y,Z for joints that have a different coordinate difference to the joint before than 0,0,0
if (joint_name[0] == 'L' or joint_name[0] == 'R'):
if (joint_name == 'LElbowYaw' or joint_name == 'RElbowYaw'):
T[:3, -1] = self.lengths['ArmShoulderRollElbowYaw'].T
elif (joint_name == 'LWristYaw' or joint_name == 'RWristYaw'):
T[:3, -1] = self.lengths['ArmElbowRollWristYaw'].T
elif (joint_name == 'LKneePitch' or joint_name == 'RKneePitch'):
T[:3, -1] = self.lengths['LegHipPitchKneePitch'].T
elif (joint_name == 'LAnklePitch' or joint_name == 'RAnklePitch'):
T[:3, -1] = self.lengths['LegKneePitchAnklePitch'].T
#for transformation from torso to hip (left leg)
elif (joint_name == 'LRoll'):
T[:3, -1] = self.lengths['LLegTorsoHip'].T
#for transformation from torso to hip (right leg)
elif (joint_name == 'RRoll'):
T[:3, -1] = self.lengths['RLegTorsoHip'].T
#for transformation from torso to shoulder (left arm)
elif (joint_name == 'LYaw'):
T[:3, -1] = self.lengths['LArmTorsoShoulder'].T
#for transformation from torso to shoulder (right arm)
elif (joint_name == 'RYaw'):
T[:3, -1] = self.lengths['RArmTorsoShoulder'].T
return T
def LagkJointMomentsFromMRAP (H, K=0, L=1):
"""
Returns the lag-L joint moments of a marked rational
arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP to check
K : int, optional
The dimension of the matrix of joint moments to
compute. If K=0, the MxM joint moments will be
computed. The default value is 0
L : int, optional
The lag at which the joint moments are computed.
The default value is 1
prec : double, optional
Numerical precision to check if the input is valid.
The default value is 1e-14
Returns
-------
Nm : list/cell of matrices of shape(K+1,K+1), length(N)
The matrices containing the lag-L joint moments,
starting from moment 0.
"""
if butools.checkInput and not CheckMRAPRepresentation (H):
raise Exception("LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!")
if K==0:
K = H[0].shape[0]-1
M = len(H)-1
H0 = H[0]
sumH = ml.zeros(H[0].shape)
for i in range(M):
sumH += H[i+1]
H0i = la.inv(-H0)
P = H0i*sumH
pi = DRPSolve(P)
Pw = ml.eye(H0.shape[0])
H0p = [ml.matrix(Pw)]
for i in range(1,K+1):
Pw *= i*H0i
H0p.append(ml.matrix(Pw))
Pl = la.matrix_power (P, L-1)
Nm = []
for m in range(M):
Nmm = ml.zeros ((K+1,K+1))
for i in range(K+1):
for j in range(K+1):
Nmm[i,j] = np.sum (pi * H0p[i] * H0i * H[m+1] * Pl * H0p[j])
Nm.append(ml.matrix(Nmm))
return Nm
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
if joint_name == 'LHipYawPitch':
Rl = matrix(
[[1, 0, 0],
[0, math.cos(math.pi / 4), -1 * math.sin(math.pi / 4)],
[0, math.sin(math.pi / 4),
math.cos(math.pi / 4)]])
T[0:3, 0:3] = Rl
elif joint_angle == 'RHipYawPitch':
Rr = matrix(
[[1, 0, 0],
[
0,
math.cos(-1 * math.pi / 4),
-1 * math.sin(-1 * math.pi / 4)
],
[0, math.sin(-1 * math.pi / 4),
math.cos(-1 * math.pi / 4)]])
T[0:3, 0:3] = Rr
else:
if joint_name.endswith("Roll"):
Rx = matrix(
[[1, 0, 0],
[0, math.cos(joint_angle), -1 * math.sin(joint_angle)],
[0, math.sin(joint_angle),
math.cos(joint_angle)]])
T[0:3, 0:3] = Rx
elif joint_name.endswith("Pitch"):
Ry = matrix(
[[math.cos(joint_angle), 0,
math.sin(joint_angle)], [0, 1, 0],
[-1 * math.sin(joint_angle), 0,
math.cos(joint_angle)]])
T[0:3, 0:3] = Ry
elif joint_name.endswith("Pitch"):
Rz = matrix(
[[math.cos(joint_angle), -1 * math.sin(joint_angle), 0],
[math.sin(joint_angle),
math.cos(joint_angle), 0], [0, 0, 1]])
T[0:3, 0:3] = Rz
for key in self.chains.keys():
if joint_name in self.chains[key]:
T[0, 3] = self.l[key][self.chains[key].index(joint_name)][0]
T[1, 3] = self.l[key][self.chains[key].index(joint_name)][1]
T[2, 3] = self.l[key][self.chains[key].index(joint_name)][2]
return T
def plot_belief_trajectory_cpp(B, U, model, start, goal, T):
bDim = model.bDim
uDim = model.uDim
B = np.matrix(B)
B = np.matrix(B.reshape(bDim, T))
model.T = T
model.setStartState(ml.matrix(start).T)
model.setGoalState(ml.matrix(goal).T)
plot_belief_trajectory(B, U, model)
def plot_belief_trajectory_cpp(B, U, model, start, goal, T):
bDim = model.bDim
uDim = model.uDim
B = np.matrix(B)
B = np.matrix(B.reshape(bDim, T))
model.T = T
model.setStartState(ml.matrix(start).T)
model.setGoalState(ml.matrix(goal).T)
plot_belief_trajectory(B, U, model)
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = math.sin(joint_angle)
c = math.sin(joint_angle)
# Roll = X Pitch = Y Yaw = Z
if joint_name.endswith("Roll"):
T = matrix([
[1, 0, 0, self.links[joint_name][0]],
[0, c, -s, self.links[joint_name][1]],
[0, s, c, self.links[joint_name][2]],
[0, 0, 0, 1],
])
if joint_name.endswith("Pitch"):
T = matrix([
[c, 0, s, self.links[joint_name][0]],
[0, 1, 0, self.links[joint_name][1]],
[-s, 0, c, self.links[joint_name][2]],
[0, 0, 0, 1],
])
if joint_name.endswith("Yaw"):
T = matrix([
[c, s, 0, self.links[joint_name][0]],
[-s, c, 0, self.links[joint_name][1]],
[0, 0, 1, self.links[joint_name][2]],
[0, 0, 0, 1],
])
if joint_name.endswith("YawPitch"):
Y = matrix([
[c, s, 0, self.links[joint_name][0]],
[-s, c, 0, self.links[joint_name][1]],
[0, 0, 1, self.links[joint_name][2]],
[0, 0, 0, 1],
])
P = matrix([
[c, 0, s, self.links[joint_name][0]],
[0, 1, 0, self.links[joint_name][1]],
[-s, 0, c, self.links[joint_name][2]],
[0, 0, 0, 1],
])
T = np.dot(Y, P)
return T
def QBDSolve (B, L, F, L0, prec=1e-14):
"""
Returns the parameters of the matrix-geometrically
distributed stationary distribution of a QBD.
Using vector pi0 and matrix R provided by this function
the stationary solution can be obtained by
.. math::
\pi_k=\pi_0 R^k.
Parameters
----------
B : matrix, shape (N,N)
The matrix corresponding to backward transitions
L : matrix, shape (N,N)
The matrix corresponding to local transitions
F : matrix, shape (N,N)
The matrix corresponding to forward transitions
L0 : matrix, shape (N,N)
The matrix corresponding to local transitions at
level zero
precision : double, optional
The fundamental matrix R is computed up to this
precision. The default value is 1e-14
Returns
-------
pi0 : matrix, shape (1,N)
The stationary probability vector of level zero
R : matrix, shape (N,N)
The matrix parameter of the matrix geometrical
distribution of the QBD
"""
m = L0.shape[0]
I = ml.eye(m)
R = QBDFundamentalMatrices (B, L, F, "R", prec)
# Convert to discrete time problem, if needed
if np.sum(np.diag(L0) < 0): # continues time
lamb = float(np.max(-np.diag(L0)))
B = ml.matrix(B) / lamb
L0 = ml.matrix(L0) / lamb + I
pi0 = DTMCSolve(L0+R*B)
nr = np.sum(pi0*la.inv(I-R))
pi0 /= nr
return pi0, R
def test_board_states():
# board state tests
b = Board()
pX = Player(X,b)
pO = Player(O,b)
b.state = nm.matrix("1 0 0; 1 1 1; 1 0 0")
assert(are_winning_rows(b.state, X) and are_winning_cols(b.state, X))
assert(is_winner(b.state, pX.side))
b.state = nm.matrix("2 2 2; 0 0 2; 1 0 2")
assert(are_winning_rows(b.state, O) and are_winning_cols(b.state, O))
assert(is_winner(b.state, pO.side))
b.state = nm.matrix("1 0 0; 0 1 0; 0 0 1")
assert(are_winning_diags(b.state, X))
assert(is_winner(b.state, pX.side))
b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0")
assert(are_winning_diags(b.state, X))
assert(is_winner(b.state, pX.side))
b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0")
assert(are_winning_diags(b.state, X))
assert(is_winner(b.state, pX.side))
b.state = nm.matrix("1 1 2; 2 2 1; 1 1 2")
assert(not are_winning_diags(b.state, X) and
not are_winning_rows(b.state, O) and
not are_winning_cols(b.state, O))
assert(not is_winner(b.state, pX.side))
assert(not is_winner(b.state, pO.side))
assert(is_draw(b.state))
b.state = nm.matrix("1 1 1; 2 2 1; 2 2 1")
assert(is_winner(b.state, pX.side))
assert(not is_winner(b.state, pO.side))
b.state = nm.matrix("1 2 1; 2 2 1; 2 2 1")
assert(is_winner(b.state, pX.side))
assert(is_winner(b.state, pO.side))
b.state = nm.matrix("1 1 2; 2 2 1; 1 2 1")
assert(not is_winner(b.state, pX.side))
assert(not is_winner(b.state, pO.side))
assert(is_draw(b.state))
b.state = nm.matrix("2 1 1; 1 2 2; 1 2 1")
assert(not is_winner(b.state, pX.side))
assert(not is_winner(b.state, pO.side))
assert(is_draw(b.state))
def test_board_states():
# board state tests
b = Board()
pX = Player(X, b)
pO = Player(O, b)
b.state = nm.matrix("1 0 0; 1 1 1; 1 0 0")
assert (are_winning_rows(b.state, X) and are_winning_cols(b.state, X))
assert (is_winner(b.state, pX.side))
b.state = nm.matrix("2 2 2; 0 0 2; 1 0 2")
assert (are_winning_rows(b.state, O) and are_winning_cols(b.state, O))
assert (is_winner(b.state, pO.side))
b.state = nm.matrix("1 0 0; 0 1 0; 0 0 1")
assert (are_winning_diags(b.state, X))
assert (is_winner(b.state, pX.side))
b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0")
assert (are_winning_diags(b.state, X))
assert (is_winner(b.state, pX.side))
b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0")
assert (are_winning_diags(b.state, X))
assert (is_winner(b.state, pX.side))
b.state = nm.matrix("1 1 2; 2 2 1; 1 1 2")
assert (not are_winning_diags(b.state, X)
and not are_winning_rows(b.state, O)
and not are_winning_cols(b.state, O))
assert (not is_winner(b.state, pX.side))
assert (not is_winner(b.state, pO.side))
assert (is_draw(b.state))
b.state = nm.matrix("1 1 1; 2 2 1; 2 2 1")
assert (is_winner(b.state, pX.side))
assert (not is_winner(b.state, pO.side))
b.state = nm.matrix("1 2 1; 2 2 1; 2 2 1")
assert (is_winner(b.state, pX.side))
assert (is_winner(b.state, pO.side))
b.state = nm.matrix("1 1 2; 2 2 1; 1 2 1")
assert (not is_winner(b.state, pX.side))
assert (not is_winner(b.state, pO.side))
assert (is_draw(b.state))
b.state = nm.matrix("2 1 1; 1 2 2; 1 2 1")
assert (not is_winner(b.state, pX.side))
assert (not is_winner(b.state, pO.side))
assert (is_draw(b.state))
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(ForwardKinematicsAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode)
self.transforms = {n: identity(4) for n in self.joint_names}
# chains defines the name of chain and joints of the chain
self.chains = {'Head': ['HeadYaw', 'HeadPitch'],
'LArm': ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'],
'LLeg': ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'],
'RLeg': ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'],
'RArm': ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll']
}
#lengths bodypart from.. to.. [X, Y, Z]
self.lengths = {'ArmShoulderRollElbowYaw': matrix([105.0, 15.0, 0.0]),
'ArmElbowRollWristYaw': matrix([55.95, 0.0, 0.0]),
'LegHipPitchKneePitch': matrix([0.0, 0.0, -100.0]),
'LegKneePitchAnklePitch': matrix([0.0, 0.0, -102.9]),
'LLegTorsoHip': matrix([0.0, 50.0, -85.0]),
'RLegTorsoHip': matrix([0.0, -50.0, -85.0]),
'LArmTorsoShoulder': matrix([0.0, 98.0, 100.0]),
'RArmTorsoShoulder': matrix([0.0, -98.0, 100.0])
}
self.start_time =self.perception.time
def rotation_matrix(self, axis, angle):
matrix_dict = {
'x':
matrix([[1, 0, 0, 0], [0, np.cos(angle), -np.sin(angle), 0],
[0, np.sin(angle), np.cos(angle), 0], [0, 0, 0, 1]]),
'y':
matrix([[np.cos(angle), 0, np.sin(angle), 0], [0, 1, 0, 0],
[-np.sin(angle), 0, np.cos(angle), 0], [0, 0, 0, 1]]),
'z':
matrix([[np.cos(angle), -np.sin(angle), 0, 0],
[np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
}
return matrix_dict.get(axis)
def shortest(B) :
(j,k) = B.shape
if j==1 :
return B
b = LLL(B).getBasis() # representasi basis dalam array 2d
while True :
# aproximasi reduced basis
b_ = orthoproject(b[0], b[1:])
# rekursif
b__ = shortest(b_)
# lifting dg transformasi linear, catatan 13/12
T = solve_linear( b_.T,b__ ) # b_ perlu ditransform, liat spek
# transpose T karena msh kolom
v = matrix( T.T )*matrix( b[1:] ) # ubah ke matrix dulu
b[1:] = np.asarray(v) # override array lagi
b0n = norm(b[0])
b1n = norm(b[1])
if b1n**2 >= (3.0/4)*b0n**2 :
break
# swap
t = np.copy(b[0])
b[0] = b[1]
b[1] = t
# temukan nilai batas j0
g = Gram(b)
# cari vektor yg norm-nya lebih besar dari b0
i = 1
while i < j :
v = g.getBstar(i)
if norm(v) >= b0n :
break
i += 1
# enumerate
v1 = enumerate(b[:i], g)
# select-basis
B = select_basis( np.append([v1], b, axis=0) )
# ulangi lagi step spt yg didalam loop
B_ = orthoproject( B[0], B[1:] )
B__ = shortest(B_) # rekursif
T = solve_linear( B_.T, B__ )
# tangani kondisi ketika pengenolan
# apakah dalam enumerate alpha diubah2 secara share
V = matrix( T.T )*matrix( B[1:] ) # perlu bentuk matrix
B[1:] = np.asarray(V)
return B
def SimilarityMatrix (A1, A2):
"""
Returns the matrix that transforms A1 to A2.
Parameters
----------
A1 : matrix, shape (N,N)
The smaller matrix
A2 : matrix, shape (M,M)
The larger matrix (M>=N)
Returns
-------
B : matrix, shape (N,M)
The matrix satisfying `A_1\,B = B\,A_2`
Notes
-----
For the existence of a (unique) solution the larger
matrix has to inherit the eigenvalues of the smaller one.
"""
if A1.shape[0]!=A1.shape[1] or A2.shape[0]!=A2.shape[1]:
raise Exception("SimilarityMatrix: The input matrices must be square!")
N1 = A1.shape[0]
N2 = A2.shape[1]
if N1>N2:
raise Exception("SimilarityMatrix: The first input matrix must be smaller than the second one!")
[R1,Q1]=la.schur(A1,'complex')
[R2,Q2]=la.schur(A2,'complex')
Q1 = ml.matrix(Q1)
Q2 = ml.matrix(Q2)
c1 = ml.matrix(np.sum(Q2.H,1))
c2 = np.sum(Q1.H,1)
I = ml.eye(N2)
X = ml.zeros((N1,N2), dtype=complex)
for k in range(N1-1,-1,-1):
M = R1[k,k]*I-R2
if k==N1:
m = ml.zeros((1,N2))
else:
m = -R1[k,k+1:]*X[k+1:,:]
X[k,:] = Linsolve(np.hstack((M,c1)),np.hstack((m,c2[k])))
return (Q1*X*ml.matrix(Q2).H ).real
def predict(self, odom_pose): delta = self.get_odom_delta(odom_pose) if delta is None: # Can't do a delta update the first time. return dx, dy, dt = self.state_tuple(delta) # Give bonus change in angel since odom cannot # read that properly if abs(dt) > 0: if rospy.gettime() - self.lastdt > 0.25: bonus = copysign(ACCEL_ANGEL_BONUS, dt) delta[2] += bonus self.lastdt = rospy.get_time # PREDICT NEW STATE (X, Y, THETA) self.state = self.state + delta dist = sqrt(dx*dx + dy*dy) fwderr = 0.05 * dist sideerr = 0.1 * dist yawerr = 0.2 * dist cth = cos(odom_pose.theta) sth = sin(odom_pose.theta) V = matrix([ [cth*cth*fwderr + sth*sth*sideerr, sth*cth*(sideerr - fwderr) , 0.0 ], [sth*cth*(sideerr - fwderr) , sth*sth*fwderr + cth*cth*sideerr, 0.0 ], [0.0 , 0.0 , yawerr]]) # PREDICT COVARIANCE self.covariance = self.covariance + V
def Translation(x, y, z): Tl = identity(4) Tl = matrix([[1,0,0,x], [0,1,0,y], [0,0,1,z], [0,0,0,1]]) return Tl
def lireFichierMatrice(fichier):
with open(fichier, "r") as fichier:
noMatrice = int(fichier.readline())
dimensions = map(int,
filter(lambda x: x != "",
map(lambda x: x.strip(),
fichier.readline().strip().split(" "))))
assert (len(dimensions) == noMatrice+1),"Erreur de dimensions dans le fichier"
matrices = [None]
for i in range(0, len(dimensions)-1):
matrice = []
# On remplit la matrice
for j in range(0, dimensions[i]):
temp = map(int,
filter(lambda x: x != "",
map(lambda x: x.strip(),
fichier.readline().strip().split(" ")
)
)
)
assert(len(temp) == dimensions[i+1]),"Erreur dans le fichier source"
matrice.append(temp)
matrices.append(matlib.matrix(matrice))
return noMatrice, dimensions, matrices
def CRPSolve (Q):
"""
Computes the stationary solution of a continuous time
rational process (CRP).
Parameters
----------
Q : matrix, shape (M,M)
The generator matrix of the rational process
Returns
-------
pi : row vector, shape (1,M)
The vector that satisfies
`\pi\, Q = 0, \sum_i \pi_i=1`
Notes
-----
Continuous time rational processes are like continuous
time Markov chains, but the generator does not have to
pass the :func:`CheckGenerator` test (but the rowsums
still have to be zeros).
"""
if butools.checkInput and np.any(np.sum(Q,1)>butools.checkPrecision):
raise Exception("CRPSolve: The matrix has a rowsum which isn't zero!")
M = np.array(Q)
M[:,0] = np.ones(M.shape[0])
m = np.zeros(M.shape[0])
m[0] = 1.0
return ml.matrix(la.solve (M.T, m))
def Linsolve(A,b):
"""
Solves the linear system A*x=b (if b is a column vector), or x*A=b (if b is
a row vector).
Matrix "A" does not need to be square, this function uses rank-revealing
QR decomposition to solve the system.
Parameters
----------
A : matrix, shape (M,N)
The coefficient matrix of the linear system.
b : matrix, shape (M,1) or (1,N)
The right hand side of the linear system
Returns
-------
x : matrix, shape (M,1) or (1,N)
If b is a column vector, then x is the solution of A*x=b.
If b is a row vector, it returns the solution of x*A=b.
"""
if b.shape[0]==1:
x = Linsolve(np.conj(A.T), np.conj(b.T))
return np.conj(x.T)
elif b.shape[1]==1:
Q,R = la.qr(A)
N = A.shape[1]
return ml.matrix(la.solve(R[0:N,0:N], np.array(np.conj(Q.T)*b).flatten()[0:N])).T
def potential_fields_test():
T_f = 150
T_s = .01
t = np.arange(0, T_f, T_s)
c = controller_1.Controller(0, T_s)
c.q = np.matrix('150; 0')
p = plant.Plant(0,
npm.matrix([[0, 60, 0, 1, 0, 0]]).T,
np.matrix([[0, 0]]).T,
T_s,
)
x = np.zeros((6, t.size))
x_ref = np.zeros((2, t.size))
for i, time in enumerate(t):
x[:, i] = np.reshape(p.x.T, 6)
x_ref[:, i] = np.reshape(c.ref.T, 2)
p.iterate_state()
c.y = p.y
c.iterate_state()
p.u = c.u
x[:, -1] = np.reshape(p.x.T, 6)
plt.plot(x[0, :], x[1, :], label='boat')
plt.plot(x_ref[0, :], x_ref[1, :], label='reference')
plt.legend()
plt.axis('equal')
plt.grid()
plt.show()
def MarginalDistributionFromMRAP (H):
"""
Returns the phase type distributed marginal distribution
of a marked rational arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP
precision : double, optional
Numerical precision for checking if the input is valid.
The default value is 1e-14
Returns
-------
alpha : matrix, shape (1,M)
The initial vector of the matrix exponentially
distributed marginal
A : matrix, shape (M,M)
The matrix parameter of the matrix exponentially
distributed marginal
"""
if butools.checkInput and not CheckMRAPRepresentation (H):
raise Exception("MarginalDistributionFromMRAP: Input is not a valid MRAP representation!")
Hk = ml.matrix(H[1])
for i in range (2, len(H)):
Hk += H[i]
return (DRPSolve(la.inv(-H[0])*Hk), H[0])
def Scale2dMat(sx, sy):
"""
Return an affine translation matrix that scales the points
toward/away from the origin by a factor of sx on x-axis and sy on
the y-axis.
"""
return matrix([[sx, 0, 0], [0, sy, 0], [0, 0, 1]])
def __init__(self, **kw):
super(AffineTransform,self).__init__(**kw)
# This buffer prevents having to allocate memory for each point.
self._op_buf = matrix([[0.0],
[0.0],
[1.0]])
def Rotate2dMat(t):
"""
Return an affine transformation matrix that rotates the points
around the origin by t radians.
"""
return matrix([[cos(t), -sin(t), 0],
[sin(t), cos(t), 0],
[ 0 , 0 , 1]])
def Translate2dMat(xoff,yoff):
"""
Return an affine transformation matrix that translates points by
the offset (xoff,yoff).
"""
return matrix([[1, 0, xoff],
[0, 1, yoff],
[0, 0, 1 ]])
def RotationZ(joint_angle): Tz = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Tz = matrix([[c,-s,0,0], [s,c,0,0], [0,0,1,0], [0,0,0,1]]) return Tz
def RotationY(joint_angle): Ty = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Ty = matrix([[c,0,s,0], [0,1,0,0], [-s,0,c,0], [0,0,0,1]]) return Ty
def RotationX(joint_angle): Tx = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Tx = matrix([[1,0,0,0], [0,c,-s,0], [0,s,c,0], [0,0,0,1]]) return Tx
def __new__(cls, m, n):
"""
"""
if not all((
isinstance(m, int),
isinstance(n, int),
)):
raise ValueError
data = matlib.hstack([
matlib.vstack((
matlib.hstack((matlib.identity(n, dtype=int), matlib.matrix(p, dtype=int).T)),
matlib.hstack((matlib.matrix(p, dtype=int), matlib.ones(1))),
))
for p in product(range(m), repeat=n)
])
return super().__new__(cls, data, dtype=int).view(cls)
def GM1StationaryDistr (B, R, K):
"""
Returns the stationary distribution of the G/M/1 type
Markov chain up to a given level K.
Parameters
----------
A : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator in the
regular part, from 0 to M-1.
B : length(M) list of matrices of shape (N,N)
Matrix blocks of the G/M/1 type generator at the
R : matrix, shape (N,N)
Matrix R of the G/M/1 type Markov chain
K : integer
The stationary distribution is returned up to
this level.
Returns
-------
pi : array, shape (1,(K+1)*N)
The stationary probability vector up to level K
"""
if not isinstance(B,np.ndarray):
B = np.vstack(B)
m = R.shape[0]
I = ml.eye(m)
temp = (I-R).I
if np.max(temp<-100*butools.checkPrecision):
raise Exception("The spectral radius of R is not below 1: GM1 is not pos. recurrent")
maxb = B.shape[0]//m
BR = B[(maxb-1)*m:,:]
for i in range(maxb-1,0,-1):
BR = R * BR + B[(i-1)*m:i*m,:]
pix = DTMCSolve(BR)
pix = pix / np.sum(pix*temp)
pi = [pix]
sumpi = np.sum(pix)
numit = 1
while sumpi < 1.0-1e-10 and numit < 1+K:
pix = pix*R; # compute pi_(numit+1)
numit += 1
sumpi += np.sum(pix);
pi.append(ml.matrix(pix))
if butools.verbose:
print("Accumulated mass after ", numit, " iterations: ", sumpi)
if butools.verbose and numit == K+1:
print("Maximum Number of Components ", numit-1, " reached")
return np.hstack(pi)
