(leg_id, swing, extension, noise_stdev=0)
return motor0, motor1
Swing - extension based leg model for minitaur.

  Swing extension leg model calculates motor positions using 2 separate motions:
  swing and extension. Swing rotates the whole leg by rotating both motors
  equally towards same direction. Extension increases or decreases the length
  of the leg by turning both motors equally in opposite direction.

  This method also does the same conversion as leg_pose_to_motor_angles, but 0
  swing and 0 extension maps to a neutral standing still motor positions with
  motors at + or - pi.
  Args:
      leg_id: The id of the leg that the conversion is made for (0, 1, 2, 3).
      swing: Swing degree for the leg (in radians). 0 means perpendicular to the
        body).
      extension: Extension level (length) of the leg, limited to [-1, 1].
      noise_stdev: Standard deviation of the introduced noise at the motor
        position level. Noise is turned off by default.

  Returns:
    motor0: Position for the first motor for that leg.
    motor1: Position for the second motor for that leg.
  Raises:
    ValueError: In case calculated positions are outside the allowed boundaries.
Swing - extension based leg model for minitaur.
Swing - extension based leg model for minitaur .
def swing_extend_to_motor_angles(leg_id, swing, extension, noise_stdev=0):
  """Swing - extension based leg model for minitaur.

  Swing extension leg model calculates motor positions using 2 separate motions:
  swing and extension. Swing rotates the whole leg by rotating both motors
  equally towards same direction. Extension increases or decreases the length
  of the leg by turning both motors equally in opposite direction.

  This method also does the same conversion as leg_pose_to_motor_angles, but 0
  swing and 0 extension maps to a neutral standing still motor positions with
  motors at + or - pi.
  Args:
      leg_id: The id of the leg that the conversion is made for (0, 1, 2, 3).
      swing: Swing degree for the leg (in radians). 0 means perpendicular to the
        body).
      extension: Extension level (length) of the leg, limited to [-1, 1].
      noise_stdev: Standard deviation of the introduced noise at the motor
        position level. Noise is turned off by default.

  Returns:
    motor0: Position for the first motor for that leg.
    motor1: Position for the second motor for that leg.
  Raises:
    ValueError: In case calculated positions are outside the allowed boundaries.
  """
  # Check if the leg_id is in valid range
  if not 0 <= leg_id <= 3:
    raise ValueError('leg {} does not exist for a quadruped.'.format(leg_id))

  # Front legs can not swing too much towards the body.
  if leg_id % 2 == 0:
    swing = np.clip(swing, LEG_SWING_LIMIT_LOW, LEG_SWING_LIMIT_HIGH)
  # Back legs can not swing too much towards the body (opposite direction).
  else:
    swing = np.clip(swing, -LEG_SWING_LIMIT_HIGH, -LEG_SWING_LIMIT_LOW)

  # Check if the motors are too close or too far away to make it impossible
  # for the physical robot.
  gap = math.pi - 2 * extension
  if gap < MOTORS_GAP_LIMIT_LOW or gap > MOTORS_GAP_LIMIT_HIGH:
    top_extension = (math.pi - MOTORS_GAP_LIMIT_LOW) / 2.0
    least_extension = (math.pi - MOTORS_GAP_LIMIT_HIGH) / 2.0
    extension = np.clip(extension, least_extension, top_extension)

  # Initialization to neutral standing position where both motors point to
  # opposite directions
  motor0 = math.pi / 2
  motor1 = math.pi / 2
  # Rotational move
  if leg_id in (0, 1):
    motor0 += swing
    motor1 -= swing
  elif leg_id in (2, 3):
    motor0 -= swing
    motor1 += swing
  # Extension
  motor0 += extension
  motor1 += extension

  # Add noise if requested.
  if noise_stdev > 0:
    motor0 += np.random.normal(0, noise_stdev)
    motor1 += np.random.normal(0, noise_stdev)

  return motor0, motor1
def swing_extend_to_motor_angles ( leg_id , swing , extension , noise_stdev = 0 ) : # Check if the leg_id is in valid range if not 0 <= leg_id <= 3 : raise ValueError ( 'leg {} does not exist for a quadruped.' . format ( leg_id ) ) # Front legs can not swing too much towards the body. if leg_id % 2 == 0 : swing = np . clip ( swing , LEG_SWING_LIMIT_LOW , LEG_SWING_LIMIT_HIGH ) # Back legs can not swing too much towards the body (opposite direction). else : swing = np . clip ( swing , - LEG_SWING_LIMIT_HIGH , - LEG_SWING_LIMIT_LOW ) # Check if the motors are too close or too far away to make it impossible # for the physical robot. gap = math . pi - 2 * extension if gap < MOTORS_GAP_LIMIT_LOW or gap > MOTORS_GAP_LIMIT_HIGH : top_extension = ( math . pi - MOTORS_GAP_LIMIT_LOW ) / 2.0 least_extension = ( math . pi - MOTORS_GAP_LIMIT_HIGH ) / 2.0 extension = np . clip ( extension , least_extension , top_extension ) # Initialization to neutral standing position where both motors point to # opposite directions motor0 = math . pi / 2 motor1 = math . pi / 2 # Rotational move if leg_id in ( 0 , 1 ) : motor0 += swing motor1 -= swing elif leg_id in ( 2 , 3 ) : motor0 -= swing motor1 += swing # Extension motor0 += extension motor1 += extension # Add noise if requested. if noise_stdev > 0 : motor0 += np . random . normal ( 0 , noise_stdev ) motor1 += np . random . normal ( 0 , noise_stdev ) return motor0 , motor1