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