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!)
Rotate Object to Face Origin/Point
-
- Posts: 14
- Joined: Tue Sep 18, 2018 7:50 pm
Re: Rotate Object to Face Origin/Point
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:
I hope this is of help.
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()