1import pybullet as p
2import time
3#p.connect(p.UDP,"192.168.86.100")
4import pybullet_data
5
6cid = p.connect(p.SHARED_MEMORY)
7if (cid < 0):
8  p.connect(p.GUI)
9p.resetSimulation()
10
11p.setAdditionalSearchPath(pybullet_data.getDataPath())
12#disable rendering during loading makes it much faster
13p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
14objects = [
15    p.loadURDF("plane.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000)
16]
17#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
18objects = [
19    p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
20               1.000000)
21]
22pr2_gripper = objects[0]
23print("pr2_gripper=")
24print(pr2_gripper)
25
26jointPositions = [0.550569, 0.000000, 0.549657, 0.000000]
27for jointIndex in range(p.getNumJoints(pr2_gripper)):
28  p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex])
29  p.setJointMotorControl2(pr2_gripper, jointIndex, p.POSITION_CONTROL, targetPosition=0, force=0)
30
31pr2_cid = p.createConstraint(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2, 0, 0],
32                             [0.500000, 0.300006, 0.700000])
33print("pr2_cid")
34print(pr2_cid)
35
36pr2_cid2 = p.createConstraint(pr2_gripper,
37                              0,
38                              pr2_gripper,
39                              2,
40                              jointType=p.JOINT_GEAR,
41                              jointAxis=[0, 1, 0],
42                              parentFramePosition=[0, 0, 0],
43                              childFramePosition=[0, 0, 0])
44p.changeConstraint(pr2_cid2, gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3)
45
46objects = [
47    p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000, -0.200000, 0.600000, 0.000000, 0.000000,
48               0.000000, 1.000000)
49]
50kuka = objects[0]
51jointPositions = [-0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001]
52for jointIndex in range(p.getNumJoints(kuka)):
53  p.resetJointState(kuka, jointIndex, jointPositions[jointIndex])
54  p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0)
55
56objects = [
57    p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.700000, 0.000000, 0.000000, 0.000000,
58               1.000000)
59]
60objects = [
61    p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.800000, 0.000000, 0.000000, 0.000000,
62               1.000000)
63]
64objects = [
65    p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.900000, 0.000000, 0.000000, 0.000000,
66               1.000000)
67]
68objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
69kuka_gripper = objects[0]
70print("kuka gripper=")
71print(kuka_gripper)
72
73p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036],
74                                  [-0.000000, 0.964531, -0.000002, -0.263970])
75jointPositions = [
76    0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000
77]
78for jointIndex in range(p.getNumJoints(kuka_gripper)):
79  p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex])
80  p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex],
81                          0)
82
83kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05],
84                              [0, 0, 0])
85
86objects = [
87    p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
88               0.707107)
89]
90objects = [
91    p.loadURDF("jenga/jenga.urdf", 1.200000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
92               0.707107)
93]
94objects = [
95    p.loadURDF("jenga/jenga.urdf", 1.100000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
96               0.707107)
97]
98objects = [
99    p.loadURDF("jenga/jenga.urdf", 1.000000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
100               0.707107)
101]
102objects = [
103    p.loadURDF("jenga/jenga.urdf", 0.900000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
104               0.707107)
105]
106objects = [
107    p.loadURDF("jenga/jenga.urdf", 0.800000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
108               0.707107)
109]
110objects = [
111    p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107,
112               0.707107)
113]
114objects = [
115    p.loadURDF("teddy_vhacd.urdf", 1.050000, -0.500000, 0.700000, 0.000000, 0.000000, 0.707107,
116               0.707107)
117]
118objects = [
119    p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000, 0.000000, 0.707107,
120               0.707107)
121]
122objects = [
123    p.loadURDF("sphere_small.urdf", 0.850000, -0.400000, 0.700000, 0.000000, 0.000000, 0.707107,
124               0.707107)
125]
126objects = [
127    p.loadURDF("duck_vhacd.urdf", 0.850000, -0.400000, 0.900000, 0.000000, 0.000000, 0.707107,
128               0.707107)
129]
130objects = p.loadSDF("kiva_shelf/model.sdf")
131ob = objects[0]
132p.resetBasePositionAndOrientation(ob, [0.000000, 1.000000, 1.204500],
133                                  [0.000000, 0.000000, 0.000000, 1.000000])
134objects = [
135    p.loadURDF("teddy_vhacd.urdf", -0.100000, 0.600000, 0.850000, 0.000000, 0.000000, 0.000000,
136               1.000000)
137]
138objects = [
139    p.loadURDF("sphere_small.urdf", -0.100000, 0.955006, 1.169706, 0.633232, -0.000000, -0.000000,
140               0.773962)
141]
142objects = [
143    p.loadURDF("cube_small.urdf", 0.300000, 0.600000, 0.850000, 0.000000, 0.000000, 0.000000,
144               1.000000)
145]
146objects = [
147    p.loadURDF("table_square/table_square.urdf", -1.000000, 0.000000, 0.000000, 0.000000, 0.000000,
148               0.000000, 1.000000)
149]
150ob = objects[0]
151jointPositions = [0.000000]
152for jointIndex in range(p.getNumJoints(ob)):
153  p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
154
155objects = [
156    p.loadURDF("husky/husky.urdf", 2.000000, -5.000000, 1.000000, 0.000000, 0.000000, 0.000000,
157               1.000000)
158]
159ob = objects[0]
160jointPositions = [
161    0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
162    0.000000
163]
164for jointIndex in range(p.getNumJoints(ob)):
165  p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
166
167p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
168
169p.setGravity(0.000000, 0.000000, 0.000000)
170p.setGravity(0, 0, -10)
171
172##show this for 10 seconds
173#now = time.time()
174#while (time.time() < now+10):
175#	p.stepSimulation()
176p.setRealTimeSimulation(1)
177
178CONTROLLER_ID = 0
179POSITION = 1
180ORIENTATION = 2
181ANALOG = 3
182BUTTONS = 6
183
184controllerId = -1
185
186print("waiting for VR controller trigger")
187while (controllerId < 0):
188  events = p.getVREvents()
189  for e in (events):
190    if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
191      controllerId = e[CONTROLLER_ID]
192    if (e[BUTTONS][32] == p.VR_BUTTON_IS_DOWN):
193      controllerId = e[CONTROLLER_ID]
194
195print("Using controllerId=" + str(controllerId))
196
197while (1):
198
199  #keep the gripper centered/symmetric
200  b = p.getJointState(pr2_gripper, 2)[0]
201  p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
202
203  events = p.getVREvents()
204  for e in (events):
205    if e[
206        CONTROLLER_ID] == controllerId:  # To make sure we only get the value for one of the remotes
207      #sync the vr pr2 gripper with the vr controller position
208      p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
209      relPosTarget = 1 - e[ANALOG]
210      #open/close the gripper, based on analogue
211      p.changeConstraint(pr2_cid2,
212                         gearRatio=1,
213                         erp=1,
214                         relativePositionTarget=relPosTarget,
215                         maxForce=3)
216