def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.lojoint = NeckKinematics.lower_neck()
# XXX TODO remove hard-coded physical dimensions
# Han neck mechanism has the upper joint being 8.93 centimeters
# in front of the lower joint, and 112.16 centimeters above it.
self.kappa = math.atan2(8.93, 112.16)
# Returns the upper-neck left motor position, in radians
def get_upper_left(q):
(phi, theta, psi) = quat_to_asa(q)
try:
self.hijoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Upper left motor jam", theta, phi
# print "Upper motors:", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q):
(phi, theta, psi) = quat_to_asa(q)
try:
self.hijoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Upper right motor jam", theta, phi
return self.hijoint.theta_r
# Returns the lower-neck left motor position, in radians
def get_lower_left(q):
(phi, theta, psi) = quat_to_asa(q)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
try:
self.lojoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Lower left motor jam", theta, phi
# print "Lower motors:", self.lojoint.theta_l, self.lojoint.theta_r
return self.lojoint.theta_l
# Returns the lower-neck right motor position, in radians
def get_lower_right(q):
(phi, theta, psi) = quat_to_asa(q)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
# print "Lower theta-phi:", theta, phi
try:
self.lojoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Lower right motor jam", theta, phi
return self.lojoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q):
(phi, theta, psi) = quat_to_asa(q)
# XXX TODO The neck cant produces a small correction to the yaw,
# the formulas are there in neck_cant, but need to be integrated
# here...
# The byaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
byaw = psi + phi
if 3.1415926 < byaw:
byaw -= 2 * 3.14159265358979
return byaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'loleft': lambda q: get_lower_left(q),
'loright': lambda q: get_lower_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]
def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.lojoint = NeckKinematics.lower_neck()
# split the angles in two, giving sume to the upper and some to the
# lower ujoint. Valid values for split are -1.0 to 2.0, with a
# 50%-50% given by setting split to 0.5.
# Setting upper_split to 1.0 should result in exactly the same
# behavior as the Quaternion2Upper class above.
self.upper_split = 0.5
self.lower_split = 1.0 - self.upper_split
# XXX TODO remove hard-coded physical dimensions
# Han neck mechanism has the upper joint being 8.93 centimeters
# in front of the lower joint, and 112.16 centimeters above it.
self.kappa = math.atan2(8.93, 112.16)
# Returns the upper-neck left motor position, in radians
def get_upper_left(q):
fq = quat_fraction(q, self.upper_split)
(phi, theta, psi) = quat_to_asa(fq)
self.hijoint.inverse_kinematics(theta, phi)
# print "Upper theta-phi:", theta, phi
# print "Upper motors:", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q):
fq = quat_fraction(q, self.upper_split)
(phi, theta, psi) = quat_to_asa(fq)
self.hijoint.inverse_kinematics(theta, phi)
return self.hijoint.theta_r
# Returns the lower-neck left motor position, in radians
def get_lower_left(q):
fq = quat_fraction(q, self.lower_split)
(phi, theta, psi) = quat_to_asa(fq)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
self.lojoint.inverse_kinematics(theta, phi)
# print "Lower theta-phi:", theta, phi
# print "Lower motors:", self.lojoint.theta_l, self.lojoint.theta_r
return self.lojoint.theta_l
# Returns the lower-neck right motor position, in radians
def get_lower_right(q):
fq = quat_fraction(q, self.lower_split)
(phi, theta, psi) = quat_to_asa(fq)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
self.lojoint.inverse_kinematics(theta, phi)
return self.lojoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q):
(phi, theta, psi) = quat_to_asa(q)
# XXX TODO The neck cant produces a small correction to the yaw,
# the formulas are there in neck_cant, but need to be integrated
# here...
# The byaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
byaw = psi + phi
if 3.1415926 < byaw:
byaw -= 2 * 3.14159265358979
return byaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'loleft': lambda q: get_lower_left(q),
'loright': lambda q: get_lower_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]
def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.worst = 0
# Returns the upper-neck left motor position, in radians
def get_upper_left(q):
(phi, theta, psi) = quat_to_asa(q)
self.hijoint.inverse_kinematics(theta, phi)
# print "Motors: left right", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q):
(phi, theta, psi) = quat_to_asa(q)
self.hijoint.inverse_kinematics(theta, phi)
return self.hijoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q):
(phi, theta, psi) = quat_to_asa(q)
# The twist factor is the actual correction for the fact
# that the gimbal in the neck assembly prevents the u-joint
# from rotating.
twist = math.atan2(math.tan(phi), math.cos(theta))
if twist < 0:
twist += 3.14159265358979
# The actual neck yaw.
yaw = psi + twist
if 3.1415926 < yaw:
yaw -= 2 * 3.14159265358979
# The bad_yaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
bad_yaw = psi + phi
if 3.1415926 < bad_yaw:
bad_yaw -= 2 * 3.14159265358979
# Hacky corrections to bad quadrant of the arctan...
if 2 < yaw - bad_yaw:
yaw -= 3.14159265358979
if yaw - bad_yaw < -2:
yaw += 3.14159265358979
if self.worst < abs(yaw - bad_yaw):
self.worst = abs(yaw - bad_yaw)
print "worst: ", self.worst, self.worst * 180 / 3.14159
if 0.01 < yaw - bad_yaw or yaw - bad_yaw < -0.01:
print "Aieeeee! bad yaw!", yaw, bad_yaw, yaw - bad_yaw
exit
# print "Yaw: %9f" % bad_yaw, "%9.6f" % psi, "%9.6f" % phi, \
# "%9.6f" % twist, "%10.7f" % yaw, "delta %10.7f" % (yaw-bad_yaw)
# print "Yaw and approximation error", yaw, yaw-bad_yaw
return yaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]
def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.lojoint = NeckKinematics.lower_neck()
# XXX TODO remove hard-coded physical dimensions
# Han neck mechanism has the upper joint being 8.93 centimeters
# in front of the lower joint, and 112.16 centimeters above it.
self.kappa = math.atan2(8.93, 112.16)
# Returns the upper-neck left motor position, in radians
def get_upper_left(q) :
(phi, theta, psi) = quat_to_asa(q)
try:
self.hijoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Upper left motor jam", theta, phi
# print "Upper motors:", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q) :
(phi, theta, psi) = quat_to_asa(q)
try:
self.hijoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Upper right motor jam", theta, phi
return self.hijoint.theta_r
# Returns the lower-neck left motor position, in radians
def get_lower_left(q) :
(phi, theta, psi) = quat_to_asa(q)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
try:
self.lojoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Lower left motor jam", theta, phi
# print "Lower motors:", self.lojoint.theta_l, self.lojoint.theta_r
return self.lojoint.theta_l
# Returns the lower-neck right motor position, in radians
def get_lower_right(q) :
(phi, theta, psi) = quat_to_asa(q)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
# print "Lower theta-phi:", theta, phi
try:
self.lojoint.inverse_kinematics(theta, phi)
except OverflowError:
print "Lower right motor jam", theta, phi
return self.lojoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q) :
(phi, theta, psi) = quat_to_asa(q)
# XXX TODO The neck cant produces a small correction to the yaw,
# the formulas are there in neck_cant, but need to be integrated
# here...
# The byaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
byaw = psi + phi
if 3.1415926 < byaw:
byaw -= 2 * 3.14159265358979
return byaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'loleft': lambda q: get_lower_left(q),
'loright': lambda q: get_lower_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]
def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.lojoint = NeckKinematics.lower_neck()
# split the angles in two, giving sume to the upper and some to the
# lower ujoint. Valid values for split are -1.0 to 2.0, with a
# 50%-50% given by setting split to 0.5.
# Setting upper_split to 1.0 should result in exactly the same
# behavior as the Quaternion2Upper class above.
self.upper_split = 0.5
self.lower_split = 1.0 - self.upper_split
# XXX TODO remove hard-coded physical dimensions
# Han neck mechanism has the upper joint being 8.93 centimeters
# in front of the lower joint, and 112.16 centimeters above it.
self.kappa = math.atan2(8.93, 112.16)
# Returns the upper-neck left motor position, in radians
def get_upper_left(q) :
fq = quat_fraction(q, self.upper_split)
(phi, theta, psi) = quat_to_asa(fq)
self.hijoint.inverse_kinematics(theta, phi)
# print "Upper theta-phi:", theta, phi
# print "Upper motors:", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q) :
fq = quat_fraction(q, self.upper_split)
(phi, theta, psi) = quat_to_asa(fq)
self.hijoint.inverse_kinematics(theta, phi)
return self.hijoint.theta_r
# Returns the lower-neck left motor position, in radians
def get_lower_left(q) :
fq = quat_fraction(q, self.lower_split)
(phi, theta, psi) = quat_to_asa(fq)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
self.lojoint.inverse_kinematics(theta, phi)
# print "Lower theta-phi:", theta, phi
# print "Lower motors:", self.lojoint.theta_l, self.lojoint.theta_r
return self.lojoint.theta_l
# Returns the lower-neck right motor position, in radians
def get_lower_right(q) :
fq = quat_fraction(q, self.lower_split)
(phi, theta, psi) = quat_to_asa(fq)
# (phi, theta, eta) = NeckVertical.neck_cant(phi, theta, psi, self.kappa)
self.lojoint.inverse_kinematics(theta, phi)
return self.lojoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q) :
(phi, theta, psi) = quat_to_asa(q)
# XXX TODO The neck cant produces a small correction to the yaw,
# the formulas are there in neck_cant, but need to be integrated
# here...
# The byaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
byaw = psi + phi
if 3.1415926 < byaw:
byaw -= 2 * 3.14159265358979
return byaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'loleft': lambda q: get_lower_left(q),
'loright': lambda q: get_lower_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]
def __init__(self, args, motor_entry):
self.hijoint = NeckKinematics.upper_neck()
self.worst = 0
# Returns the upper-neck left motor position, in radians
def get_upper_left(q) :
(phi, theta, psi) = quat_to_asa(q)
self.hijoint.inverse_kinematics(theta, phi)
# print "Motors: left right", self.hijoint.theta_l, self.hijoint.theta_r
return self.hijoint.theta_l
# Returns the upper-neck right motor position, in radians
def get_upper_right(q) :
(phi, theta, psi) = quat_to_asa(q)
self.hijoint.inverse_kinematics(theta, phi)
return self.hijoint.theta_r
# Yaw (spin about neck-skull) axis, in radians, right hand rule.
def get_yaw(q) :
(phi, theta, psi) = quat_to_asa(q)
# The twist factor is the actual correction for the fact
# that the gimbal in the neck assembly prevents the u-joint
# from rotating.
twist = math.atan2 (math.tan(phi), math.cos(theta))
if twist < 0 :
twist += 3.14159265358979
# The actual neck yaw.
yaw = psi + twist
if 3.1415926 < yaw:
yaw -= 2 * 3.14159265358979
# The bad_yaw is a "crude" approximation to the correct yaw ...
# but in fact, the worst-case error is about half a percent,
# and usually much much better (less than 1/20th of a percent)
bad_yaw = psi + phi
if 3.1415926 < bad_yaw:
bad_yaw -= 2 * 3.14159265358979
# Hacky corrections to bad quadrant of the arctan...
if 2 < yaw-bad_yaw :
yaw -= 3.14159265358979
if yaw-bad_yaw < -2:
yaw += 3.14159265358979
if self.worst < abs(yaw-bad_yaw) :
self.worst = abs(yaw-bad_yaw)
print "worst: ", self.worst, self.worst * 180 / 3.14159
if 0.01 < yaw-bad_yaw or yaw-bad_yaw < -0.01:
print "Aieeeee! bad yaw!", yaw, bad_yaw, yaw-bad_yaw
exit
# print "Yaw: %9f" % bad_yaw, "%9.6f" % psi, "%9.6f" % phi, \
# "%9.6f" % twist, "%10.7f" % yaw, "delta %10.7f" % (yaw-bad_yaw)
# print "Yaw and approximation error", yaw, yaw-bad_yaw
return yaw
funcs = {
'upleft': lambda q: get_upper_left(q),
'upright': lambda q: get_upper_right(q),
'yaw': lambda q: get_yaw(q)
}
self.map = funcs[args['axis'].lower()]