diff --git a/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py b/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py new file mode 100644 index 000000000..343115448 --- /dev/null +++ b/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py @@ -0,0 +1,230 @@ +import pybullet as p +import time +#p.connect(p.UDP,"192.168.86.100") + + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +p.resetSimulation() + + + + +meshScale = [.1,.1,.01] +shift = [0,0,0] + +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="marble_cube.obj", rgbaColor=[1,1,1,1], specularColor=[1,1,1], visualFramePosition=shift, meshScale=meshScale) +#collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale) +collisionShapeId = -1 +uiCube = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,1,0], useMaximalCoordinates=True) + +textOrn = p.getQuaternionFromEuler([0,0,-1.5707963]) +numLines = 1 +lines = [-1]*numLines + +p.stepSimulation() +#disable rendering during loading makes it much faster +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) +#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +kitchenObj = p.loadSDF("kitchens/1.sdf") +#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] +pr2_gripper = objects[0] +print ("pr2_gripper=") +print (pr2_gripper) + +jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] +for jointIndex in range (p.getNumJoints(pr2_gripper)): + p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0) + +pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) +print ("pr2_cid") +print (pr2_cid) + +pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3) + + + +objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] +kuka = objects[0] +jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] +for jointIndex in range (p.getNumJoints(kuka)): + p.resetJointState(kuka,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) + +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] +objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") +kuka_gripper = objects[0] +print ("kuka gripper=") +print(kuka_gripper) + +p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) +jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] +for jointIndex in range (p.getNumJoints(kuka_gripper)): + p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) + + +kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) + +objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] +#objects = [p.loadURDF("textures/table2.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] + + + +objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] + +#objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)] +#bjects = p.loadSDF("kiva_shelf/model.sdf") +#ob = objects[0] +#p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000]) +#objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] +#objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)] +#objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] +#objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +#ob = objects[0] +#jointPositions=[ 0.000000 ] +#for jointIndex in range (p.getNumJoints(ob)): +# p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) + +#objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)] +#ob = objects[0] +#jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ] +#for jointIndex in range (p.getNumJoints(ob)): +# p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) + + +p.setGravity(0,0,-10) + +##show this for 10 seconds +#now = time.time() +#while (time.time() < now+10): +# p.stepSimulation() +p.setRealTimeSimulation(1) + +CONTROLLER_ID = 0 +POSITION=1 +ORIENTATION=2 +ANALOG=3 +BUTTONS=6 + +uiControllerId = -1 + +print("waiting for VR UI controller trigger") +while (uiControllerId<0): + events = p.getVREvents() + for e in (events): + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + uiControllerId = e[CONTROLLER_ID] + if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN): + uiControllerId = e[CONTROLLER_ID] + +print("Using uiControllerId="+str(uiControllerId)) + + + +controllerId = -1 + +print("waiting for VR picking controller trigger") +while (controllerId<0): + events = p.getVREvents() + for e in (events): + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + if (controllerId == uiControllerId): + controllerId = -1 + +print("Using controllerId="+str(controllerId)) + +once = 1 + +if (once): + logId = -1#p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "userDebugItems1.json") + print("logId userdebug") + print(logId) + + for i in range (numLines): + spacing = 0.01 + textPos = [.1-(i+1)*spacing,.1,0.011] + text = "ABCDEFGH\nIJKLMNOPQRSTUVWXYZ\n01234567890abcdefg\n"*10 + lines[i] = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.01, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1) + + if (once): + once = 0 + if (logId and logId>0): + p.stopStateLogging(logId) + +frameNr = 0 +objectInfo = "" +pointRay = -1 +rayLen = 100 +while (1): + + frameNr=frameNr+1 + + + for i in range (numLines): + spacing = 0.01 + textPos = [.1-(i+1)*spacing,.1,0.011] + text = "Frame:"+str(frameNr)+"\nObject UID:"+objectInfo + textUid = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.02, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1, replaceItemUniqueId = lines[i]) + lines[i] = textUid + + + #keep the gripper centered/symmetric + b = p.getJointState(pr2_gripper,2)[0] + p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) + + events = p.getVREvents() + for e in (events): + if e[CONTROLLER_ID] == uiControllerId: + p.resetBasePositionAndOrientation(uiCube,e[POSITION], e[ORIENTATION]) + pos = e[POSITION] + orn = e[ORIENTATION] + lineFrom = pos + mat = p.getMatrixFromQuaternion(orn) + dir = [mat[0],mat[3],mat[6]] + to = [pos[0]+dir[0]*rayLen,pos[1]+dir[1]*rayLen,pos[2]+dir[2]*rayLen] + hit = p.rayTest(lineFrom,to) + oldRay = pointRay + color = [1,1,0] + width = 3 + #pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1) + #if (oldRay>=0): + # p.removeUserDebugItem(oldRay) + + if (hit): + #if (hit[0][0]>=0): + hitObjectUid = hit[0][0] + linkIndex = hit[0][1] + if (hitObjectUid>=0): + objectInfo = str(hitObjectUid)+" Link Index="+str(linkIndex)+"\nBase Name:"+p.getBodyInfo(hitObjectUid)[0].decode()+"\nBody Info:"+p.getBodyInfo(hitObjectUid)[1].decode() + else: + objectInfo="None" + + if e[CONTROLLER_ID] == controllerId: # To make sure we only get the value for one of the remotes + #sync the vr pr2 gripper with the vr controller position + p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) + relPosTarget = 1 - e[ANALOG] + #open/close the gripper, based on analogue + p.changeConstraint(pr2_cid2,gearRatio=1, erp=1, relativePositionTarget=relPosTarget, maxForce=3) + + + \ No newline at end of file diff --git a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py index 0085e52e4..4465a5324 100644 --- a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py +++ b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py @@ -117,8 +117,8 @@ while (controllerId<0): print("Using controllerId="+str(controllerId)) -plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin") -#plugin = p.loadPlugin("vrSyncPlugin") +#plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin") +plugin = p.loadPlugin("vrSyncPlugin") print("PluginId="+str(plugin)) p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3]) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 2fef56aa8..69fe0abe7 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -156,7 +156,7 @@ if not _OPTIONS["no-enet"] then "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", } -if (_OPTIONS["enable_static_plugins"]) then +if (_OPTIONS["enable_static_vr_plugin"]) then files {"../../examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp"} end