implement body pitch function

Signed-off-by: Koen Kooi <koen@dominion.thruhere.net>