def check_orthonormal(basis1, basis2, basis3):
""" Return True if bases are mutually orthogonal, False otherwise , does not indicate which criterion fails """
orthogonal = round_small(vec_proj(basis1, basis2)) == 0 and round_small(
vec_proj(basis1, basis3)) == 0 and round_small(vec_proj(
basis2, basis3)) == 0
# dbgLog(-1, round_small( vec_proj(basis1,basis2) ) , round_small( vec_proj(basis1,basis3) ) , round_small( vec_proj(basis2,basis3) ) )
normal = eq(vec_mag(basis1), 1) and eq(vec_mag(basis2), 1) and eq(
vec_mag(basis3), 1)
# dbgLog(-1, vec_mag(basis1) , vec_mag(basis2) , vec_mag(basis3) )
return orthogonal and normal
def shortest_btn_vecs(v1, v2):
""" Return the Quaternion representing the shortest rotation from vector 'v1' to vector 'v2' """
# print "DEBUG , Got vectors: " , v1 , v2
# print "DEBUG , Crossed vectors: " , np.cross( v1 , v2 )
# print "DEBUG , Crossed unit vec:" , vec_unit( np.cross( v1 , v2 ) )
# print "DEBUG , Angle between: " , vec_angle_between( v1 , v2 )
angle = vec_angle_between(v1, v2)
if eq(angle, 0.0):
return Quaternion.no_turn_quat()
elif eq(angle, pi):
# If we are making a pi turn, then just pick any axis perpendicular to the opposing vectors and make the turn
# The axis that is picked will result in different poses
return Quaternion.k_rot_to_Quat(vec_unit(np.cross(v1, [1, 0, 0])),
angle)
else:
return Quaternion.k_rot_to_Quat(vec_unit(np.cross(v1, v2)), angle)
def joint_homog( pitch , q ):
""" Return the joint homogeneous transform matrix for a joint with 'pitch' and joint variable 'q' """
# See page 135 of [3] for the homogeneous transformations for each type
if eq( pitch , 0.0 ): # Revolute Joint : Implements pure rotation
E = z_trn( q )
r = [ 0 , 0 , 0 ]
elif pitch == infty: #- Prismatic Joint : Implements pure translation
E = np.eye( 3 )
r = [ 0 , 0 , q ]
else: # --------------- Helical Joint : Implements a screwing motion
E = z_trn( q )
r = [ 0 , 0 , pitch * q ]
return homog_xform( E , r )
def joint_spatl( pitch , q ): # Featherstone: jcalc
""" Return the joint spatial coordinate transform and subspace matrix for a joint with 'pitch' and joint variable 'q' """
# NOTE : This function is for transform coordinate bases , not positions
if eq( pitch , 0.0 ): # Revolute Joint : Implements pure rotation
E = z_trn( q )
# E = z_rot( q )
r = [ 0 , 0 , 0 ]
s_i = [ 0 , 0 , 1 , 0 , 0 , 0 ]
XJ_s = sp_rot_xfrm( E )
# XJ_s = sp_rot_xfrm_alt( E )
elif pitch == infty: #- Prismatic Joint : Implements pure translation
E = np.eye( 3 ); r = [ 0 , 0 , q ]
s_i = [ 0 , 0 , 0 , 0 , 0 , 1 ]
XJ_s = sp_trn_xfrm_mtn( r )
else: # --------------- Helical Joint : Implements a screwing motion
E = z_trn( q ); r = [ 0 , 0 , pitch * q ]
s_i = [ 0 , 0 , 1 , 0 , 0 , pitch ]
XJ_s = np.dot( sp_rot_xfrm( E ) , sp_trn_xfrm_mtn( r ) )
return XJ_s , s_i
def eq(p, q):
""" Determine if two quaterions are equal enough """
return eq(p.sclr, q.sclr) and vec_eq(p._vctr, q._vctr)