@Test
public void testToRot_PmRpy() throws Exception {

    System.out.println("toRot");
    PmRpy rpy = new PmRpy(0.0,0.0,Math.PI/2.0);
    PmRotationVector expResult = new PmRotationVector(Math.PI/2.0,0.0,0.0,1.0);
    PmRotationVector result = Posemath.toRot(rpy);
    checkEquals("yaw",expResult,result);
}