## LookAt function help

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

### LookAt function help

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 / position)
yangle = np.arctan(position / position)
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 + U + F

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