Page 1 of 1

how to determine rotational velocity relative to direction robot is "facing"

Posted: Thu May 31, 2018 6:25 pm
by keithmgould
This feels like a common question but I could not find it...

Generally: I'm looking to know rotational velocity relative to world orientation.

Specifically: I have a "balancing robot", (a stick on two wheels), where the stick is the base link. I want to know how fast it's falling "forward", and "forward" is of course relative to the direction the robot is facing.

I tried: `getBaseVelocity` but I believe this returns the velocity relative to the world frame, not the direction the robot is "facing"
I tried: `getLinkState` but I believe that does not work for '-1' as linkID, so I can't use it.

Do I need to do something like this?:
1. determine the orientation of the base via `getBasePositionAndOrientation`
2. determine the rotational velocity of base (relative to world)
3. some sort of matrix transform here to calculate the base's rotational velocity relative to the direction its facing

Is there an example anywhere?

Thank you!

PS: I found this example, but it feels overly complicated. ...

Re: how to determine rotational velocity relative to direction robot is "facing"

Posted: Thu May 31, 2018 10:07 pm
by keithmgould
Believe I've figured it out.

My guess above is correct, or at least, it works, though it might not be the best or most "pybullet/bullet" way to do things.

Below is the code I used to verify. I take R2D2, rotate him about the Z axis a bit, and set him spinning "forward."

At this point, what I'd want to see for his relative rotational velocity is some mostly constant value for wx, and numbers close to 0 for wy and wz.

The below accomplishes this, and can be seen when looking at the results of the print statement.

Code: Select all

import pybullet as pb
import numpy as np
import time
import pybullet_data
import pdb
physicsClient = pb.connect(pb.GUI)#or pb.DIRECT for non-graphical version
pb.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
planeId = pb.loadURDF("plane.urdf")
r2d2StartPos = [0,0,1] # elevated

# originally facing green. So Z with no rotation points along green (Y)
# points R2 diagonally (via yaw rotation around Z)
r2d2StartOrientation = pb.getQuaternionFromEuler([0,0,0.785398163397448])

# load the robot
r2d2Id = pb.loadURDF("r2d2.urdf",r2d2StartPos, r2d2StartOrientation)

# set rotational velocity same direction as robot is facing.
pb.resetBaseVelocity(1,(0,0,0),(-1, -1,0))

# goal is to see rot velocity relative to R2's orientation
# so X should be non-zero, and Y,Z both at zero.

for i in range (10000):
	r2d2Vel = pb.getBaseVelocity(r2d2Id)
	r2d2Pos, r2d2Orn = pb.getBasePositionAndOrientation(r2d2Id)
	r2d2ornEuler = pb.getEulerFromQuaternion(r2d2Orn)

	yaw = r2d2ornEuler[2]

	# I noticed this first in the link mentioned in my first post.
	# this is a "basic rotation around Z
	# see "Basic Rotations"
	rot_around_z = np.array(
		[[np.cos(-yaw), -np.sin(-yaw), 0],
		[np.sin(-yaw), np.cos(-yaw), 0],
		[		0,			 0, 1]]

	# once we have that, we want to see what the world-frame rotations
	# are translated via rot_around_z
	wx, wy, wz =, r2d2Vel[1])

	print(wx, wy, wz)