p=robot.get_cartesian_pos()
print "Pos:      ",  p
print "Cache pos:",  robot.cartesian_pos
print "Cache dest:      ",  robot.cartesian_destination


print "Points: ",  robot.get_current_points()
for segment in known_segments:
        if is_on_segment(segment):  
            print " On : " + str(segment)   + " - Dist:" + str(get_dist_to_segment(segment))



p2 = robot.get_cartesian_pos()
if arrsub( p2, p) != [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]:
    print "Pos:   ",  p2