def reset(self):
		self.ordered_joints = []

		print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf))

		if self.self_collision:
			self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
				p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
				basePosition=self.basePosition,
				baseOrientation=self.baseOrientation,
				useFixedBase=self.fixed_base,
				flags=p.URDF_USE_SELF_COLLISION))
		else:
			self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
				p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
				basePosition=self.basePosition,
				baseOrientation=self.baseOrientation,
				useFixedBase=self.fixed_base))

		self.robot_specific_reset()

		s = self.calc_state()  # optimization: calc_state() can calculate something in self.* for calc_potential() to use
		self.potential = self.calc_potential()

		return s

def setupWorld():
	p.resetSimulation()
	p.loadURDF("planeMesh.urdf")
	kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
	for i in range (p.getNumJoints(kukaId)):
		p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
	for i in range (numObjects):
		cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
		#p.changeDynamics(cube,-1,mass=100)
	p.stepSimulation()
	p.setGravity(0,0,-10)

 def reset(self):
   objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
   self.kukaUid = objects[0]
   #for i in range (p.getNumJoints(self.kukaUid)):
   #  print(p.getJointInfo(self.kukaUid,i))
   p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
   self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
   self.numJoints = p.getNumJoints(self.kukaUid)
   for jointIndex in range (self.numJoints):
     p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
     p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
   
   self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
   self.endEffectorPos = [0.537,0.0,0.5]
   self.endEffectorAngle = 0
   
   
   self.motorNames = []
   self.motorIndices = []
   
   for i in range (self.numJoints):
     jointInfo = p.getJointInfo(self.kukaUid,i)
     qIndex = jointInfo[3]
     if qIndex > -1:
       #print("motorname")
       #print(jointInfo[1])
       self.motorNames.append(str(jointInfo[1]))
       self.motorIndices.append(i)

def get_cube(x, y, z):	
	body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z)
	p.changeDynamics(body,-1, mass=1.2)#match Roboschool
	part_name, _ = p.getBodyInfo(body, 0)
	part_name = part_name.decode("utf8")
	bodies = [body]
	return BodyPart(part_name, bodies, 0, -1)

  def _randomly_place_objects(self, urdfList):
    """Randomly places the objects in the bin.

    Args:
      urdfList: The list of urdf files to place in the bin.

    Returns:
      The list of object unique ID's.
    """


    # Randomize positions of each object urdf.
    objectUids = []
    for urdf_name in urdfList:
      xpos = 0.4 +self._blockRandom*random.random()
      ypos = self._blockRandom*(random.random()-.5)
      angle = np.pi/2 + self._blockRandom * np.pi * random.random()
      orn = p.getQuaternionFromEuler([0, 0, angle])
      urdf_path = os.path.join(self._urdfRoot, urdf_name)
      uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
        [orn[0], orn[1], orn[2], orn[3]])
      objectUids.append(uid)
      # Let each object fall to the tray individual, to prevent object
      # intersection.
      for _ in range(500):
        p.stepSimulation()
    return objectUids

  def setupWorld(self):
    numObjects = 50

    maximalCoordinates = False

    p.resetSimulation()
    p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
    p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
    kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
                        useMaximalCoordinates=maximalCoordinates)
    for i in range(p.getNumJoints(kukaId)):
      p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
    for i in range(numObjects):
      cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
      #p.changeDynamics(cube,-1,mass=100)
    p.stepSimulation()
    p.setGravity(0, 0, -10)

def evaluate_params(evaluateFunc,
                    params,
                    objectiveParams,
                    urdfRoot='',
                    timeStep=0.01,
                    maxNumSteps=10000,
                    sleepTime=0):
  print('start evaluation')
  beforeTime = time.time()
  p.resetSimulation()

  p.setTimeStep(timeStep)
  p.loadURDF("%s/plane.urdf" % urdfRoot)
  p.setGravity(0, 0, -10)

  global minitaur
  minitaur = Minitaur(urdfRoot)
  start_position = current_position()
  last_position = None  # for tracing line
  total_energy = 0

  for i in range(maxNumSteps):
    torques = minitaur.getMotorTorques()
    velocities = minitaur.getMotorVelocities()
    total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep

    joint_values = evaluate_func_map[evaluateFunc](i, params)
    minitaur.applyAction(joint_values)
    p.stepSimulation()
    if (is_fallen()):
      break

    if i % 100 == 0:
      sys.stdout.write('.')
      sys.stdout.flush()
    time.sleep(sleepTime)

  print(' ')

  alpha = objectiveParams[0]
  final_distance = np.linalg.norm(start_position - current_position())
  finalReturn = final_distance - alpha * total_energy
  elapsedTime = time.time() - beforeTime
  print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
        "finalReturn", finalReturn, "elapsed_time", elapsedTime)
  return finalReturn

 def _reset(self):
   p.resetSimulation()
   #p.setPhysicsEngineParameter(numSolverIterations=300)
   p.setTimeStep(self._timeStep)
   p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
   
   dist = 5 +2.*random.random()
   ang = 2.*3.1415925438*random.random()
   
   ballx = dist * math.sin(ang)
   bally = dist * math.cos(ang)
   ballz = 1
       
   p.setGravity(0,0,-10)
   self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
   self._envStepCounter = 0
   p.stepSimulation()
   self._observation = self.getExtendedObservation()
   return np.array(self._observation)

 def test_rolling_friction(self):
   import pybullet as p
   p.connect(p.DIRECT)
   p.loadURDF("plane.urdf")
   sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
   p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
   p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
   #p.changeDynamics(sphere,-1,rollingFriction=0)
   p.setGravity(0, 0, -10)
   for i in range(1000):
     p.stepSimulation()
   vel = p.getBaseVelocity(sphere)
   self.assertLess(vel[0][0], 1e-10)
   self.assertLess(vel[0][1], 1e-10)
   self.assertLess(vel[0][2], 1e-10)
   self.assertLess(vel[1][0], 1e-10)
   self.assertLess(vel[1][1], 1e-10)
   self.assertLess(vel[1][2], 1e-10)
   p.disconnect()

 def reset(self):
   self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
   self.kp = 1
   self.kd = 0.1
   self.maxForce = 3.5
   self.nMotors = 8
   self.motorIdList = []
   self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
   self.buildJointNameToIdDict()
   self.buildMotorIdList()

 def _reset(self):
   self.terminated = 0
   p.resetSimulation()
   p.setPhysicsEngineParameter(numSolverIterations=150)
   p.setTimeStep(self._timeStep)
   p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
   
   p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
   
   xpos = 0.5 +0.2*random.random()
   ypos = 0 +0.25*random.random()
   ang = 3.1415925438*random.random()
   orn = p.getQuaternionFromEuler([0,0,ang])
   self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
           
   p.setGravity(0,0,-10)
   self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
   self._envStepCounter = 0
   p.stepSimulation()
   self._observation = self.getExtendedObservation()
   return np.array(self._observation)

  def _reset(self):
    """Environment reset called at the beginning of an episode.
    """
    # Set the camera settings.
    look = [0.23, 0.2, 0.54]
    distance = 1.
    pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
    yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
    roll = 0
    self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
        look, distance, yaw, pitch, roll, 2)
    fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
    aspect = self._width / self._height
    near = 0.01
    far = 10
    self._proj_matrix = p.computeProjectionMatrixFOV(
        fov, aspect, near, far)
    
    self._attempted_grasp = False
    self._env_step = 0
    self.terminated = 0

    p.resetSimulation()
    p.setPhysicsEngineParameter(numSolverIterations=150)
    p.setTimeStep(self._timeStep)
    p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
    
    p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
            
    p.setGravity(0,0,-10)
    self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
    self._envStepCounter = 0
    p.stepSimulation()

    # Choose the objects in the bin.
    urdfList = self._get_random_object(
      self._numObjects, self._isTest)
    self._objectUids = self._randomly_place_objects(urdfList)
    self._observation = self._get_observation()
    return np.array(self._observation)

 def reset(self):
   self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
   self.kp = 1
   self.kd = 0.1
   self.maxForce = 100
   nJoints = p.getNumJoints(self.quadruped)
   self.jointNameToId = {}
   for i in range(nJoints):
     jointInfo = p.getJointInfo(self.quadruped, i)
     self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
   self.resetPose()
   for i in range(100):
     p.stepSimulation()

def main(unused_args):
  timeStep = 0.01
  c = p.connect(p.SHARED_MEMORY)
  if (c<0):
      c = p.connect(p.GUI)
  p.resetSimulation()
  p.setTimeStep(timeStep)
  p.loadURDF("plane.urdf")
  p.setGravity(0,0,-10)

  minitaur = Minitaur()
  amplitude = 0.24795664427
  speed = 0.2860877729434
  for i in range(1000):
    a1 = math.sin(i*speed)*amplitude+1.57
    a2 = math.sin(i*speed+3.14)*amplitude+1.57
    joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
    minitaur.applyAction(joint_values)

    p.stepSimulation()
#    print(minitaur.getBasePosition())
    time.sleep(timeStep)
  final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
  print(final_distance)

  def testJacobian(self):
    import pybullet as p

    clid = p.connect(p.SHARED_MEMORY)
    if (clid < 0):
      p.connect(p.DIRECT)

    time_step = 0.001
    gravity_constant = -9.81

    urdfs = [
        "TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
        "kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
    ]
    for urdf in urdfs:
      p.resetSimulation()
      p.setTimeStep(time_step)
      p.setGravity(0.0, 0.0, gravity_constant)

      robotId = p.loadURDF(urdf, useFixedBase=True)
      p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
      numJoints = p.getNumJoints(robotId)
      endEffectorIndex = numJoints - 1

      # Set a joint target for the position control and step the sim.
      self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
      p.stepSimulation()

      # Get the joint and link state directly from Bullet.
      mpos, mvel, mtorq = self.getMotorJointStates(robotId)

      result = p.getLinkState(robotId,
                              endEffectorIndex,
                              computeLinkVelocity=1,
                              computeForwardKinematics=1)
      link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
      # Get the Jacobians for the CoM of the end-effector link.
      # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
      # The localPosition is always defined in terms of the link frame coordinates.

      zero_vec = [0.0] * len(mpos)
      jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
                                         zero_vec)

      assert (allclose(dot(jac_t, mvel), link_vt))
      assert (allclose(dot(jac_r, mvel), link_vr))
    p.disconnect()

  def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    self.timeStep = 0.01
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -10)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    p.resetJointState(self.cartpole, 1, initialAngle)
    p.resetJointState(self.cartpole, 0, initialCartPos)

    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]

    return np.array(self.state)

  def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
    p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
    p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
    self.timeStep = 0.02
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -9.8)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
    p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
    p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
    #print("randstate=",randstate)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    #print("self.state=", self.state)
    return np.array(self.state)

    def __enter__(self):
        print("connecting")
        optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
        optionstring += ' --window_backend=2 --render_device=0'

        print(self.connection_mode, optionstring,*self.argv)
        cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
        if cid < 0:
            raise ValueError
        print("connected")
        pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
        pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
        pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
        pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)

        pybullet.resetSimulation()
        pybullet.loadURDF("plane.urdf",[0,0,-1])
        pybullet.loadURDF("r2d2.urdf")
        pybullet.loadURDF("duck_vhacd.urdf")
        pybullet.setGravity(0,0,-10)

import pybullet as p
from time import sleep
import matplotlib.pyplot as plt
import numpy as np

physicsClient = p.connect(p.GUI)

p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)


useRealTimeSimulation = 1

if (useRealTimeSimulation):
	p.setRealTimeSimulation(1)

while 1:
	if (useRealTimeSimulation):
		camera = p.getDebugVisualizerCamera()
		viewMat = camera[2]
		projMat = camera[3]
		#An example of setting the view matrix for the projective texture.
		#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])

import pybullet
import time
import numpy as np #to reshape for matplotlib

plt.ion()

img = [[1,2,3]*50]*100#np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()


pybullet.connect(pybullet.DIRECT)

#pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")

pybullet.setGravity(0,0,-10)
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]

pitch = -10.0

roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01