六足动物Tcl旋转脚本

Question

我是TCL的新手，正在为编写使用六脚架（6-度自由机器人）执行正弦旋转（在Z轴上）的代码而苦苦挣扎。问题是，确实，我的六脚架执行了0.03度的旋转，但是当我要绘制旋转运动的时间序列时，我看到六脚架没有做完整的振幅。

set TimeOut 10 
set code 0 

OpenConnection $TimeOut socketID 
if {$socketID == -1} { 
    puts stdout "OpenConnection failed => $socketID" 
    return 
} 


# Main

set T0 5000; #Time delay
set pi [expr 2*asin(1.0)]; #Pi
set period 20; #period is 20 second
set delta 0.2; #Time interval
set amplitude 0.03; #Amplitude(degree)
set step [expr $period/$delta];

HexapodMoveAbsolute $socketID HEXAPOD Work 0 0 -12 0 0 $amplitude; # Position(mm,degree) 
after $T0

for {set i0 0} {$i0<4} {incr i0} {
    for {set i 0} {$i<$step} {incr i} {
        set displacement [expr $amplitude*cos(2*$pi*($i*$delta+$delta)/$period)-$amplitude*cos(2*$pi*$i*$delta/$period)]
        set velocity [expr $amplitude*abs($displacement/$delta)]
        HexapodMoveIncrementalControlWithTargetVelocity $socketID HEXAPOD Work Rotation 0 0 $displacement $velocity
    }
}


# Close TCP socket 
TCP_CloseSocket $socketID 

我期望输出0.03度的幅度，但是得到的是[+0.03, -0.01]度。我会很感激为什么我得到此输出的任何建议。