(self)
return pose_reward
Get the pose reward.
Get the pose reward.
Get the pose reward .
def _calc_reward_pose(self):
    """Get the pose reward."""
    env = self._env
    robot = env.robot
    sim_model = robot.quadruped
    ref_model = self._ref_model
    pyb = self._get_pybullet_client()

    pose_err = 0.0
    num_joints = self._get_num_joints()

    for j in range(num_joints):
      j_state_ref = pyb.getJointStateMultiDof(ref_model, j)
      j_state_sim = pyb.getJointStateMultiDof(sim_model, j)
      j_pose_ref = np.array(j_state_ref[0])
      j_pose_sim = np.array(j_state_sim[0])

      j_size_ref = len(j_pose_ref)
      j_size_sim = len(j_pose_sim)

      if (j_size_ref > 0):
        assert (j_size_ref == j_size_sim)
        j_pose_diff = j_pose_ref - j_pose_sim
        j_pose_err = j_pose_diff.dot(j_pose_diff)
        pose_err += j_pose_err

    pose_reward = np.exp(-self._pose_err_scale * pose_err)

    return pose_reward
def _calc_reward_pose ( self ) : env = self . _env robot = env . robot sim_model = robot . quadruped ref_model = self . _ref_model pyb = self . _get_pybullet_client ( ) pose_err = 0.0 num_joints = self . _get_num_joints ( ) for j in range ( num_joints ) : j_state_ref = pyb . getJointStateMultiDof ( ref_model , j ) j_state_sim = pyb . getJointStateMultiDof ( sim_model , j ) j_pose_ref = np . array ( j_state_ref [ 0 ] ) j_pose_sim = np . array ( j_state_sim [ 0 ] ) j_size_ref = len ( j_pose_ref ) j_size_sim = len ( j_pose_sim ) if ( j_size_ref > 0 ) : assert ( j_size_ref == j_size_sim ) j_pose_diff = j_pose_ref - j_pose_sim j_pose_err = j_pose_diff . dot ( j_pose_diff ) pose_err += j_pose_err pose_reward = np . exp ( - self . _pose_err_scale * pose_err ) return pose_reward