示例#1
0
def detect_orientation():
    """
    Detect the orientation of the angles on a few simple cases
    Note that if f_kin is wrong, this can cause the orientation
    to be improperly detected.
    """
    x0, y0, z0 = f_kin(0, 0, 0)
    x1, y1, z1 = f_kin(math.pi/2, 0, 0)
    x2, y2, z2 = f_kin(0, math.pi/2, 0)
    x3, y3, z3 = f_kin(0, 0, math.pi/2)
    ori0 = int(math.copysign(1, y1))
    ori1 = int(math.copysign(1, z0-z2))
    ori2 = int(math.copysign(1, z3-z0))

    orientation = (ori0, ori1, ori2)
    print("detected orientation: {}".format(orientation))
    return orientation
示例#2
0
 def test_zero(self):
     self.assertTrue(close_enough(f_kin(0, 0, 0), (164, 0, -130)))
示例#3
0
def reorient(theta1, theta2, theta3):
    ori1, ori2, ori3 = actual_orientation
    return f_kin(ori1*theta1, ori2*theta2, ori3*theta3)