yaw_vel = (ps4.axis_raw[2] - 128) / 128.0
pitch_vel = ((255 - ps4.axis_raw[3]) - 128) / 128.0
ud_normal = cross(cross(arm.vectors[1], arm.vectors[2]), arm.vectors[2])
rl_normal = cross(arm.vectors[2], [0, 0, 1])
pitch_normal = cross(arm.vectors[2], arm.vectors[1])
nav.up_down([0, 0, 1], up_down_vel)
nav.rigth_left(rl_normal, rl_vel)
nav.zoom(zoom_vel)
nav.rotate_yaw(ud_normal, yaw_vel)
nav.rotate_pitch(pitch_normal, pitch_vel)
# inp = int(raw_input("zoom?"))
# nav.zoom(inp)
p = nav.position
s = nav.vector
arm.update_destination_point(p, s)
lines = to_lines(arm.joint_points)
# g.redraw_point(arm.joint_points[1])
g.redraw(lines)
g = Grapher(lines)
t_point1 = [50, 10, 40]
t_point2 = [70, 5, 30]
g.redraw_point(t_point1)
g.redraw_point(t_point2)
g.redraw(lines)
g.show(anim)
