EC机器人RPY与旋转向量转换代码实现

-- author: chenliao@elibot.cn
-- Aug.21,2023
function rpy_to_quaternion(roll, pitch, yaw)
    -- 输入RPY 弧度，返回四元数
    local cy = math.cos(yaw * 0.5)
    local sy = math.sin(yaw * 0.5)
    local cp = math.cos(pitch * 0.5)
    local sp = math.sin(pitch * 0.5)
    local cr = math.cos(roll * 0.5)
    local sr = math.sin(roll * 0.5)

    local qx = sr * cp * cy - cr * sp * sy
    local qy = cr * sp * cy + sr * cp * sy
    local qz = cr * cp * sy - sr * sp * cy
    local qw = cr * cp * cy + sr * sp * sy

    return {qx, qy, qz, qw}
end

function rpy2rotvec(roll,pitch,yaw)
    -- 输入为rx，ry，rz的弧度，返回旋转向量
    local q = rpy_to_quaternion(roll,pitch,yaw)
    local theta = 2 * math.acos(q[4]);
    local r1 = q[1] / math.sin(theta * 0.5) * theta
    local r2 = q[2] / math.sin(theta * 0.5) * theta
    local r3 = q[3 ]/ math.sin(theta * 0.5) * theta
    return {r1,r2,r3}
end

function quaternion_to_rpy(qx, qy, qz, qw)
    -- 输入四元数，返回RPY 弧度
    local roll = math.atan(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy))
    local pitch = math.asin(2 * (qw * qy - qx * qz))
    local yaw = math.atan(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz))

    return {roll, pitch, yaw}
end

function rotvec2rpy(r1,r2,r3)
    -- 输入旋转向量，返回RPY弧度
    local theta = math.sqrt(r1 * r1 + r2 * r2 + r3 * r3)
    local qw    = math.cos(theta * 0.5);
    local qx    = r1 * math.sin(theta * 0.5) / theta
    local qy    = r2 * math.sin(theta * 0.5) / theta
    local qz    = r3 * math.sin(theta * 0.5) / theta
    local RPY = quaternion_to_rpy(qx,qy,qz,qw)
    return RPY
end
-- 示例使用
-- 以下为RPY：rx，ry和rz的弧度
local roll = 0.1
local pitch = 0.2
local yaw = 0.3

vec = rpy2rotvec(roll,pitch,yaw) --得到旋转向量
print(table.concat(vec,','))
RPY = rotvec2rpy(vec[1],vec[2],vec[3]) --得到RPY 弧度
print(table.concat(RPY,','))