Пример #1
0
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}
Пример #3
0
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
Пример #4
0
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)}
Пример #6
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
Пример #7
0
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
Пример #8
0
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)
Пример #10
0
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
Пример #11
0
    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)
Пример #13
0
 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
Пример #17
0
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
Пример #18
0
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
Пример #20
0
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 
Пример #23
0
 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)
        }
Пример #25
0
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
Пример #26
0
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))
Пример #27
0
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
                                }
Пример #29
0
 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
Пример #31
0
    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
Пример #35
0
 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
Пример #37
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)
        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
Пример #38
0
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
Пример #41
0
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
Пример #42
0
    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
Пример #44
0
 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