From 0c464e6848d5fadf1893c9589377ad0dbd4028b9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 8 Feb 2017 09:27:51 -0800 Subject: [PATCH] [pybullet] add example for roll, pitch, yaw --- examples/pybullet/rollPitchYaw.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 examples/pybullet/rollPitchYaw.py diff --git a/examples/pybullet/rollPitchYaw.py b/examples/pybullet/rollPitchYaw.py new file mode 100644 index 000000000..d807fb845 --- /dev/null +++ b/examples/pybullet/rollPitchYaw.py @@ -0,0 +1,26 @@ +import pybullet as p +import time + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True) +rollId = p.addUserDebugParameter("roll",-1.5,1.5,0) +pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0) +yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0) +fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0) +fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0) +fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0) + +while True: + roll = p.readUserDebugParameter(rollId) + pitch = p.readUserDebugParameter(pitchId) + yaw = p.readUserDebugParameter(yawId) + x = p.readUserDebugParameter(fwdxId) + y = p.readUserDebugParameter(fwdyId) + z = p.readUserDebugParameter(fwdzId) + + orn = p.getQuaternionFromEuler([roll,pitch,yaw]) + p.resetBasePositionAndOrientation(q,[x,y,z],orn) + #p.stepSimulation()#not really necessary for this demo, no physics used +