text: import numpy as np
  
  
  def leg_forward_kinematics(alpha, leg_index, config):
      """Find the body-centric coordinates of a given foot given the joint angles.
      Parameters
      ----------
      alpha : Numpy array (3)
          Joint angles ordered as (abduction, hip, knee)
      leg_index : int
          Leg index.
      config : Config object
          Robot parameters object
      Returns
      -------
      Numpy array (3)
          Body-centric coordinates of the specified foot
      """
      pass
  
  
  def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config):
      """Find the joint angles corresponding to the given body-relative foot position for a given leg and configuration
      Parameters
      ----------
      r_body_foot : [type]
          [description]
      leg_index : [type]
          [description]
      config : [type]
          [description]
      Returns
      -------
      numpy array (3)
          Array of corresponding joint angles.
      """
      (x, y, z) = r_body_foot
  
      # Distance from the leg origin to the foot, projected into the y-z plane
      R_body_foot_yz = (y ** 2 + z ** 2) ** 0.5
  
      # Distance from the leg's forward/back point of rotation to the foot
      R_hip_foot_yz = (R_body_foot_yz ** 2 - config.ABDUCTION_OFFSET ** 2) ** 0.5
  
      # Interior angle of the right triangle formed in the y-z plane by the leg that is coincident to the ab/adduction axis
      # For feet 2 (front left) and 4 (back left), the abduction offset is positive, for the right feet, the abduction offset is negative.
      cos_param = config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz
      if abs(cos_param) > 0.9:
          print("Clipping 1st cos param")
      cos_param = np.clip(cos_param, -0.9, 0.9)
      phi = np.arccos(cos_param)
  
      # Angle of the y-z projection of the hip-to-foot vector, relative to the positive y-axis
      hip_foot_angle = np.arctan2(z, y)
  
      # Ab/adduction angle, relative to the positive y-axis
      abduction_angle = phi + hip_foot_angle
  
      # theta: Angle between the tilted negative z-axis and the hip-to-foot vector
      theta = np.arctan2(-x, R_hip_foot_yz)
  
      # Distance between the hip and foot
      R_hip_foot = (R_hip_foot_yz ** 2 + x ** 2) ** 0.5
  
      # Using law of cosines to determine the angle between upper leg links
      cos_param = (config.UPPER_LEG ** 2 + R_hip_foot ** 2 - config.LOWER_LEG ** 2) / (2.0*config.UPPER_LEG*R_hip_foot)
  
      # Ensure that the leg isn't over or under extending
      cos_param = np.clip(cos_param, -0.9, 0.9)
      if abs(cos_param) > 0.9:
          print("Clipping 2nd cos param")
  
      # gamma: Angle between upper leg links and the center of the leg
      gamma = np.arccos(cos_param)
  
      return np.array([abduction_angle, theta - gamma, theta + gamma])
  
  
  def four_legs_inverse_kinematics(r_body_foot, config):
      """Find the joint angles for all twelve DOF correspoinding to the given matrix of body-relative foot positions.
      Parameters
      ----------
      r_body_foot : numpy array (3,4)
          Matrix of the body-frame foot positions. Each column corresponds to a separate foot.
      config : Config object
          Object of robot configuration parameters.
      Returns
      -------
      numpy array (3,4)
          Matrix of corresponding joint angles.
      """
      alpha = np.zeros((3, 4))
      for i in range(4):
          body_offset = config.LEG_ORIGINS[:, i]
          alpha[:, i] = leg_explicit_inverse_kinematics(
              r_body_foot[:, i] - body_offset, i, config
          )
      return alpha