import pybullet as p
#p.connect(p.UDP,"192.168.86.100")
p.connect(p.SHARED_MEMORY)
p.resetSimulation()

objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
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])

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)

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("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("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,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)]
objects = 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.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)

p.stepSimulation()

p.disconnect()

[((56, 82), 'pybullet.connect', 'p.connect', (['p.SHARED_MEMORY'], {}), '(p.SHARED_MEMORY)\n', (65, 82), True, 'import pybullet as p\n'), ((83, 102), 'pybullet.resetSimulation', 'p.resetSimulation', ([], {}), '()\n', (100, 102), True, 'import pybullet as p\n'), ((681, 789), 'pybullet.createConstraint', 'p.createConstraint', (['pr2_gripper', '(-1)', '(-1)', '(-1)', 'p.JOINT_FIXED', '[0, 0, 0]', '[0.2, 0, 0]', '[0.5, 0.300006, 0.7]'], {}), '(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2,\n    0, 0], [0.5, 0.300006, 0.7])\n', (699, 789), True, 'import pybullet as p\n'), ((1579, 1641), 'pybullet.loadSDF', 'p.loadSDF', (['"""gripper/wsg50_one_motor_gripper_new_free_base.sdf"""'], {}), "('gripper/wsg50_one_motor_gripper_new_free_base.sdf')\n", (1588, 1641), True, 'import pybullet as p\n'), ((1713, 1828), 'pybullet.resetBasePositionAndOrientation', 'p.resetBasePositionAndOrientation', (['kuka_gripper', '[0.923103, -0.2, 1.250036]', '[-0.0, 0.964531, -2e-06, -0.26397]'], {}), '(kuka_gripper, [0.923103, -0.2, 1.250036],\n    [-0.0, 0.964531, -2e-06, -0.26397])\n', (1746, 1828), True, 'import pybullet as p\n'), ((2172, 2272), 'pybullet.createConstraint', 'p.createConstraint', (['kuka', '(6)', 'kuka_gripper', '(0)', 'p.JOINT_FIXED', '[0, 0, 0]', '[0, 0, 0.05]', '[0, 0, 0]'], {}), '(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, \n    0, 0.05], [0, 0, 0])\n', (2190, 2272), True, 'import pybullet as p\n'), ((3460, 3493), 'pybullet.loadSDF', 'p.loadSDF', (['"""kiva_shelf/model.sdf"""'], {}), "('kiva_shelf/model.sdf')\n", (3469, 3493), True, 'import pybullet as p\n'), ((3510, 3589), 'pybullet.resetBasePositionAndOrientation', 'p.resetBasePositionAndOrientation', (['ob', '[0.0, 1.0, 1.2045]', '[0.0, 0.0, 0.0, 1.0]'], {}), '(ob, [0.0, 1.0, 1.2045], [0.0, 0.0, 0.0, 1.0])\n', (3543, 3589), True, 'import pybullet as p\n'), ((4564, 4591), 'pybullet.setGravity', 'p.setGravity', (['(0.0)', '(0.0)', '(0.0)'], {}), '(0.0, 0.0, 0.0)\n', (4576, 4591), True, 'import pybullet as p\n'), ((4605, 4628), 'pybullet.setGravity', 'p.setGravity', (['(0)', '(0)', '(-10)'], {}), '(0, 0, -10)\n', (4617, 4628), True, 'import pybullet as p\n'), ((4628, 4646), 'pybullet.stepSimulation', 'p.stepSimulation', ([], {}), '()\n', (4644, 4646), True, 'import pybullet as p\n'), ((4648, 4662), 'pybullet.disconnect', 'p.disconnect', ([], {}), '()\n', (4660, 4662), True, 'import pybullet as p\n'), ((115, 174), 'pybullet.loadURDF', 'p.loadURDF', (['"""plane.urdf"""', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('plane.urdf', 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)\n", (125, 174), True, 'import pybullet as p\n'), ((216, 277), 'pybullet.loadURDF', 'p.loadURDF', (['"""samurai.urdf"""', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('samurai.urdf', 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)\n", (226, 277), True, 'import pybullet as p\n'), ((319, 396), 'pybullet.loadURDF', 'p.loadURDF', (['"""pr2_gripper.urdf"""', '(0.5)', '(0.300006)', '(0.7)', '(-0.0)', '(-0.0)', '(-3.1e-05)', '(1.0)'], {}), "('pr2_gripper.urdf', 0.5, 0.300006, 0.7, -0.0, -0.0, -3.1e-05, 1.0)\n", (329, 396), True, 'import pybullet as p\n'), ((570, 597), 'pybullet.getNumJoints', 'p.getNumJoints', (['pr2_gripper'], {}), '(pr2_gripper)\n', (584, 597), True, 'import pybullet as p\n'), ((601, 671), 'pybullet.resetJointState', 'p.resetJointState', (['pr2_gripper', 'jointIndex', 'jointPositions[jointIndex]'], {}), '(pr2_gripper, jointIndex, jointPositions[jointIndex])\n', (618, 671), True, 'import pybullet as p\n'), ((829, 914), 'pybullet.loadURDF', 'p.loadURDF', (['"""kuka_iiwa/model_vr_limits.urdf"""', '(1.4)', '(-0.2)', '(0.6)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('kuka_iiwa/model_vr_limits.urdf', 1.4, -0.2, 0.6, 0.0, 0.0, 0.0, 1.0\n    )\n", (839, 914), True, 'import pybullet as p\n'), ((1074, 1094), 'pybullet.getNumJoints', 'p.getNumJoints', (['kuka'], {}), '(kuka)\n', (1088, 1094), True, 'import pybullet as p\n'), ((1098, 1161), 'pybullet.resetJointState', 'p.resetJointState', (['kuka', 'jointIndex', 'jointPositions[jointIndex]'], {}), '(kuka, jointIndex, jointPositions[jointIndex])\n', (1115, 1161), True, 'import pybullet as p\n'), ((1161, 1257), 'pybullet.setJointMotorControl2', 'p.setJointMotorControl2', (['kuka', 'jointIndex', 'p.POSITION_CONTROL', 'jointPositions[jointIndex]', '(0)'], {}), '(kuka, jointIndex, p.POSITION_CONTROL,\n    jointPositions[jointIndex], 0)\n', (1184, 1257), True, 'import pybullet as p\n'), ((1262, 1326), 'pybullet.loadURDF', 'p.loadURDF', (['"""lego/lego.urdf"""', '(1.0)', '(-0.2)', '(0.7)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('lego/lego.urdf', 1.0, -0.2, 0.7, 0.0, 0.0, 0.0, 1.0)\n", (1272, 1326), True, 'import pybullet as p\n'), ((1368, 1432), 'pybullet.loadURDF', 'p.loadURDF', (['"""lego/lego.urdf"""', '(1.0)', '(-0.2)', '(0.8)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('lego/lego.urdf', 1.0, -0.2, 0.8, 0.0, 0.0, 0.0, 1.0)\n", (1378, 1432), True, 'import pybullet as p\n'), ((1474, 1538), 'pybullet.loadURDF', 'p.loadURDF', (['"""lego/lego.urdf"""', '(1.0)', '(-0.2)', '(0.9)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('lego/lego.urdf', 1.0, -0.2, 0.9, 0.0, 0.0, 0.0, 1.0)\n", (1484, 1538), True, 'import pybullet as p\n'), ((1959, 1987), 'pybullet.getNumJoints', 'p.getNumJoints', (['kuka_gripper'], {}), '(kuka_gripper)\n', (1973, 1987), True, 'import pybullet as p\n'), ((1991, 2062), 'pybullet.resetJointState', 'p.resetJointState', (['kuka_gripper', 'jointIndex', 'jointPositions[jointIndex]'], {}), '(kuka_gripper, jointIndex, jointPositions[jointIndex])\n', (2008, 2062), True, 'import pybullet as p\n'), ((2062, 2166), 'pybullet.setJointMotorControl2', 'p.setJointMotorControl2', (['kuka_gripper', 'jointIndex', 'p.POSITION_CONTROL', 'jointPositions[jointIndex]', '(0)'], {}), '(kuka_gripper, jointIndex, p.POSITION_CONTROL,\n    jointPositions[jointIndex], 0)\n', (2085, 2166), True, 'import pybullet as p\n'), ((2274, 2351), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(1.3)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 1.3, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2284, 2351), True, 'import pybullet as p\n'), ((2382, 2459), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(1.2)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 1.2, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2392, 2459), True, 'import pybullet as p\n'), ((2490, 2567), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(1.1)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 1.1, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2500, 2567), True, 'import pybullet as p\n'), ((2598, 2675), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(1.0)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 1.0, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2608, 2675), True, 'import pybullet as p\n'), ((2706, 2783), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(0.9)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 0.9, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2716, 2783), True, 'import pybullet as p\n'), ((2814, 2891), 'pybullet.loadURDF', 'p.loadURDF', (['"""jenga/jenga.urdf"""', '(0.8)', '(-0.7)', '(0.75)', '(0.0)', '(0.707107)', '(0.0)', '(0.707107)'], {}), "('jenga/jenga.urdf', 0.8, -0.7, 0.75, 0.0, 0.707107, 0.0, 0.707107)\n", (2824, 2891), True, 'import pybullet as p\n'), ((2922, 2998), 'pybullet.loadURDF', 'p.loadURDF', (['"""table/table.urdf"""', '(1.0)', '(-0.2)', '(0.0)', '(0.0)', '(0.0)', '(0.707107)', '(0.707107)'], {}), "('table/table.urdf', 1.0, -0.2, 0.0, 0.0, 0.0, 0.707107, 0.707107)\n", (2932, 2998), True, 'import pybullet as p\n'), ((3030, 3107), 'pybullet.loadURDF', 'p.loadURDF', (['"""teddy_vhacd.urdf"""', '(1.05)', '(-0.5)', '(0.7)', '(0.0)', '(0.0)', '(0.707107)', '(0.707107)'], {}), "('teddy_vhacd.urdf', 1.05, -0.5, 0.7, 0.0, 0.0, 0.707107, 0.707107)\n", (3040, 3107), True, 'import pybullet as p\n'), ((3138, 3214), 'pybullet.loadURDF', 'p.loadURDF', (['"""cube_small.urdf"""', '(0.95)', '(-0.1)', '(0.7)', '(0.0)', '(0.0)', '(0.707107)', '(0.707107)'], {}), "('cube_small.urdf', 0.95, -0.1, 0.7, 0.0, 0.0, 0.707107, 0.707107)\n", (3148, 3214), True, 'import pybullet as p\n'), ((3245, 3323), 'pybullet.loadURDF', 'p.loadURDF', (['"""sphere_small.urdf"""', '(0.85)', '(-0.4)', '(0.7)', '(0.0)', '(0.0)', '(0.707107)', '(0.707107)'], {}), "('sphere_small.urdf', 0.85, -0.4, 0.7, 0.0, 0.0, 0.707107, 0.707107)\n", (3255, 3323), True, 'import pybullet as p\n'), ((3354, 3430), 'pybullet.loadURDF', 'p.loadURDF', (['"""duck_vhacd.urdf"""', '(0.85)', '(-0.4)', '(0.9)', '(0.0)', '(0.0)', '(0.707107)', '(0.707107)'], {}), "('duck_vhacd.urdf', 0.85, -0.4, 0.9, 0.0, 0.0, 0.707107, 0.707107)\n", (3364, 3430), True, 'import pybullet as p\n'), ((3626, 3693), 'pybullet.loadURDF', 'p.loadURDF', (['"""teddy_vhacd.urdf"""', '(-0.1)', '(0.6)', '(0.85)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('teddy_vhacd.urdf', -0.1, 0.6, 0.85, 0.0, 0.0, 0.0, 1.0)\n", (3636, 3693), True, 'import pybullet as p\n'), ((3734, 3828), 'pybullet.loadURDF', 'p.loadURDF', (['"""sphere_small.urdf"""', '(-0.1)', '(0.955006)', '(1.169706)', '(0.633232)', '(-0.0)', '(-0.0)', '(0.773962)'], {}), "('sphere_small.urdf', -0.1, 0.955006, 1.169706, 0.633232, -0.0, -\n    0.0, 0.773962)\n", (3744, 3828), True, 'import pybullet as p\n'), ((3845, 3910), 'pybullet.loadURDF', 'p.loadURDF', (['"""cube_small.urdf"""', '(0.3)', '(0.6)', '(0.85)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('cube_small.urdf', 0.3, 0.6, 0.85, 0.0, 0.0, 0.0, 1.0)\n", (3855, 3910), True, 'import pybullet as p\n'), ((3951, 4036), 'pybullet.loadURDF', 'p.loadURDF', (['"""table_square/table_square.urdf"""', '(-1.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('table_square/table_square.urdf', -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0\n    )\n", (3961, 4036), True, 'import pybullet as p\n'), ((4131, 4149), 'pybullet.getNumJoints', 'p.getNumJoints', (['ob'], {}), '(ob)\n', (4145, 4149), True, 'import pybullet as p\n'), ((4153, 4214), 'pybullet.resetJointState', 'p.resetJointState', (['ob', 'jointIndex', 'jointPositions[jointIndex]'], {}), '(ob, jointIndex, jointPositions[jointIndex])\n', (4170, 4214), True, 'import pybullet as p\n'), ((4225, 4291), 'pybullet.loadURDF', 'p.loadURDF', (['"""husky/husky.urdf"""', '(2.0)', '(-5.0)', '(1.0)', '(0.0)', '(0.0)', '(0.0)', '(1.0)'], {}), "('husky/husky.urdf', 2.0, -5.0, 1.0, 0.0, 0.0, 0.0, 1.0)\n", (4235, 4291), True, 'import pybullet as p\n'), ((4481, 4499), 'pybullet.getNumJoints', 'p.getNumJoints', (['ob'], {}), '(ob)\n', (4495, 4499), True, 'import pybullet as p\n'), ((4503, 4564), 'pybullet.resetJointState', 'p.resetJointState', (['ob', 'jointIndex', 'jointPositions[jointIndex]'], {}), '(ob, jointIndex, jointPositions[jointIndex])\n', (4520, 4564), True, 'import pybullet as p\n')]