(self)
return end_effector_reward
Get the end effector reward.
Get the end effector reward.
Get the end effector reward .
def _calc_reward_end_effector(self):
    """Get the end effector reward."""
    env = self._env
    robot = env.robot
    sim_model = robot.quadruped
    ref_model = self._ref_model
    pyb = self._get_pybullet_client()

    root_pos_ref = self._get_ref_base_position()
    root_rot_ref = self._get_ref_base_rotation()
    root_pos_sim = self._get_sim_base_position()
    root_rot_sim = self._get_sim_base_rotation()

    heading_rot_ref = self._calc_heading_rot(root_rot_ref)
    heading_rot_sim = self._calc_heading_rot(root_rot_sim)
    inv_heading_rot_ref = transformations.quaternion_conjugate(heading_rot_ref)
    inv_heading_rot_sim = transformations.quaternion_conjugate(heading_rot_sim)

    end_eff_err = 0.0
    num_joints = self._get_num_joints()
    height_err_scale = self._end_effector_height_err_scale

    for j in range(num_joints):
      is_end_eff = (j in robot._foot_link_ids)
      if (is_end_eff):
        end_state_ref = pyb.getLinkState(ref_model, j)
        end_state_sim = pyb.getLinkState(sim_model, j)
        end_pos_ref = np.array(end_state_ref[0])
        end_pos_sim = np.array(end_state_sim[0])

        rel_end_pos_ref = end_pos_ref - root_pos_ref
        rel_end_pos_ref = pose3d.QuaternionRotatePoint(rel_end_pos_ref,
                                                       inv_heading_rot_ref)

        rel_end_pos_sim = end_pos_sim - root_pos_sim
        rel_end_pos_sim = pose3d.QuaternionRotatePoint(rel_end_pos_sim,
                                                       inv_heading_rot_sim)

        rel_end_pos_diff = rel_end_pos_ref - rel_end_pos_sim
        end_pos_diff_height = end_pos_ref[2] - end_pos_sim[2]

        end_pos_err = (
            rel_end_pos_diff[0] * rel_end_pos_diff[0] +
            rel_end_pos_diff[1] * rel_end_pos_diff[1] +
            height_err_scale * end_pos_diff_height * end_pos_diff_height)

        end_eff_err += end_pos_err

    end_effector_reward = np.exp(-self._end_effector_err_scale * end_eff_err)

    return end_effector_reward
def _calc_reward_end_effector ( self ) : env = self . _env robot = env . robot sim_model = robot . quadruped ref_model = self . _ref_model pyb = self . _get_pybullet_client ( ) root_pos_ref = self . _get_ref_base_position ( ) root_rot_ref = self . _get_ref_base_rotation ( ) root_pos_sim = self . _get_sim_base_position ( ) root_rot_sim = self . _get_sim_base_rotation ( ) heading_rot_ref = self . _calc_heading_rot ( root_rot_ref ) heading_rot_sim = self . _calc_heading_rot ( root_rot_sim ) inv_heading_rot_ref = transformations . quaternion_conjugate ( heading_rot_ref ) inv_heading_rot_sim = transformations . quaternion_conjugate ( heading_rot_sim ) end_eff_err = 0.0 num_joints = self . _get_num_joints ( ) height_err_scale = self . _end_effector_height_err_scale for j in range ( num_joints ) : is_end_eff = ( j in robot . _foot_link_ids ) if ( is_end_eff ) : end_state_ref = pyb . getLinkState ( ref_model , j ) end_state_sim = pyb . getLinkState ( sim_model , j ) end_pos_ref = np . array ( end_state_ref [ 0 ] ) end_pos_sim = np . array ( end_state_sim [ 0 ] ) rel_end_pos_ref = end_pos_ref - root_pos_ref rel_end_pos_ref = pose3d . QuaternionRotatePoint ( rel_end_pos_ref , inv_heading_rot_ref ) rel_end_pos_sim = end_pos_sim - root_pos_sim rel_end_pos_sim = pose3d . QuaternionRotatePoint ( rel_end_pos_sim , inv_heading_rot_sim ) rel_end_pos_diff = rel_end_pos_ref - rel_end_pos_sim end_pos_diff_height = end_pos_ref [ 2 ] - end_pos_sim [ 2 ] end_pos_err = ( rel_end_pos_diff [ 0 ] * rel_end_pos_diff [ 0 ] + rel_end_pos_diff [ 1 ] * rel_end_pos_diff [ 1 ] + height_err_scale * end_pos_diff_height * end_pos_diff_height ) end_eff_err += end_pos_err end_effector_reward = np . exp ( - self . _end_effector_err_scale * end_eff_err ) return end_effector_reward