LookAt function help

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
cmon
Posts: 3
Joined: Tue May 21, 2019 5:54 pm

LookAt function help

Post by cmon »

Hi,

I have been trying to get an object to point at the centre while spawning randomly on a sphere in python. Specifically, the object is a gripper for machine learning tasks. I have also tried with a simple elongated rectangular prism.

I've implemented this code in python https://stackoverflow.com/questions/524 ... 3#52551983 as well as tried Euler angles with a restricted domain but neither works consistently.

Has anyone accomplished this? I've been trying for 2 days now and the problems I suspect comes from differing coordinate systems
in the URDF and Pybullet.

If the function works properly, perhaps it's something to add to Pybullet?

Euler angle code (works for most postions)

Code: Select all

def lookAt(position):
    zangle = np.arctan(position[1] / position[0])
    yangle = np.arctan(position[0] / position[2])
    return p.getQuaternionFromEuler([0, yangle, zangle])
Look at function translated from C (never works)

Code: Select all

def lookAt(centre, target, worldUp):
    F = (target - centre) / np.linalg.norm(target - centre)
    R = np.cross(F, worldUp) / np.linalg.norm(np.cross(F, worldUp))
    U = np.cross(R, F)

    q = Quaternion()
    trace = R[0] + U[1] + F[2]

    if trace > 0:
        s = 0.5 / np.sqrt(trace + 1.0)
        q = Quaternion(0.25 / s, (U[2] - F[1]) * s, (F[0] - R[2]) * s, (R[1] - U[0]) * s)
    else:
        if R[0] > U[1] and R[0] > F[2]:
            s = 2.0 * np.sqrt(1.0 + R[0] - U[1] - F[2])
            q = Quaternion((U[2] - F[1]) / s, 0.25 * s, (U[0] + R[1]) / s, (F[0] + R[2]) / s)
        elif U[1] > F[2]:
            s = 2.0 * np.sqrt(1.0 + U[1] - R[0] - F[2])
            q = Quaternion((F[0] - R[2]) / s, (U[0] + R[1]) / s, 0.25 * s, (F[1] + U[2]) / s)
        else:
            s = 2.0 * np.sqrt(1.0 + F[2] - R[0] - U[1])
            q = Quaternion((R[1] - U[0]) / s, (F[0] + R[2]) / s, (F[1] + U[2]) / s, 0.25 * s)
    return q.elements
Post Reply