In CS system, we use RPY angle (static angle) to decribe the pose and movement of robot arm, which means robot rotate along fixed X axis, along fixed Y axis, and along fixed Z axis to get the final pose data. And the transform matrix is the same with Euler ZYX ( rotate along Z axis, rotate along the new Y axis, and rotate along the new X axis)

And quaternion is another method to decribe robot pose, users could refer to related manuals.

When planning the path, it is easier to use quaternion data to make the robot arm move smoothly. The transform script from RPY to quaternion is as below:

def RPY2Quaternion(rpy_vector):
    # input parameter RPY, rad
    # return quaternion

    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):
    # input parameter quaternion [x,y,z,w]
    # return 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] #input RPY

global quat
quat = RPY2Quaternion([rx,ry,rz]) #Get quaternion data
global rpy
rpy = Quaternion2RPY(quat) #reverse to RPY