Exemplo n.º 1
0
    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()]
Exemplo n.º 2
0
    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()]
Exemplo n.º 3
0
    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()]
Exemplo n.º 4
0
  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()]
Exemplo n.º 5
0
  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()]
Exemplo n.º 6
0
  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()]