1import pybullet as p 2import time 3 4import pybullet_data 5 6p.connect(p.GUI) 7p.setAdditionalSearchPath(pybullet_data.getDataPath()) 8p.loadURDF("plane.urdf", [0, 0, -0.25]) 9minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) 10print(p.getNumJoints(minitaur)) 11p.resetDebugVisualizerCamera(cameraDistance=1, 12 cameraYaw=23.2, 13 cameraPitch=-6.6, 14 cameraTargetPosition=[-0.064, .621, -0.2]) 15motorJointId = 1 16p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0) 17 18p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1) 19angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0) 20jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0) 21 22textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2]) 23p.setRealTimeSimulation(1) 24while (1): 25 frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) 26 angularDamping = p.readUserDebugParameter(angularDampingSlider) 27 p.setJointMotorControl2(minitaur, 28 motorJointId, 29 p.VELOCITY_CONTROL, 30 targetVelocity=0, 31 force=frictionForce) 32 p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping) 33 34 time.sleep(0.01) 35 txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1]) 36 prevTextId = textId 37 textId = p.addUserDebugText(txt, [0, 0, -0.2]) 38 p.removeUserDebugItem(prevTextId) 39