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 __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}
def getCorrelationMatrix(DAG,default=False, default_weight=1):
#compute correlation matrix from DAG
f=open(str(DAG),'r')
edges=[float(i) for i in f.read().strip().split()] # read in edges (and weights)
f.close()
num = 3 #using specified weights
if default == "True":
num = 2 #using default weight
for i in xrange(len(edges)/num):
edges[num*i]=int(edges[num*i])
edges[num*i+1]=int(edges[num*i+1])
temp = [[edges[num*i],edges[num*i+1]] for i in xrange(len(edges)/num)] #get a list of all the vertices
temp = list(itertools.chain(*temp))
n = reduce(max,temp) #compute number of vertices
checkConnected(temp, n) #sanity check:make sure every vertex is used
adj_list = [[edges[num*i+j] for j in xrange(num)] for i in xrange(len(edges)/num)] #create adjacency list
if default == "True":
for i in xrange(len(adj_list)):
adj_list[i].append(default_weight) #add weights to adjacency list
I = matlib.identity(n)
A = matlib.zeros((n,n))
for edge in adj_list:
A[edge[0]-1,edge[1]-1]=edge[2] #create adjacency matrix from adjacency list using weights
return (I-A)*(I-A).getT(),n #(I-A)D(I-A)^T, here D=I, but in general can choose D differently
def tikhonov(A, b, alpha, allowNegative=True):
"""
Wendet die Tikhonov-Regularisierung auf die Matrix A und die Lösung b an.
@type A: matrix
@param A: Die zu regularisierende Matrix.
@type b: vector
@param b: Die Lösung der Matrixoperation.
@type alpha: number
@param alpha: Der Regularisierungsparameter.
@type allowNegative: boolean
@param allowNegative: Wenn false, werden nur positive x-Werte zur Annäherung erlaubt; sonst auch negative.
@rtype: vector, number, number
@return: Solution, Residuum, Norm der Lösung
"""
n = A.shape[0]
m = A.shape[1]
A1 = np.concatenate((A, alpha * matlib.identity(m)))
b1 = np.concatenate((b, np.zeros(shape=(n,1))))
print A, A.shape
print A1, A1.shape
if (allowNegative):
x, res, rank, s = lstsq(A1, np.squeeze(b1))
return x, res, norm(x)
else:
x, res = spOpt.nnls(A1, np.squeeze(b1))
return x, res, norm(x)
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
# YOUR CODE HERE
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'],
}
# , 'LWristYaw', 'LHand'
# , 'RWristYaw', 'RHand'
# Laenge der Abstaende der einzelnen Gelenke in den chains
self.jointLengths = {'HeadYaw' : (0, 0, 0.1265), 'HeadPitch' : (0, 0, 0),
# left arm and leg
'LShoulderPitch' : (0, 0.098, 0.1), 'LShoulderRoll' : (0, 0, 0), 'LElbowYaw' : (0.105, 0.015, 0),
'LElbowRoll' : (0, 0, 0), 'LWristYaw' : (0.05595, 0, 0), 'LHand' : (0.05775, 0, 0.01231),
'LHipYawPitch' : (0, 0.05, -0.085), 'LHipRoll' : (0, 0, 0), 'LHipPitch' : (0, 0, 0),
'LKneePitch' : (0, 0, -0.1), 'LAnklePitch' : (0, 0, -0.1029), 'LAnkleRoll' : (0, 0, 0),
# right arm and leg
'RShoulderPitch' : (0, -0.098, 0.1), 'RShoulderRoll' : (0, 0, 0), 'RElbowYaw' : (0.105, -0.015, 0),
'RElbowRoll' : (0, 0, 0), 'RWristYaw' : (0.05595, 0, 0), 'RHand' : (0.05775, 0, 0.01231),
'RHipYawPitch' : (0, -0.05, -0.085), 'RHipRoll' : (0, 0, 0), 'RHipPitch' : (0, 0, 0),
'RKneePitch' : (0, 0, -0.1), 'RAnklePitch' : (0, 0, -0.1029), 'RAnkleRoll' : (0, 0, 0)}
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 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 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 forward_kinematics(self, joints):
"""forward kinematics
:param joints: {joint_name: joint_angle}
"""
T = identity(4)
for joint in joints.keys():
angle = joints[joint]
Tl = self.local_trans(joint, angle)
self.transforms[joint] = np.dot(T, Tl)
def end_transform(joints):
"""Return the homogeneous transformation matrix of the whole serial chain
Return the homogeneous transformation matrix of the whole serial chain as
numpy.matrix.
"""
T = matlib.identity(4)
for jnt in joints:
T *= jnt.T
return T
def KFDA_J(self,K,N_p,N_n,q=0):
#calculate the kfda J
#N_p:the number of the positive feature
#N_n:the number of the negtive feature
if q==0:
q=N_p+N_q
a=Nmat.ones([N_p+N_n,1])
a[0:N_p]=1/N_p
a[N_p:]=-1/N_n
A=Nmat.matrix([np.diag(1/np.sqrt(N_p)*(Nmat.identity(N_p)-1/N_p*Nmat.ones([N_p,N_p]))),\
np.diag(1/np.sqrt(N_n)*(Nmat.identity(N_n)-1/N_n*Nmat.ones([N_n,N_n])))])
#
Q=Clustr_kmeans(K,q)
#
tmp=np.linaly.inv(1e-8*Nmat.identity(N_p+N_n)+A*K*A)
tmp=1e8*Nmat.identity(N_p+N_n)-1e8*A*Q*LA.inv(1e8*Nmat.identity(q)+Q.T*A*A(Q))*Q.T*A
J=1/(1e-8)*(a.T*K*a-a.T*K*A*tmp*A*K*a)
return J
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
for chain_joints in CHAINS.values():
T = identity(4)
for joint in chain_joints:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
self.transforms[joint] = np.dot(T, Tl)
def __new__(cls, c):
"""
"""
c = c.view(matlib.matrix).reshape(1, -1)
T = matlib.vstack((
matlib.hstack((matlib.identity(c.size), c)),
matlib.hstack((matlib.zeros(c.size), matlib.ones(1))),
))
return super().__new__(cls, T)
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:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
# YOUR CODE HERE
self.transforms[joint] = T
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']
# YOUR CODE HERE
}
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
return T
def frame_diff(T1, T2):
"""Return the difference between two frames given their transformation
Return the difference (translation and rotation) between two frames given
their homogeneous transformation matrices T1 and T2 from a common frame.
The rotation is expressed by a vector representing the axis of rotation,
and which norm is the rotation angle.
"""
# TODO: add clear reference to Khalil
dtrans = T2[0:3, 3] - T1[0:3, 3]
# TODO: write (find?) a module for homogeneous transforms
# Transpose of the rotational part of T1
Rt = T1[0:3, 0:3].transpose()
from numpy.matlib import identity
invT1 = identity(4)
invT1[0:3, 0:3] = Rt
invT1[0:3, 3] = -Rt * T1[0:3, 3]
# Transform from 1 to 2
T = invT1 * T2
sx = T[0, 0]
sy = T[1, 0]
sz = T[2, 0]
nx = T[0, 1]
ny = T[1, 1]
nz = T[2, 1]
ax = T[0, 2]
ay = T[1, 2]
az = T[2, 2]
from math import sqrt, atan2
cos_theta = 0.5 * (sx + ny + az - 1)
sin_theta = 0.5 * sqrt(
(nz - ay) ** 2 +
(ax - sz) ** 2 +
(sy - nx) ** 2)
theta = atan2(sin_theta, cos_theta)
from numpy import matrix
if abs(sin_theta) < 1e-8:
u = matrix([0.0, 0.0, 1.0]).transpose()
else:
u = matrix([(nz - ay) / (2 * sin_theta),
(ax - sz) / (2 * sin_theta),
(sy - nx) / (2 * sin_theta)]).transpose()
drot = theta * u
from numpy.matlib import zeros
dx = zeros((6, 1))
dx[0:3] = dtrans
dx[3:6] = drot
return dx
def compute_R(T):
A = ml.identity(T)* -2;
A[0,0] = 1;
A[T-1,T-1] = 1;
for i in range(1,T-1):
A[i,i+1] = 1;
A[i,i-1] = 1;
R = A.T*A;
R = R*200
return R
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:
#Some joints are not in the array (for Example RWristYaw)
if joint in joints:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
# YOUR CODE HERE
T = T * Tl
self.transforms[joint] = T
def makeSystem(p,q,m): A=zeros((m,m)) for i in xrange(m): A[i-1,i]=1.0 B=zeros((m,1)) F=zeros((m,m)) I=identity(m) Q=zeros((m,m)) H=zeros((1,m)) R=zeros((1,1)) K=zeros((m,1)) return (p,q,m,A,B,F,I,Q,H,R,K)
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']
}
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:
angle = joints[joint]
Tl = local_trans(joint, angle)
T = np.dot(T,Tl)
#transformation is the product of all local transformations
# YOUR CODE HERE
self.transforms[joint] = T
def __init__(self, numStateVars, measurements): measurements = np.asarray(measurements) self.NZ = NZ = measurements.shape[1] self.N = N = measurements.shape[0] self.NS = NS = numStateVars sz_state = (N, NS, 1) sz_cov = (N, NS, numStateVars) sz_K = (N, NS, NZ) self.I = mt.identity(NS) self.xhat = np.zeros(sz_state) # a posteri estimate of x self.P = np.zeros(sz_cov) # a posteri error estimate self.xhatminus = np.zeros(sz_state) # a priori estimate of x self.Pminus = np.zeros(sz_cov) # a priori error estimate self.K = np.zeros(sz_K) # gain or blending factor self.z = np.reshape(measurements, (N, NZ, 1))
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
# YOUR CODE HERE
self.chains = {
'Head': ['HeadYaw', 'HeadPitch'],
'LArm': [
'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll',
'LWristYaw'
],
'LLeg': [
'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch',
'LAnklePitch', 'LAnkleRoll'
],
'RLeg': [
'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch',
'RAnklePitch', 'RAnkleRoll'
],
'RArm': [
'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll',
'RWristYaw'
]
}
self.links = {
'HeadYaw': (0.0, 0.0, 126.5),
'LShoulderPitch': (0.0, 98.0, 100.0),
'LElbowYaw': (105.0, 15.0, 0.0),
'LWristYaw': (55.95, 0.0, 0.0),
'RShoulderPitch': (0.0, -98.0, 100.0),
'RElbowYaw': (105.0, -15.0, 0.0),
'RWristYaw': (55.95, 0.0, 0.0),
'LHipYawPitch': (0.0, 50.0, -85.0),
'LKneePitch': (0.0, 0.0, -100.0),
'LAnklePitch': (0.0, 0.0, -102.9),
'RHipYawPitch': (0.0, -50.0, -85.0),
'RKneePitch': (0.0, 0.0, -100.0),
'RAnklePitch': (0.0, 0.0, -102.9)
}
def rotation(theta):
"""
2D affine rotation to be used in a matrix multiply operation
to rotate a 2D affine point about the origin.
"""
m = identity(3)
s = sin(theta)
c = cos(theta)
m[0, 0] = c
m[0, 1] = -s
m[1, 0] = s
m[1, 1] = c
return m
def lu_fact(matrix):
matrixCopy = numpy.matrix(matrix, dtype=float);
columns = matrix.shape[1]
rows = matrix.shape[0]
identity = matlib.identity(rows);
#for each non-trivial pivot
for m in range (0, rows-1):
#for each row under pivot
for n in range (m+1, rows):
#Subtract leading/pivot times pivot row from current row
subRow = matrixCopy[m,:]/float(matrixCopy[m,m])*matrixCopy[m,n]
matrixCopy[n] = matrixCopy[n] - subRow
#Put leading/pivot in corresponding location in identity
identity[n,m] = matrixCopy[m,n]/float(matrixCopy[m,m])
#return L, U, and error
return (identity, matrixCopy, common.error(common.mult_mat(identity, matrixCopy), matrix))
def build_se3_transform(xyzrpy):
"""Creates an SE3 transform from translation and Euler angles.
Args:
xyzrpy (list[float]): translation and Euler angles for transform. Must have six components.
Returns:
numpy.matrixlib.defmatrix.matrix: SE3 homogeneous transformation matrix
Raises:
ValueError: if `len(xyzrpy) != 6`
"""
if len(xyzrpy) != 6:
raise ValueError("Must supply 6 values to build transform")
se3 = matlib.identity(4)
se3[0:3, 0:3] = euler_to_so3(xyzrpy[3:6])
se3[0:3, 3] = np.matrix(xyzrpy[0:3]).transpose()
return se3
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'],
# YOUR CODE HERE
'LArm': ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'],# 'LWristYaw'],
'LLeg': ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'],
'RLeg': ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'],
'RArm': ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll'],#'RWristYaw']
}
self.joint_offsets = { "HeadYaw": [.0, .0, 126.50], #from Torso
"HeadPitch": [.0, .0, .0], #from
#Left Shoulder
"LShoulderPitch": [.0, 98.0, 100.0], #from Torso
"LShoulderRoll": [.0, .0, .0], #from LShoulderPitch
"LElbowYaw": [105.0, 15.0, 0.0], #from LShoulderRoll
"LElbowRoll": [.0, .0, .0], #from LElbowYaw
"LWristYaw": [55.95, .0, .0], #from LElbowRoll
#Left Leg
"LHipYawPitch": [.0, 50.0, -85.0], #from Torso
"LHipRoll": [.0, .0, .0], #from LHipYawPitch
"LHipPitch": [.0, .0, .0], #from LHipRoll
"LKneePitch": [.0, .0, -100.0], #from LHipPitch
"LAnklePitch": [.0, .0, -102.9], #from LKneePitch
"LAnkleRoll": [.0, .0, .0], #from LAnklePitch
#Right Shoulder
"RShoulderPitch": [.0, -98.0, 100.0], #from Torso
"RShoulderRoll": [.0, .0, .0], #from RShoulderPitch
"RElbowYaw": [105.0, -15.0, 0.0], #from RShoulderRoll
"RElbowRoll": [.0, .0, .0], #from RElbowYaw
"RWristYaw": [55.95, .0, .0], #from RElbowRoll
#Right Leg
"RHipYawPitch": [.0, -50.0, -85.0], #from Torso
"RHipRoll": [.0, .0, .0], #from RHipYawPitch
"RHipPitch": [.0, .0, .0], #from RHipRoll
"RKneePitch": [.0, .0, -100.0], #from RHipPitch
"RAnklePitch": [.0, .0, -102.9], #from RKneePitch
"RAnkleRoll": [.0, .0, .0], #from RAnklePitch
}
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 rot(angle, d):
T = identity(4)
if d == 'x':
T = numpy.matrix([[1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0],
[0, math.sin(angle),
math.cos(angle), 0], [0, 0, 0, 1]])
if d == 'y':
T = numpy.matrix([[math.cos(angle), 0,
math.sin(angle), 0], [0, 1, 0, 0],
[-math.sin(angle), 0,
math.cos(angle), 0], [0, 0, 0, 1]])
if d == 'z':
T = numpy.matrix([[math.cos(angle), -math.sin(angle), 0, 0],
[math.sin(angle),
math.cos(angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
return T
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:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
# YOUR CODE HERE
T = np.dot(T, Tl)
self.transforms[joint] = T
#print joint
#print T
#if joint == "LAnkleRoll":
# print T
return self.transforms
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', 'LElbowRoll', 'LElbowYaw'],
'LLeg': [
'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch',
'LAnklePitch', 'LAnkleRoll'
],
'RLeg': [
'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch',
'RAnklePitch', 'RAnkleRoll'
],
'RArm':
['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll']
}
self.links = {
"Head-Offset": (0, 0, 126.5),
"HeadYaw": (0, 0, 0),
"HeadPitch": (0, 0, 0),
"Arm-Offset": (0, 0, 126.5),
"ShoulderPitch": (0, 0, 0),
"ShoulderRoll": (105, 15, 0),
"ElbowYaw": (0, 0, 0),
"ElbowRoll": (55.95, 0, 0),
"Leg-Offset": (0, 50, -85),
"HipYawPitch": (0, 0, 0),
"HipRoll": (0, 0, 0),
"HipPitch": (0, 0, -100),
"KneePitch": (0, 0, -102.9),
"AnklePitch": (0, 0, 0),
"AnkleRoll": (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
'''
T = identity(4)
c = cos(joint_angle)
#print "cos(%s)=%s"%(joint_angle, c)
s = sin(joint_angle)
x_offset, y_offset, z_offset = self.joint_offsets[joint_name]
# YOUR CODE HERE
if(joint_name == "LShoulderRoll"):
joint_name = "LShoulderYaw"
if(joint_name == "RShoulderRoll"):
joint_name = "RShoulderYaw"
if(joint_name == "LElbowYaw"):
joint_name = "LElbowRoll"
if(joint_name == "LElbowRoll"):
joint_name = "LElbowYaw"
if(joint_name.find('Roll')>0): #arround x-Axis -> Rx
T=matrix([ [1.0, 0.0, 0.0, x_offset],
[0.0, c , -s , y_offset],
[0.0, s , c , z_offset],
[0.0, 0.0, 0.0 , 1.0]])
#print "Rotate joint %s about x-axis (Roll) for %s"%(joint_name,str(joint_angle))
elif(joint_name.find('Pitch')>0): #arround y-Axis -> Ry
T=matrix([ [ c , 0.0, s , x_offset],
[ 0.0, 1.0, 0.0, y_offset],
[-s , 0.0, c , z_offset],
[ 0.0, 0.0, 0.0, 1.0]])
# print "Rotate joint %s about y-axis (Pitch) for %s"%(joint_name,str(joint_angle))
elif(joint_name.find('Yaw')>0): #arround z -> Rz
T=matrix([ [ c , -s , 0.0, x_offset],
[ s , c , 0.0, y_offset],
[ 0.0, 0.0, 1.0, z_offset],
[ 0.0, 0.0, 0.0, 1.0]])
# print "Rotate joint %s about z-axis (Yaw) for %s"%(joint_name, str(joint_angle))
else:
print "WARNING: In Kinematics.local_trans() found neighter 'Pitch', 'Roll' nor 'Yaw' in joint_name"
return T
def __montar_matriz_transformacao(self):
''' A Matriz de Transformação é a Matriz Identidade de dimenssão igual
ao número de varíaveis, trocando-se a linha correspondente à variável
ativa pelo simétrico dos coeficientes da restrição ativa divididos pelo
valor da restrição ativa .
'''
i_restr_ativ = self.__indice_restr_ativa()
i_var_ativ = self.__indice_var_ativa()
# Cria a matriz identidade.
matriz_T = matlib.identity(self.qtd_variaveis())
#Coeficiente da variável ativa na restrição ativa.
valor = self.restricoes[i_restr_ativ, i_var_ativ]
matriz_T[i_var_ativ] = np.dot(self.restricoes[i_restr_ativ], (-1.0 / valor))
return matriz_T
def __new__(cls, n, a, b):
"""
"""
if not (
isinstance(n, int)
and isinstance(a, int)
and isinstance(b, int)
and a < n
and b < n
):
raise ValueError
R = matlib.identity(n+1, dtype=int)
R[a, a] = 0
R[b, b] = 0
R[a, b] = -1
R[b, a] = 1
return super().__new__(cls, R)
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:
if (joint=='RWristYaw' or 'LWristYaw'):
angle=0
else:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
T=np.dot(T,Tl)
#print(T)
# YOUR CODE HERE
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)
if joint_name in [
"HeadYaw", "LElbowYaw", "RElbowYaw", "LHipYawPitch",
"RHipYawPitch"
]:
t = np.dot(
t,
np.array([[np.cos(joint_angle), -np.sin(joint_angle), 0, 0],
[np.sin(joint_angle),
np.cos(joint_angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
elif joint_name.endswith("Pitch"):
t = np.dot(
t,
np.array([[np.cos(joint_angle), 0,
np.sin(joint_angle), 0], [0, 1, 0, 0],
[-np.sin(joint_angle), 0,
np.cos(joint_angle), 0], [0, 0, 0, 1]]))
elif joint_name.endswith("Roll"):
t = np.dot(
t,
np.array([[1, 0, 0, 0],
[0, np.cos(joint_angle), -np.sin(joint_angle), 0],
[0, np.sin(joint_angle),
np.cos(joint_angle), 0], [0, 0, 0, 1]]))
for i in self.chains.keys():
if joint_name in self.chains[i]:
t[0,
3] = self.joint_len[i][self.chains[i].index(joint_name)][0]
t[1,
3] = self.joint_len[i][self.chains[i].index(joint_name)][1]
t[2,
3] = self.joint_len[i][self.chains[i].index(joint_name)][2]
return t
def homework_3_problem_1():
# Generate X matrix with border
dim = 256
A = M.identity(dim)
A = A + M.fliplr(A)
A[0, :] = 1
A[-1, :] = 1
A[:, 0] = 1
A[:, -1] = 1
A[dim / 2, dim / 2] = 1
A = A * 255
fig = plt.gcf()
fig.suptitle("Problem 1: Haar Transform")
# Display original matrix
plt.subplot(321)
plt.title('original')
plt.imshow(A, cm.Greys_r)
plt.subplot(322)
plt.title('forward')
wavelet.forward(h, A, recursive=False)
plt.imshow(A, cm.Greys_r)
plt.subplot(323)
plt.title('top left')
plt.imshow(A[:dim / 2, :dim / 2], cm.Greys_r)
plt.subplot(324)
plt.title('top right')
plt.imshow(A[:dim / 2, dim / 2:], cm.Greys_r)
plt.subplot(325)
plt.title('bottom left')
plt.imshow(A[dim / 2:, :dim / 2], cm.Greys_r)
plt.subplot(326)
plt.title('bottom right')
plt.imshow(A[dim / 2:, dim / 2:], cm.Greys_r)
plt.show()
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
#print("joints dic : ", joints)
#print (len(self.chains.values()))
for chain_joints in self.chains.values():
T = identity(4)
for joint in chain_joints:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
# YOUR CODE HERE
T = np.dot(T,Tl)
self.transforms[joint] = T
#print(self.transforms['HeadYaw'][3][2])
"""
def seidel_method(A, b, x_init=None, n_steps=100, eps=None, log=False):
#A, b = replace_zeroes(A, b)
B1 = np.mat([[A[i, j] / A[i, i] if i < j else 0 for j in range(len(b))] for i in range(len(b))])
B2 = np.mat([[A[i, j] / A[i, i] if i > j else 0 for j in range(len(b))] for i in range(len(b))])
b = np.mat([[b[i] / A[i, i]] for i in range(len(b))])
if eps:
B2n = norm(B2, ord=2)
Bn = norm(B1 + B2, ord=2)
B1 = (identity(len(b)) + B1).I
x = x_init.copy() if x_init else np.mat([[0]] * len(b))
my_steps = 0
while n_steps == None or my_steps < n_steps:
x1 = B1 * (b - B2 * x)
my_steps += 1
if eps and norm(x1 - x) <= eps: #norm(x1 - x) <= (1 - Bn) * eps / B2n:
break
x = x1
if log:
print('Seidel:', my_steps, 'iterations')
return x1
def build_se3_transform(xyzrpy):
"""Creates an SE3 transform from translation and Euler angles.
Args:
xyzrpy (list[float]): translation and Euler angles for transform. Must have six components.
Returns:
numpy.matrixlib.defmatrix.matrix: SE3 homogeneous transformation matrix
Raises:
ValueError: if `len(xyzrpy) != 6`
"""
if len(xyzrpy) != 6:
raise ValueError("Must supply 6 values to build transform")
se3 = matlib.identity(4)
se3[0:3, 0:3] = euler_to_so3(xyzrpy[3:6])
se3[0:3, 3] = np.matrix(xyzrpy[0:3]).transpose()
return se3
def __montar_matriz_transformacao(self):
''' A Matriz de Transformação é a Matriz Identidade de dimenssão igual
ao número de varíaveis, trocando-se a linha correspondente à variável
ativa pelo simétrico dos coeficientes da restrição ativa divididos pelo
valor da restrição ativa .
'''
i_restr_ativ = self.__indice_restr_ativa()
i_var_ativ = self.__indice_var_ativa()
# Cria a matriz identidade.
matriz_T = matlib.identity(self.qtd_variaveis())
#Coeficiente da variável ativa na restrição ativa.
valor = self.restricoes[i_restr_ativ, i_var_ativ]
matriz_T[i_var_ativ] = np.dot(self.restricoes[i_restr_ativ],
(-1.0 / valor))
return matriz_T
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
i = 1
for chain_joints in self.chains.values():
T = identity(4)
for joint in chain_joints:
if joint == 'LWristYaw' or joint == 'RWristYaw':
angle = 0
else:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
T = T * Tl
self.transforms[joint] = T
print chain_joints
print T
def __new__(self, **kwargs):
self = matlib.identity(4)
if 'RP' in kwargs:
R, P = kwargs['RP']
for i in range(0, 3):
R[:, i] = force_vector_unity(R[:, i])
if not is_Orthogonal(R):
raise ValueError('R is not orthogonal')
self[0:3, 0:3] = R
self[0:3, 3] = P
elif 'xyzabc' in kwargs:
x, y, z, a, b, c = kwargs['xyzabc']
R = numpy.matrix(gu.Rz_matrix(c))*numpy.matrix(gu.Ry_matrix(b))*numpy.matrix(gu.Rx_matrix(a))
P = numpy.matrix([x, y, z]).T
self = HomogenousTransformation(RP=(R, P))
else:
pass
#
# Euler sequence : translate xyz -> rotate about x alpha radians -> rotate about y
#
return self
