def step2(self, action):
    for i in range(self._actionRepeat):
      self._kuka.applyAction(action)
      p.stepSimulation()
      if self._termination():
        break
      self._envStepCounter += 1
    if self._renders:
      time.sleep(self._timeStep)
    self._observation = self.getExtendedObservation()

    #print("self._envStepCounter")
    #print(self._envStepCounter)

    done = self._termination()
    npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]])
    actionCost = np.linalg.norm(npaction)*10.
    #print("actionCost")
    #print(actionCost)
    reward = self._reward()-actionCost
    #print("reward")
    #print(reward)

    #print("len=%r" % len(self._observation))

    return np.array(self._observation), reward, done, {}

  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 _termination(self):
   #print (self._kuka.endEffectorPos[2])
   state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
   actualEndEffectorPos = state[0]
     
   #print("self._envStepCounter")
   #print(self._envStepCounter)
   if (self.terminated or self._envStepCounter>1000):
     self._observation = self.getExtendedObservation()
     return True
   
   if (actualEndEffectorPos[2] <= 0.10):
     self.terminated = 1
     
     #print("closing gripper, attempting grasp")
     #start grasp and terminate
     fingerAngle = 0.3
     
     for i in range (1000):
       graspAction = [0,0,0.001,0,fingerAngle]
       self._kuka.applyAction(graspAction)
       p.stepSimulation()
       fingerAngle = fingerAngle-(0.3/100.)
       if (fingerAngle<0):
         fingerAngle=0
       
     self._observation = self.getExtendedObservation()
     return True
   return False

 def buildJointNameToIdDict(self):
   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 applyAction(self, actions):
    forces = [0.] * len(self.motors)
    for m in range(len(self.motors)):
      forces[m] = self.motor_power[m]*actions[m]*0.082
    p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces)

    p.stepSimulation()
    time.sleep(0.01)
    distance=5
    yaw = 0

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 test(args):	
	p.connect(p.GUI)
	p.setAdditionalSearchPath(pybullet_data.getDataPath())
	fileName = os.path.join("mjcf", args.mjcf)
	print("fileName")
	print(fileName)
	p.loadMJCF(fileName)
	while (1):
		p.stepSimulation()
		p.getCameraImage(320,240)
		time.sleep(0.01)

 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 step(self,action):
     p.stepSimulation()
     start = time.time()
     yaw = next(self.iter)
     viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
     aspect = pixelWidth / pixelHeight;
     projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
     img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
         projectionMatrix, shadow=1,lightDirection=[1,1,1],
         renderer=p.ER_BULLET_HARDWARE_OPENGL)
         #renderer=pybullet.ER_TINY_RENDERER)
     self._observation = img_arr[2]
     return np.array(self._observation), 0, 0, {}

def test(num_runs=300, shadow=1, log=True, plot=False):
    if log:
        logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")

    if plot:
        plt.ion()

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


    times = np.zeros(num_runs)
    yaw_gen = itertools.cycle(range(0,360,10))
    for i, yaw in zip(range(num_runs),yaw_gen):
        pybullet.stepSimulation()
        start = time.time()
        viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
        aspect = pixelWidth / pixelHeight;
        projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
        img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
            projectionMatrix, shadow=shadow,lightDirection=[1,1,1],
            renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
            #renderer=pybullet.ER_TINY_RENDERER)
        stop = time.time()
        duration = (stop - start)
        if (duration):
        	fps = 1./duration
        	#print("fps=",fps)
        else:
        	fps=0
        	#print("fps=",fps)
        #print("duraction=",duration)
        #print("fps=",fps)
        times[i] = fps

        if plot:
            rgb = img_arr[2]
            image.set_data(rgb)#np_img_arr)
            ax.plot([0])
            #plt.draw()
            #plt.show()
            plt.pause(0.01)

    mean_time = float(np.mean(times))
    print("mean: {0} for {1} runs".format(mean_time, num_runs))
    print("")
    if log:
        pybullet.stopStateLogging(logId)
    return mean_time

  def _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state
    force = action
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {}

 def _step(self, action):
   self._humanoid.applyAction(action)
   for i in range(self._actionRepeat):
     p.stepSimulation()
     if self._renders:
       time.sleep(self._timeStep)
     self._observation = self.getExtendedObservation()
     if self._termination():
       break
     self._envStepCounter += 1
   reward = self._reward()
   done = self._termination()
   #print("len=%r" % len(self._observation))
   
   return np.array(self._observation), reward, done, {}

  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 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 _step(self, action):
    force = self.force_mag if action==1 else -self.force_mag

    p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
    p.stepSimulation()

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

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    done = bool(done)
    reward = 1.0
    #print("state=",self.state)
    return np.array(self.state), reward, done, {}

  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 _termination(self):
    #print (self._kuka.endEffectorPos[2])
    state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
    actualEndEffectorPos = state[0]

    #print("self._envStepCounter")
    #print(self._envStepCounter)
    if (self.terminated or self._envStepCounter>self._maxSteps):
      self._observation = self.getExtendedObservation()
      return True
    maxDist = 0.005
    closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)

    if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
      self.terminated = 1

      #print("terminating, closing gripper, attempting grasp")
      #start grasp and terminate
      fingerAngle = 0.3
      for i in range (100):
        graspAction = [0,0,0.0001,0,fingerAngle]
        self._kuka.applyAction(graspAction)
        p.stepSimulation()
        fingerAngle = fingerAngle-(0.3/100.)
        if (fingerAngle<0):
          fingerAngle=0

      for i in range (1000):
        graspAction = [0,0,0.001,0,fingerAngle]
        self._kuka.applyAction(graspAction)
        p.stepSimulation()
        blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
        if (blockPos[2] > 0.23):
          #print("BLOCKPOS!")
          #print(blockPos[2])
          break
        state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
        actualEndEffectorPos = state[0]
        if (actualEndEffectorPos[2]>0.5):
          break


      self._observation = self.getExtendedObservation()
      return True
    return False

  def _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state
    
    dv = 0.1
    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
    
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {}

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