【Coppeliasim仿真】 坐标系间平滑插补

在仿真环境中控制两个参考框架（ReferenceFrame1和ReferenceFrame2）之间进行平滑的插值运动。在两个参考框架之间插值运动的过程中，使用了两种不同的方法，通过设置useMethodNb来选择使用的方法。

方法1使用了旋转轴和角度的计算，然后逐步移动参考框架1到参考框架2的姿态。在每一步，都计算了旋转轴绕着旋转中心旋转一定角度后的新的变换矩阵，并更新了参考框架1的姿态。

方法2则直接使用了sim.interpolateMatrices函数，该函数用于在两个矩阵之间进行插值。在每一步，都计算了两个参考框架之间的插值矩阵，并将其应用于参考框架1，实现了平滑的姿态过渡。

总体来说，该脚本的目的是演示如何在仿真中使用不同的方法控制两个参考框架之间的插值运动，以实现平滑的姿态过渡效果。

-- 引入sim库
sim = require'sim'

-- 线程函数，在仿真中会以线程的方式运行
function sysCall_thread()
    -- 你可以使用两种方法在两个参考框架之间进行插值：

    -- 获取参考框架的句柄
    frame1 = sim.getObject('/ReferenceFrame1')
    frame2 = sim.getObject('/ReferenceFrame2')

    -- 获取参考框架的变换矩阵
    matrix1 = sim.getObjectMatrix(frame1)
    matrix2 = sim.getObjectMatrix(frame2)

    -- 选择使用的方法编号（1或2）
    useMethodNb = 2

    if useMethodNb == 1 then
        -- 方法1
        -- 获取旋转轴和角度
        rotAxis, angle = sim.getRotationAxis(matrix1, matrix2)
        -- 获取参考框架1的位置
        pos1 = sim.getObjectPosition(frame1)
        -- 获取参考框架2的位置
        pos2 = sim.getObjectPosition(frame2)
        -- 计算位置向量
        posVector = {pos2[1] - pos1[1], pos2[2] - pos1[2], pos2[3] - pos1[3]}

        -- 将参考框架1移动到参考框架2，分为200步：
        stepCnt = 200
        dAngle = angle / stepCnt--角度增量
        dPos = {posVector[1] / stepCnt, posVector[2] / stepCnt, posVector[3] / stepCnt}--位置增量
        for i = 1, stepCnt, 1 do
            -- 处理旋转：
			-- matrix1是原始的变换矩阵，表示参考框架1的姿态（包括旋转和平移）。
			-- rotAxis是旋转轴，由sim.getRotationAxis(matrix1, matrix2)获得。
			-- {0, 0, 0}表示旋转轴的旋转中心为参考框架1的原点。
			-- dAngle * i表示旋转的角度，其中dAngle是计算得到的每步的旋转角度，而i表示当前是第几步。
			-- sim.rotateAroundAxis函数的作用是根据给定的旋转轴、旋转中心和旋转角度，计算新的矩阵，即matrixOut。
			-- 这个新的矩阵表示了在参考框架1上绕指定轴旋转了一定角度后的姿态。
            matrixOut = sim.rotateAroundAxis(matrix1, rotAxis, {0, 0, 0}, dAngle * i)
            -- 处理位置：
            matrixOut[4] = pos1[1] + dPos[1] * i
            matrixOut[8] = pos1[2] + dPos[2] * i
            matrixOut[12] = pos1[3] + dPos[3] * i
            -- 将新矩阵应用于参考框架1：
            sim.setObjectMatrix(frame1, matrixOut)
            -- 等待下一仿真步骤：
            sim.step()
        end
    else
        -- 方法2
        -- 将参考框架1移动到参考框架2，分为200步：
        stepCnt = 200
        for i = 1, stepCnt, 1 do
            matrixOut = sim.interpolateMatrices(matrix1, matrix2, i / stepCnt)
            -- 将新矩阵应用于参考框架1：
            sim.setObjectMatrix(frame1, matrixOut)
            -- 等待下一仿真步骤：
            sim.step()
        end
    end
end