mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
add example comparing explicit PD control (Python), explicit PD control (plugin), position constraint and stable PD control (Python)
This commit is contained in:
parent
69321a9ee6
commit
78de1f070f
@ -1,26 +1,49 @@
|
|||||||
|
|
||||||
import pybullet as p
|
import pybullet as p
|
||||||
|
from pdControllerExplicit import PDControllerExplicit
|
||||||
|
from pdControllerExplicit import PDControllerStable
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
|
||||||
useMaximalCoordinates=False
|
useMaximalCoordinates=False
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
|
pole = p.loadURDF("cartpole.urdf", [0,0,0], useMaximalCoordinates=useMaximalCoordinates)
|
||||||
for i in range (p.getNumJoints(pole)):
|
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 = PDControllerExplicit(p)
|
||||||
|
sPD = PDControllerStable(p)
|
||||||
|
|
||||||
|
|
||||||
|
for i in range (p.getNumJoints(pole2)):
|
||||||
#disable default constraint-based motors
|
#disable default constraint-based motors
|
||||||
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
|
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
|
||||||
print("joint",i,"=",p.getJointInfo(pole,i))
|
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)
|
desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
|
||||||
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
|
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
|
||||||
kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
|
kpCartId = p.addUserDebugParameter("kpCart",0,500,1300)
|
||||||
kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
|
kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
|
||||||
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
|
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
|
||||||
|
|
||||||
|
textColor = [1,1,1]
|
||||||
|
shift = 0.05
|
||||||
|
p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,parentLinkIndex=1)
|
||||||
|
p.addUserDebugText("explicit PD plugin", [shift,0,-.1],textColor,parentObjectUniqueId=pole2,parentLinkIndex=1)
|
||||||
|
p.addUserDebugText("stablePD", [shift,0,.1],textColor,parentObjectUniqueId=pole4,parentLinkIndex=1)
|
||||||
|
p.addUserDebugText("position constraint", [shift,0,-.1],textColor,parentObjectUniqueId=pole3,parentLinkIndex=1)
|
||||||
|
|
||||||
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
|
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
|
||||||
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
|
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
|
||||||
kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
|
kpPoleId = p.addUserDebugParameter("kpPole",0,500,1200)
|
||||||
kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
|
kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
|
||||||
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
|
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
|
||||||
|
|
||||||
@ -29,34 +52,50 @@ pd = p.loadPlugin("pdControlPlugin")
|
|||||||
|
|
||||||
p.setGravity(0,0,-10)
|
p.setGravity(0,0,-10)
|
||||||
|
|
||||||
useRealTimeSim = True
|
useRealTimeSim = False
|
||||||
|
|
||||||
p.setRealTimeSimulation(useRealTimeSim)
|
p.setRealTimeSimulation(useRealTimeSim)
|
||||||
|
|
||||||
p.setTimeStep(0.001)
|
timeStep = 0.001
|
||||||
|
|
||||||
|
|
||||||
while p.isConnected():
|
while p.isConnected():
|
||||||
if (pd>=0):
|
p.getCameraImage(320,200)
|
||||||
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
|
timeStep = p.readUserDebugParameter(timeStepId)
|
||||||
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
|
p.setTimeStep(timeStep)
|
||||||
kpCart = p.readUserDebugParameter(kpCartId)
|
|
||||||
kdCart = p.readUserDebugParameter(kdCartId)
|
|
||||||
maxForceCart = p.readUserDebugParameter(maxForceCartId)
|
|
||||||
link = 0
|
|
||||||
p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart)
|
|
||||||
|
|
||||||
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
|
|
||||||
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
|
|
||||||
kpPole = p.readUserDebugParameter(kpPoleId)
|
|
||||||
kdPole = p.readUserDebugParameter(kdPoleId)
|
|
||||||
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
|
|
||||||
link = 1
|
|
||||||
p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
|
|
||||||
|
|
||||||
|
|
||||||
|
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
|
||||||
|
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
|
||||||
|
kpCart = p.readUserDebugParameter(kpCartId)
|
||||||
|
kdCart = p.readUserDebugParameter(kdCartId)
|
||||||
|
maxForceCart = p.readUserDebugParameter(maxForceCartId)
|
||||||
|
|
||||||
|
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
|
||||||
|
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
|
||||||
|
kpPole = p.readUserDebugParameter(kpPoleId)
|
||||||
|
kdPole = p.readUserDebugParameter(kdPoleId)
|
||||||
|
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
|
||||||
|
|
||||||
|
taus = exPD.computePD(pole, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
|
||||||
|
p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
|
||||||
|
|
||||||
|
if (pd>=0):
|
||||||
|
link = 0
|
||||||
|
p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart)
|
||||||
|
link = 1
|
||||||
|
p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
taus = sPD.computePD(pole4, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
|
||||||
|
p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
|
||||||
|
|
||||||
|
p.setJointMotorControl2(pole3,0, p.POSITION_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, positionGain=timeStep*(kpCart/150.), velocityGain=0.5, force=maxForceCart)
|
||||||
|
p.setJointMotorControl2(pole3,1, p.POSITION_CONTROL, targetPosition=desiredPosPole, targetVelocity=desiredVelPole, positionGain=timeStep*(kpPole/150.), velocityGain=0.5, force=maxForcePole)
|
||||||
|
|
||||||
if (not useRealTimeSim):
|
if (not useRealTimeSim):
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
time.sleep(1./240.)
|
time.sleep(timeStep)
|
||||||
|
|
||||||
|
|
||||||
|
67
examples/pybullet/examples/pdControllerExplicit.py
Normal file
67
examples/pybullet/examples/pdControllerExplicit.py
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
import numpy as np
|
||||||
|
|
||||||
|
class PDControllerExplicit(object):
|
||||||
|
def __init__(self, pb):
|
||||||
|
self._pb = pb
|
||||||
|
|
||||||
|
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
|
||||||
|
numJoints = self._pb.getNumJoints(bodyUniqueId)
|
||||||
|
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
|
||||||
|
q1 = []
|
||||||
|
qdot1 = []
|
||||||
|
for i in range (numJoints):
|
||||||
|
q1.append(jointStates[i][0])
|
||||||
|
qdot1.append(jointStates[i][1])
|
||||||
|
q = np.array(q1)
|
||||||
|
qdot=np.array(qdot1)
|
||||||
|
qdes = np.array(desiredPositions)
|
||||||
|
qdotdes = np.array(desiredVelocities)
|
||||||
|
qError = qdes - q
|
||||||
|
qdotError = qdotdes - qdot
|
||||||
|
Kp = np.diagflat(kps)
|
||||||
|
Kd = np.diagflat(kds)
|
||||||
|
forces = Kp.dot(qError) + Kd.dot(qdotError)
|
||||||
|
maxF = np.array(maxForces)
|
||||||
|
forces = np.clip(forces, -maxF , maxF )
|
||||||
|
return forces
|
||||||
|
|
||||||
|
|
||||||
|
class PDControllerStable(object):
|
||||||
|
def __init__(self, pb):
|
||||||
|
self._pb = pb
|
||||||
|
|
||||||
|
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
|
||||||
|
numJoints = self._pb.getNumJoints(bodyUniqueId)
|
||||||
|
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
|
||||||
|
q1 = []
|
||||||
|
qdot1 = []
|
||||||
|
zeroAccelerations = []
|
||||||
|
for i in range (numJoints):
|
||||||
|
q1.append(jointStates[i][0])
|
||||||
|
qdot1.append(jointStates[i][1])
|
||||||
|
zeroAccelerations.append(0)
|
||||||
|
q = np.array(q1)
|
||||||
|
qdot=np.array(qdot1)
|
||||||
|
qdes = np.array(desiredPositions)
|
||||||
|
qdotdes = np.array(desiredVelocities)
|
||||||
|
qError = qdes - q
|
||||||
|
qdotError = qdotdes - qdot
|
||||||
|
Kp = np.diagflat(kps)
|
||||||
|
Kd = np.diagflat(kds)
|
||||||
|
p = Kp.dot(qError)
|
||||||
|
d = Kd.dot(qdotError)
|
||||||
|
forces = p + d
|
||||||
|
|
||||||
|
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
|
||||||
|
M2 = np.array(M1)
|
||||||
|
M = (M2 + Kd * timeStep)
|
||||||
|
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
|
||||||
|
c = np.array(c1)
|
||||||
|
A = M
|
||||||
|
b = -c + p + d
|
||||||
|
qddot = np.linalg.solve(A, b)
|
||||||
|
tau = p + d - Kd.dot(qddot) * timeStep
|
||||||
|
maxF = np.array(maxForces)
|
||||||
|
forces = np.clip(tau, -maxF , maxF )
|
||||||
|
#print("c=",c)
|
||||||
|
return tau
|
Loading…
Reference in New Issue
Block a user