Ejemplo n.º 1
0
 def get_cjoint_diff(self):
     """Return the frame difference at a cut joint"""
     # The first cut joint will be used as reference.
     from serialmechanism import end_transform
     T0 = end_transform(self.chains[0])
     # The other cut joints will be used to make a difference from the
     # first cut joint.
     from numpy.matlib import zeros
     dx = zeros((6 * (len(self.chains) - 1), 1))
     for k, chain in enumerate(self.chains[1:]):
         T = end_transform(chain)
         dx[(k * 6):(k * 6 + 6)] = frame_diff(T0, T)
     return dx
Ejemplo n.º 2
0
 def get_cjoint_diff(self):
     """Return the frame difference at a cut joint"""
     # The first cut joint will be used as reference.
     from serialmechanism import end_transform
     T0 = end_transform(self.chains[0])
     # The other cut joints will be used to make a difference from the
     # first cut joint.
     from numpy.matlib import zeros
     dx = zeros((6 * (len(self.chains) - 1), 1))
     for k, chain in enumerate(self.chains[1:]):
         T = end_transform(chain)
         dx[(k * 6):(k * 6 + 6)] = frame_diff(T0, T)
     return dx
Ejemplo n.º 3
0
def serialKinematicJacobian(joints):
    """Return the kinematic Jacobian of a serial chain

    Passive joints are considered to be as if they were actuated. The end
    joint is the last joint in the list and the base joint is its farest
    antecedent.
    """
    from chain import Chain
    from chain import get_subchain_to

    chain = Chain(get_subchain_to(joint=joints[-1], joints=joints))

    # We need the position of last joint frame in the base joints frame
    from serialmechanism import end_transform
    Tend = end_transform(chain.joints)
    Pend = Tend[0:3, 3]

    from numpy.matlib import zeros, identity, cross
    jacobian = zeros((6, len(chain.get_mjoints())))
    T = identity(4)
    k = 0
    for jnt in chain.joints:
        T *= jnt.T
        #Take P and a vectors
        P = T[0:3, 3]
        a = T[0:3, 2]
        if jnt.isrevolute():
            #revolute: [ahat*(Pend-P), a]
            DP = Pend - P
            a_hat = zeros((3, 3))
            a_hat[0, 1] = -a[2]
            a_hat[0, 2] = a[1]
            a_hat[1, 0] = a[2]
            a_hat[1, 2] = -a[0]
            a_hat[2, 0] = -a[1]
            a_hat[2, 1] = a[0]
            jacobian_trans = a_hat * DP
            #jacobian_trans = cross(P, DP, axis=0)
            jacobian_rot = a
        if jnt.isprismatic():
            #prismatic: [a, 0]
            jacobian_trans = a
            jacobian_rot = zeros((3, 1))

#skip fixed joints
        if not (jnt.isfixed()):
            jacobian[:3, k] = jacobian_trans
            jacobian[3:, k] = jacobian_rot
            k += 1
    # If the last joint is fixed, apply the Jacobian transposition
    if (joints[-1].isfixed() and not joints[-1].sameas == None):
        P = jnt.T[0:3, 2]
        jac_transposition = identity(6)
        jac_transposition[3, 1] = -P[2]
        jac_transposition[3, 2] = P[1]
        jac_transposition[4, 0] = P[2]
        jac_transposition[4, 2] = -P[0]
        jac_transposition[5, 0] = -P[1]
        jac_transposition[5, 1] = P[0]

        jacobian = jac_transposition * jacobian
    return jacobian
Ejemplo n.º 4
0
def serialKinematicJacobian(joints):
    """Return the kinematic Jacobian of a serial chain

    Passive joints are considered to be as if they were actuated. The end
    joint is the last joint in the list and the base joint is its farest
    antecedent.
    """
    from chain import Chain
    from chain import get_subchain_to

    chain = Chain(get_subchain_to(joint=joints[-1], joints=joints))

    # We need the position of last joint frame in the base joints frame
    from serialmechanism import end_transform
    Tend = end_transform(chain.joints)
    Pend = Tend[0:3, 3]

    from numpy.matlib import zeros, identity, cross
	# jacobian only includes moving joints
    jacobian = zeros((6, len(chain.get_mjoints())))
    T = identity(4)
    k = 0
    for jnt in chain.joints:
        T *= jnt.T
		# Take P and a vectors
        P = T[0:3, 3]
        a = T[0:3, 2]
        if jnt.isrevolute():
			# revolute: [ahat*(Pend-P), a]
            DP = Pend - P
            a_hat = zeros((3,3))
            a_hat[0, 1] = -a[2]
            a_hat[0, 2] = a[1]
            a_hat[1, 0] = a[2]
            a_hat[1, 2] = -a[0]
            a_hat[2, 0] = -a[1]
            a_hat[2, 1] = a[0]
            jacobian_trans = a_hat * DP
            #jacobian_trans = cross(P, DP, axis=0)
            jacobian_rot = a
        if jnt.isprismatic():
			# prismatic: [a, 0]
            jacobian_trans = a
            jacobian_rot = zeros((3, 1))
		# skip fixed joints
        if not(jnt.isfixed()):
            jacobian[:3, k] = jacobian_trans
            jacobian[3:, k] = jacobian_rot
            k += 1
    # If the last joint is fixed, apply the Jacobian transposition
    if (joints[-1].isfixed()):
        P = jnt.T[0:3, 2]
        jac_transposition = identity(6)
        jac_transposition[3, 1] = -P[2]
        jac_transposition[3, 2] = P[1]
        jac_transposition[4, 0] = P[2]
        jac_transposition[4, 2] = -P[0]
        jac_transposition[5, 0] = -P[1]
        jac_transposition[5, 1] = P[0]

        jacobian = jac_transposition * jacobian
    return jacobian