p.getEulerFromQuaternion() implementation

[Tengoles]

Hello! I'm trying to implement the conversion made by the python binding method p.getEulerFromQuaternion() in pytorch and I'm not quite sure what is the right code I need to follow. I think it is this one: https://github.com/bulletphysics/bullet3/blob/101c98cfb8fd297ebae6007fd10619f74c4a9748/src/LinearMath/btQuaternion.h#L161

Would love a confirmation. Thanks!

[ManuCorrea]

Hello Enzo!

I don't have much experience with the backend of the library but I would tell you so.

Also as stated in the README, for support questions go to the Bullet forum https://pybullet.org/Bullet/phpBB3/

[erwincoumans]

Here is the implementation, in pybullet.c:

/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
                                                 PyObject* args, PyObject* keywds)
{
    double squ;
    double sqx;
    double sqy;
    double sqz;

    double quat[4];

    PyObject* quatObj;

    int physicsClientId = 0;

    static char* kwlist[] = {"quaternion", "physicsClientId", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|i", kwlist, &quatObj, &physicsClientId))
    {
        return NULL;
    }

    if (quatObj)
    {
        PyObject* seq;
        int len, i;
        seq = PySequence_Fast(quatObj, "expected a sequence");
        len = PySequence_Size(quatObj);
        if (len == 4)
        {
            for (i = 0; i < 4; i++)
            {
                quat[i] = pybullet_internalGetFloatFromSequence(seq, i);
            }
        }
        else
        {
            PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
            Py_DECREF(seq);
            return NULL;
        }
        Py_DECREF(seq);
    }
    else
    {
        PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
        return NULL;
    }

    {
        double rpy[3];
        double sarg;
        sqx = quat[0] * quat[0];
        sqy = quat[1] * quat[1];
        sqz = quat[2] * quat[2];
        squ = quat[3] * quat[3];
        sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);

        // If the pitch angle is PI/2 or -PI/2, we can only compute
        // the sum roll + yaw.  However, any combination that gives
        // the right sum will produce the correct orientation, so we
        // set rollX = 0 and compute yawZ.
        if (sarg <= -0.99999)
        {
            rpy[0] = 0;
            rpy[1] = -0.5 * PYBULLET_PI;
            rpy[2] = 2 * atan2(quat[0], -quat[1]);
        }
        else if (sarg >= 0.99999)
        {
            rpy[0] = 0;
            rpy[1] = 0.5 * PYBULLET_PI;
            rpy[2] = 2 * atan2(-quat[0], quat[1]);
        }
        else
        {
            rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), squ - sqx - sqy + sqz);
            rpy[1] = asin(sarg);
            rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), squ + sqx - sqy - sqz);
        }
        {
            PyObject* pylist;
            int i;
            pylist = PyTuple_New(3);
            for (i = 0; i < 3; i++)
                PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i]));
            return pylist;
        }
    }
    Py_INCREF(Py_None);
    return Py_None;
}