1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
|
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)
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)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
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)
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7
taus = exPD.computePD(pole, [0, 1], [
basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
desiredPosCart, desiredPosPole
], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
[0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
[0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole,
j,
controlMode=p.TORQUE_CONTROL,
force=[taus[j + baseDof]])
#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)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
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):
p.stepSimulation()
time.sleep(timeStep)
|
