PyBullet.changeDynamics: expose jointDamping
PyBullet: Implement a few more APIs of PhysX backend, resetJointState and setJointMotorControl2 allow useMaximalCoordinate=True for PhysX loadURDF (only for single rigid bodies, articulations require reduced coordinates at the moment)
This commit is contained in:
@@ -1,14 +1,43 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.PhysX)
|
||||
p.connect(p.PhysX)#GUI)
|
||||
p.loadPlugin("eglRendererPlugin")
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
for i in range (50):
|
||||
p.loadURDF("r2d2.urdf",[0,0,1+i*2])
|
||||
|
||||
for i in range (10):
|
||||
sphere = p.loadURDF("marble_cube.urdf",[0,-1,1+i*1], useMaximalCoordinates=False)
|
||||
p.changeDynamics(sphere ,-1, mass=1000)
|
||||
|
||||
door = p.loadURDF("door.urdf",[0,0,1])
|
||||
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
|
||||
print("numJoints = ", p.getNumJoints(door))
|
||||
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
position_control = True
|
||||
angle = math.pi*0.25
|
||||
p.resetJointState(door,1,angle)
|
||||
|
||||
prevTime = time.time()
|
||||
|
||||
angle = math.pi*0.5
|
||||
|
||||
while (1):
|
||||
p.stepSimulation()
|
||||
time.sleep(1./240.)
|
||||
|
||||
curTime = time.time()
|
||||
|
||||
diff = curTime - prevTime
|
||||
#every second, swap target angle
|
||||
if (diff>1):
|
||||
angle = - angle
|
||||
prevTime = curTime
|
||||
|
||||
if position_control:
|
||||
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001)
|
||||
else:
|
||||
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
|
||||
p.stepSimulation()
|
||||
time.sleep(1./240.)
|
||||
Reference in New Issue
Block a user