Rotate Object to Face Origin/Point

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
carcamdou
Posts: 1
Joined: Wed Nov 07, 2018 12:56 pm

Rotate Object to Face Origin/Point

Post by carcamdou »

Hello All!

I'm completely new to pybullet, and I've been having some trouble figuring things out. I read through the quickstart guide, but it doesn't seem to have anything along these lines.

I am interested in loading a URDF object and having it move around a sphere centered at the origin, always facing towards the origin. I have no problems getting a random (x,y,z) coordinate to place the position of the object, but figuring out how to get the quaternion is a bit of a problem for me. At the moment, I'm using spherical coordinates for the position and using from_spherical_coords(theta, phi) from this library (https://github.com/moble/quaternion) to get the equivalent quaternion. I know this isn't right (both because it isn't working, and because it doesn't make much sense.

TLDR: If I loadURDF an object into the scene, how to do I figure out the quaternion needed to point its face at the origin (or an arbitrary point!)
richardbloemenkamp
Posts: 14
Joined: Tue Sep 18, 2018 7:50 pm

Re: Rotate Object to Face Origin/Point

Post by richardbloemenkamp »

I would do the same as you using the spherical coordinates and then transform to quaternion. I found however that with the Euler angles the order of the angle operations is important (first roll around the world-x, then yaw around the world-z, finally pitch around the world-y). In addition for the r2d2.urdf below, the x-direction is sideways, such that for the robot-pitch, I had to use the roll-angle to first rotate the robot around its y-axis and then add yaw.

Below the code I used:

Code: Select all

import pybullet as p
import time
import numpy as np
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.resetDebugVisualizerCamera( cameraDistance=2, cameraYaw=10, cameraPitch=-20, cameraTargetPosition=[0.0, 0.0, 0.25])
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)

x=1
y=0.5
r_hor=np.sqrt(x*x+y*y)  #4.12 
z=.7
rollini = np.arctan2(z,r_hor) #0.4516 rad Needs roll because urdf x-direction points sideways
phiini = np.arctan2(y,x)   #0.245 radians
thetaini = 0

roll=-rollini
phi=phiini-np.pi-np.pi/2  #-np.pi/2 because urdf x-direction points sideways
theta=thetaini
q=p.getQuaternionFromEuler((roll,theta,phi))
print(q)
p.resetBasePositionAndOrientation(boxId,(x,y,z),q)

t0=time.time()
t=time.time()
while ((t-t0)<5):
    t=time.time()
p.disconnect()
I hope this is of help.
Post Reply