How to use pybullet - 10 common examples

# double m_kDP;        // Damping coefficient [0,1]
        # double m_kDG;        // Drag coefficient [0,+inf]
        # double m_kLF;        // Lift coefficient [0,+inf]
        # double m_kPR;        // Pressure coefficient [-inf,+inf]
        # double m_kVC;        // Volume conversation coefficient [0,+inf]
        # double m_kDF;        // Dynamic friction coefficient [0,1]
        # double m_kMT;        // Pose matching coefficient [0,1]
        # double m_kCHR;       // Rigid contacts hardness [0,1]
        # double m_kKHR;       // Kinetic contacts hardness [0,1]
        # double m_kSHR;       // Soft contacts hardness [0,1]
        # double m_kAHR;       // Anchors hardness [0,1]
        # int m_viterations;   // Velocities solver iterations
        # int m_piterations;   // Positions solver iterations
        # int m_diterations;   // Drift solver iterations

        p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
        self.robot.set_gravity(0, 0, 0)
        self.human.set_gravity(0, 0, -1)
        self.cloth_attachment.set_gravity(0, 0, 0)

        p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id)

        # Enable rendering
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)

        # Wait for the cloth to settle
        for _ in range(50):
            # Force the end effector attachment to stay at the end effector
            self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
            p.stepSimulation(physicsClientId=self.id)

        p.setGravity(0, 0, -9.81, physicsClientId=self.id)

### Taken from pb tutorial

import pybullet as p
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setGravity(0,0,-10)
planeId = p.loadURDF("jaco_robot.urdf")

#!/usr/bin/env python

import pybullet as p
import time
import math

useRealTime = 0
fixedTimeStep = 0.001

physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
	p.connect(p.GUI)

p.loadURDF("ground.urdf")
p.setGravity(0,0,-9.81)
p.setTimeStep(fixedTimeStep)

p.setRealTimeSimulation(0)
#body = p.loadURDF("simple-snakev0.urdf",1,-2,1)
#body = p.loadURDF("springy-snakev0.urdf",1,-2,1)
body = p.loadURDF("phantomx/phantomx.urdf",0,0,2)

position,orientation = p.getBasePositionAndOrientation(body)
print position

num_joints = p.getNumJoints(body)

minLimit = -math.pi/2
maxLimit = math.pi/2

position,orientation = p.getBasePositionAndOrientation(body)
print position

num_joints = p.getNumJoints(body)

minLimit = -math.pi/2
maxLimit = math.pi/2

p_gain =2
speed = 5

t = 0
while True:
	print math.sin(t*speed)/2*(maxLimit-minLimit)+minLimit
	for i in range(num_joints):
		p.setJointMotorControl2(body,i,p.POSITION_CONTROL,math.sin(t*speed)/2*(maxLimit-minLimit),p_gain)	
	p.stepSimulation()
	t = t + fixedTimeStep

	print p.getBasePositionAndOrientation(body)
	
	if t > 2:
		print "resetting position..."
		p.resetBasePositionAndOrientation(body, position, (0,0,0,1))
		
		print "resetting joint states..."
		for i in range(num_joints):
			p.resetJointState(body,i,0)
		t = 0

	print t

def goToPosXY(coords):
        desiredOrientation = pybullet.getQuaternionFromEuler([0, 3.14, -1.57])
        action = pybullet.calculateInverseKinematics(0, 7, coords,
                                                     desiredOrientation,
                                                     maxNumIterations=1000,
                                                     residualThreshold=0.001)
        return action[:9]

def init(self, furniture_type, directory, id, np_random, wheelchair_mounted=False):
        if 'wheelchair' in furniture_type:
            furniture = p.loadURDF(os.path.join(directory, 'wheelchair', 'wheelchair.urdf' if not wheelchair_mounted else 'wheelchair_jaco.urdf'), basePosition=[0, 0, 0.06], baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=id), physicsClientId=id)
        elif furniture_type == 'bed':
            furniture = p.loadURDF(os.path.join(directory, 'bed', 'bed.urdf'), basePosition=[-0.1, 0, 0], baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)

            # mesh_scale = [1.1]*3
            # bed_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced.obj'), rgbaColor=[1, 1, 1, 1], specularColor=[0.2, 0.2, 0.2], meshScale=mesh_scale, physicsClientId=self.id)
            # bed_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced_vhacd.obj'), meshScale=mesh_scale, flags=p.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=self.id)
            # furniture = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=bed_collision, baseVisualShapeIndex=bed_visual, basePosition=[0, 0, 0], useMaximalCoordinates=True, physicsClientId=self.id)
            # # Initialize bed position
            # p.resetBasePositionAndOrientation(furniture, [-0.1, 0, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        elif furniture_type == 'table':
            furniture = p.loadURDF(os.path.join(directory, 'table', 'table_tall.urdf'), basePosition=[0.25, -0.9, 0], baseOrientation=[0, 0, 0, 1], physicsClientId=id)
        elif furniture_type == 'bowl':
            bowl_pos = np.array([-0.15, -0.55, 0.75]) + np.array([np_random.uniform(-0.05, 0.05), np_random.uniform(-0.05, 0.05), 0])
            furniture = p.loadURDF(os.path.join(directory, 'dinnerware', 'bowl.urdf'), basePosition=bowl_pos, baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)
        elif furniture_type == 'nightstand':
            furniture = p.loadURDF(os.path.join(directory, 'nightstand', 'nightstand.urdf'), basePosition=np.array([-0.9, 0.7, 0]), baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)
        else:
            furniture = None

        super(Furniture, self).init(furniture, id, np_random, indices=-1)

def init(self, directory, id, np_random, fixed_base=True):
        self.body = p.loadURDF(os.path.join(directory, 'baxter', 'baxter_custom.urdf'), useFixedBase=fixed_base, basePosition=[-2, -2, 0.975], physicsClientId=id)
        super(Baxter, self).init(self.body, id, np_random)

        # Recolor robot
        for i in [20, 21, 23, 31, 32, 42, 43, 45, 53, 54]:
            p.changeVisualShape(self.body, i, rgbaColor=[1.0, 1.0, 1.0, 0.0], physicsClientId=id)

self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=0.01)
        self.human.set_mass(self.human.base, mass=0)
        self.human.set_base_velocity(linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0])

        # Let the right arm fall to the ground
        for _ in range(100):
            p.stepSimulation(physicsClientId=self.id)

        elbow_pos = self.human.get_pos_orient(self.human.right_elbow)[0]
        wrist_pos = self.human.get_pos_orient(self.human.right_wrist)[0]
        stomach_pos = self.human.get_pos_orient(self.human.stomach)[0]
        waist_pos = self.human.get_pos_orient(self.human.waist)[0]

        target_ee_right_pos = np.array([-0.9, -0.3, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
        target_ee_left_pos = np.array([-0.9, 0.7, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
        target_ee_orient = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task]), physicsClientId=self.id))
        # Use TOC with JLWKI to find an optimal base position for the robot near the person
        if self.robot.has_single_arm:
            target_ee_right_pos = np.array([-0.9, 0.4, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
            base_position, _, _ = self.robot.position_robot_toc(self.task, 'right', [(target_ee_right_pos, target_ee_orient)], [(wrist_pos, None), (waist_pos, None), (elbow_pos, None), (stomach_pos, None)], self.human, step_sim=True, check_env_collisions=False)
        else:
            base_position, _, _ = self.robot.position_robot_toc(self.task, ['right', 'left'], [[(target_ee_right_pos, target_ee_orient)], [(target_ee_left_pos, target_ee_orient)]], [[(wrist_pos, None), (waist_pos, None)], [(elbow_pos, None), (stomach_pos, None)]], self.human, step_sim=True, check_env_collisions=False)
        if self.robot.wheelchair_mounted:
            # Load a nightstand in the environment for the jaco arm
            self.nightstand = Furniture()
            self.nightstand.init('nightstand', self.directory, self.id, self.np_random)
            self.nightstand.set_base_pos_orient(np.array([-0.9, 0.7, 0]) + base_position, [np.pi/2.0, 0, 0])
        # Open gripper to hold the tools
        self.robot.set_gripper_open_position(self.robot.right_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True)
        # Initialize the tool in the robot's gripper
        self.tool_right.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.001]*3)
        if not self.robot.has_single_arm:

def set_kinematic_pose(u):
				if isinstance(u, zencad.assemble.kinematic_unit):
					info = p.getJointState(u.pybullet_base.boxId, u.simulation_hint2)
					pose=info[0]
					u.set_coord(pose + u.simulation_start_pose)
			zencad.assemble.for_each_unit(self.base_interactive, set_kinematic_pose)

resetPose()

# Initialise the gamepad object using the gamepad inputs Python package
gamepad = ThreadedInputs()
for gamepadInput in gamepadInputs:
    gamepad.append_command(gamepadInput, gamepadInputs[gamepadInput])
gamepad.start()
trotting=TrottingGait()


s=False
while True:

    bodyPos=robot.getPos()
    bodyOrn,_,_=robot.getIMU()
    xr,yr,_= p.getEulerFromQuaternion(bodyOrn)
    distance=math.sqrt(bodyPos[0]**2+bodyPos[1]**2)
    if distance>50:
        robot.resetBody()
   
    ir=xr/(math.pi/180)
    handleGamepad()
    d=time.time()-rtime
    height = p.readUserDebugParameter(IDheight)

    # wait 3 seconds to start
    if d>3:
        robot.feetPosition(trotting.positions(d-3))
    else:
        robot.feetPosition(Lp)
    #roll=-xr
    roll=0