Python pybullet.stepSimulation函数代码示例

  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, {}