CS机器人使用RPY（静态角，即先绕固定坐标系的x轴转，再绕原有固定坐标系的y轴转，最后绕原有固定坐标系的z轴转)

RPY(rx,ry,rz)中的数据 等同于 欧拉角ZYX（rz,ry,rx)中的数据  （欧拉角ZYX表示先绕坐标系的z轴转，再绕新坐标系的y轴转，最后再绕新坐标系的x轴转)

可以使用以下脚本函数，实现RPY角与四元数的转换

def RPY2Quaternion(rpy_vector):
    # 输入RPY，单位弧度
    # 返回四元数

    roll = rpy_vector[0]
    pitch = rpy_vector[1]
    yaw = rpy_vector[2]

    cosRoll =cos(roll*0.5)
    sinRoll =sin(roll*0.5)

    cosPitch  =cos(pitch*0.5)
    sinPitch =sin(pitch*0.5)

    cosHeading =cos(yaw*0.5)
    sinHeading =sin(yaw*0.5)

    q0 =cosRoll*cosPitch*cosHeading+sinRoll*sinPitch*sinHeading
    q1 =sinRoll*cosPitch*cosHeading-cosRoll*sinPitch*sinHeading
    q2 =cosRoll*sinPitch*cosHeading+sinRoll*cosPitch*sinHeading
    q3 =cosRoll*cosPitch*sinHeading-sinRoll*sinPitch*cosHeading

    # [x,y,z,w]
    return [q0,q1,q2,q3]


def Quaternion2RPY(Quaternion):
    # 输入四元数[x,y,z,w]
    # 返回对应RPY

    q0 = Quaternion[0]
    q1 = Quaternion[1]
    q2 = Quaternion[2]
    q3 = Quaternion[3]

    roll=atan2(2*(q2*q3+q0*q1),q0*q0-q1*q1-q2*q2+q3*q3);
    pitch=asin(2*(q0*q2-q1*q3));
    yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3);

    return [roll,pitch,yaw]


rx = d2r(-88.7011)
ry = d2r(-9.89776)
rz = d2r(172.484)
global rpyinput
rpyinput = [rx,ry,rz] #输入RPY角

global quat
quat = RPY2Quaternion([rx,ry,rz]) #计算得到对应的四元数

global rpy
rpy = Quaternion2RPY(quat) #再将四元数转会RPY角