请编写一个机器人末端执行器走三角形的程序，要求机器人从起始点先到安全点，然后到第一个点，接着画三角形，最后回到起始点。