def goal_reachable()

in robogym/envs/dactyl/goals/face_cube_solver.py [0:0]

• 23 lines of code
• 7 McCabe index (conditional complexity)

    def goal_reachable(self, goal_state, current_state):
        """ Check if goal is in reach from current state."""
        relative_goal = self.relative_goal(goal_state, current_state)
        face_rotation_angles = relative_goal["cube_face_angle"]
        goal_type = goal_state["goal_type"]
        eps = 1e-6

        rounded_rotation_angles = rotation.round_to_straight_angles(
            np.abs(rotation.normalize_angles(face_rotation_angles))
        )

        rotated_faces = list(np.where(rounded_rotation_angles > eps)[0])

        if goal_type == "rotation":
            # When doing face rotation, three conditions should met:
            # 1. Goal face should face up
            # 2. Rounded rotation angle for goal face should be at most 90 degree.
            # 3. Rounded rotation angle for other faces should be 0.
            goal_face_idx = self._get_goal_action().face_idx
            face_up = cube_utils.face_up(
                self.mujoco_simulation.sim, self.face_geom_names
            )

            return (
                goal_face_idx == face_up
                and rounded_rotation_angles[goal_face_idx] < np.pi / 2 + eps
                and rotated_faces in ([], [goal_face_idx])
            )
        elif goal_type == "flip":
            # When doing flipping, rounded rotation angles should be 0.
            return len(rotated_faces) == 0
        else:
            raise ValueError('unknown goal_type "{}"'.format(goal_type))