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() # 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.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()]