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()

[(3, 'pybullet.connect', 'p.connect', True, 'import pybullet as p\n'), (4, 'pybullet.resetSimulation', 'p.resetSimulation', True, 'import pybullet as p\n'), (17, 'pybullet.createConstraint', 'p.createConstraint', True, 'import pybullet as p\n'), (31, 'pybullet.loadSDF', 'p.loadSDF', True, 'import pybullet as p\n'), (36, 'pybullet.resetBasePositionAndOrientation', 'p.resetBasePositionAndOrientation', True, 'import pybullet as p\n'), (43, 'pybullet.createConstraint', 'p.createConstraint', True, 'import pybullet as p\n'), (56, 'pybullet.loadSDF', 'p.loadSDF', True, 'import pybullet as p\n'), (58, 'pybullet.resetBasePositionAndOrientation', 'p.resetBasePositionAndOrientation', True, 'import pybullet as p\n'), (74, 'pybullet.setGravity', 'p.setGravity', True, 'import pybullet as p\n'), (75, 'pybullet.setGravity', 'p.setGravity', True, 'import pybullet as p\n'), (77, 'pybullet.stepSimulation', 'p.stepSimulation', True, 'import pybullet as p\n'), (79, 'pybullet.disconnect', 'p.disconnect', True, 'import pybullet as p\n'), (6, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (7, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (8, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (14, 'pybullet.getNumJoints', 'p.getNumJoints', True, 'import pybullet as p\n'), (15, 'pybullet.resetJointState', 'p.resetJointState', True, 'import pybullet as p\n'), (21, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (24, 'pybullet.getNumJoints', 'p.getNumJoints', True, 'import pybullet as p\n'), (25, 'pybullet.resetJointState', 'p.resetJointState', True, 'import pybullet as p\n'), (26, 'pybullet.setJointMotorControl2', 'p.setJointMotorControl2', True, 'import pybullet as p\n'), (28, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (29, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (30, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (38, 'pybullet.getNumJoints', 'p.getNumJoints', True, 'import pybullet as p\n'), (39, 'pybullet.resetJointState', 'p.resetJointState', True, 'import pybullet as p\n'), (40, 'pybullet.setJointMotorControl2', 'p.setJointMotorControl2', True, 'import pybullet as p\n'), (45, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (46, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (47, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (48, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (49, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (50, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (51, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (52, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (53, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (54, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (55, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (59, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (60, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (61, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (62, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (65, 'pybullet.getNumJoints', 'p.getNumJoints', True, 'import pybullet as p\n'), (66, 'pybullet.resetJointState', 'p.resetJointState', True, 'import pybullet as p\n'), (68, 'pybullet.loadURDF', 'p.loadURDF', True, 'import pybullet as p\n'), (71, 'pybullet.getNumJoints', 'p.getNumJoints', True, 'import pybullet as p\n'), (72, 'pybullet.resetJointState', 'p.resetJointState', True, 'import pybullet as p\n')]