def find_twist_frame(hexapod, unit_coxia_vector): twist = angle_between(unit_coxia_vector, hexapod.x_axis) is_ccw = is_counter_clockwise(unit_coxia_vector, hexapod.x_axis, hexapod.z_axis) if is_ccw: twist = -twist twist_frame = rotz(twist) return twist, twist_frame
def find_twist_to_recompute_hexapod(a, b): twist = angle_between(a, b) z_axis = Point(0, 0, -1) is_ccw = is_counter_clockwise(a, b, z_axis) if is_ccw: twist = -twist twist_frame = rotz(twist) return twist, twist_frame