*x1存储图像坐标x
x1:=[15,15,17,19,18.5,17]
*y1存储图像坐标y
y1:=[16,17,16.5,13,15,18]
*x2存储两轴的第一轴关节角度
x2:=[11.1011563001571, 17.6582443983142, 21.1763751936513, 5.974950644959, 17.2273723357506, 36.869897645847]
*y2存储两轴的第二轴关节角度
y2:=[58.6677485024058, 50.9498774638465, 38.0475074536199, 46.895326180104,36.1476111219164, 16.260204708312]
vector_to_hom_mat2d (x1, y1,x2,y2, HomMat2D)
for Index1 := 1 to 5 by 1
affine_trans_point_2d (HomMat2D, x1[Index1], y1[Index1], Qx, Qy)
x:= x2[Index1]-Qx
y:= y2[Index1]-Qy
stop ()
endfor
|