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