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