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