PyBullet+PhysX backend: expose getJointState reading link position/velocity
This commit is contained in:
@@ -2,8 +2,12 @@ import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.PhysX)#GUI)
|
||||
p.loadPlugin("eglRendererPlugin")
|
||||
usePhysX = True
|
||||
if usePhysX:
|
||||
p.connect(p.PhysX)
|
||||
p.loadPlugin("eglRendererPlugin")
|
||||
else:
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
|
||||
@@ -18,9 +22,11 @@ print("numJoints = ", p.getNumJoints(door))
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
position_control = True
|
||||
|
||||
angle = math.pi*0.25
|
||||
p.resetJointState(door,1,angle)
|
||||
|
||||
angleread = p.getJointState(door,1)
|
||||
print("angleread = ",angleread)
|
||||
prevTime = time.time()
|
||||
|
||||
angle = math.pi*0.5
|
||||
@@ -40,4 +46,4 @@ while (1):
|
||||
else:
|
||||
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
|
||||
p.stepSimulation()
|
||||
time.sleep(1./240.)
|
||||
time.sleep(1./240.)
|
||||
|
||||
Reference in New Issue
Block a user