from pdControllerStable import PDControllerStable
import time
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0],
useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0],
useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0],
useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0],
useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole,
i,
p.POSITION_CONTROL,
targetPosition=0,
force=0)
p.setJointMotorControl2(pole2,
i,
p.POSITION_CONTROL,
targetPosition=0,
force=0)
p.setJointMotorControl2(pole3,
useGUI = True
if useGUI:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
useZUp = False
useYUp = not useZUp
if useYUp:
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p)
p.resetDebugVisualizerCamera(cameraDistance=7,
cameraYaw=-94,
cameraPitch=-14,
cameraTargetPosition=[0.31, 0.03, -1.16])
import pybullet_data
p.setTimeOut(10000)
useMotionCapture = False
useMotionCaptureReset = False #not useMotionCapture
useExplicitPD = True
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30)
import pybullet as p
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerExplicit import PDControllerExplicit
from pdControllerStable import PDControllerStable
import time
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
useGUI = True if useGUI: p.connect(p.GUI) else: p.connect(p.DIRECT) useZUp = False useYUp = not useZUp if useYUp: p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False useMotionCaptureReset=False#not useMotionCapture useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #explicit PD control requires small timestep