48 lines
1.6 KiB
Python
48 lines
1.6 KiB
Python
#a mimic joint can act as a gear between two joints
|
|
#you can control the gear ratio in magnitude and sign (>0 reverses direction)
|
|
|
|
import pybullet as p
|
|
import time
|
|
p.connect(p.GUI)
|
|
p.loadURDF("plane.urdf", 0, 0, -2)
|
|
wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
|
|
for i in range(p.getNumJoints(wheelA)):
|
|
print(p.getJointInfo(wheelA, i))
|
|
p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
|
|
|
c = p.createConstraint(wheelA,
|
|
1,
|
|
wheelA,
|
|
3,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = p.createConstraint(wheelA,
|
|
2,
|
|
wheelA,
|
|
4,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(wheelA,
|
|
1,
|
|
wheelA,
|
|
4,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
p.setRealTimeSimulation(1)
|
|
while (1):
|
|
p.setGravity(0, 0, -10)
|
|
time.sleep(0.01)
|
|
#p.removeConstraint(c)
|