(self,
                        motor_commands,
                        motor_angle,
                        motor_velocity,
                        true_motor_velocity,
                        motor_control_mode=None)
return motor_torques, motor_torques
Convert the commands (position control or torque control) to torque.

    Args:
      motor_commands: The desired motor angle if the motor is in position
        control mode. The pwm signal if the motor is in torque control mode.
      motor_angle: The motor angle observed at the current time step. It is
        actually the true motor angle observed a few milliseconds ago (pd
        latency).
      motor_velocity: The motor velocity observed at the current time step, it
        is actually the true motor velocity a few milliseconds ago (pd latency).
      true_motor_velocity: The true motor velocity. The true velocity is used to
        compute back EMF voltage and viscous damping.
      motor_control_mode: A MotorControlMode enum.

    Returns:
      actual_torque: The torque that needs to be applied to the motor.
      observed_torque: The torque observed by the sensor.
Convert the commands (position control or torque control) to torque.
Convert the commands ( position control or torque control ) to torque .
def convert_to_torque(self,
                        motor_commands,
                        motor_angle,
                        motor_velocity,
                        true_motor_velocity,
                        motor_control_mode=None):
    """Convert the commands (position control or torque control) to torque.

    Args:
      motor_commands: The desired motor angle if the motor is in position
        control mode. The pwm signal if the motor is in torque control mode.
      motor_angle: The motor angle observed at the current time step. It is
        actually the true motor angle observed a few milliseconds ago (pd
        latency).
      motor_velocity: The motor velocity observed at the current time step, it
        is actually the true motor velocity a few milliseconds ago (pd latency).
      true_motor_velocity: The true motor velocity. The true velocity is used to
        compute back EMF voltage and viscous damping.
      motor_control_mode: A MotorControlMode enum.

    Returns:
      actual_torque: The torque that needs to be applied to the motor.
      observed_torque: The torque observed by the sensor.
    """
    del true_motor_velocity
    if not motor_control_mode:
      motor_control_mode = self._motor_control_mode

    # No processing for motor torques
    if motor_control_mode is MOTOR_CONTROL_TORQUE:
      assert len(motor_commands) == NUM_MOTORS
      motor_torques = self._strength_ratios * motor_commands
      return motor_torques, motor_torques

    desired_motor_angles = None
    desired_motor_velocities = None
    kp = None
    kd = None
    additional_torques = np.full(NUM_MOTORS, 0)
    if motor_control_mode is MOTOR_CONTROL_POSITION:
      assert len(motor_commands) == NUM_MOTORS
      kp = self._kp
      kd = self._kd
      desired_motor_angles = motor_commands
      desired_motor_velocities = np.full(NUM_MOTORS, 0)
    elif motor_control_mode is MOTOR_CONTROL_HYBRID:
      # The input should be a 60 dimension vector
      assert len(motor_commands) == MOTOR_COMMAND_DIMENSION * NUM_MOTORS
      kp = motor_commands[POSITION_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
      kd = motor_commands[VELOCITY_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
      desired_motor_angles = motor_commands[
          POSITION_INDEX::MOTOR_COMMAND_DIMENSION]
      desired_motor_velocities = motor_commands[
          VELOCITY_INDEX::MOTOR_COMMAND_DIMENSION]
      additional_torques = motor_commands[TORQUE_INDEX::MOTOR_COMMAND_DIMENSION]
    motor_torques = -1 * (kp * (motor_angle - desired_motor_angles)) - kd * (
        motor_velocity - desired_motor_velocities) + additional_torques
    motor_torques = self._strength_ratios * motor_torques
    if self._torque_limits is not None:
      if len(self._torque_limits) != len(motor_torques):
        raise ValueError(
            "Torque limits dimension does not match the number of motors.")
      motor_torques = np.clip(motor_torques, -1.0 * self._torque_limits,
                              self._torque_limits)

    return motor_torques, motor_torques
def convert_to_torque ( self , motor_commands , motor_angle , motor_velocity , true_motor_velocity , motor_control_mode = None ) : del true_motor_velocity if not motor_control_mode : motor_control_mode = self . _motor_control_mode # No processing for motor torques if motor_control_mode is MOTOR_CONTROL_TORQUE : assert len ( motor_commands ) == NUM_MOTORS motor_torques = self . _strength_ratios * motor_commands return motor_torques , motor_torques desired_motor_angles = None desired_motor_velocities = None kp = None kd = None additional_torques = np . full ( NUM_MOTORS , 0 ) if motor_control_mode is MOTOR_CONTROL_POSITION : assert len ( motor_commands ) == NUM_MOTORS kp = self . _kp kd = self . _kd desired_motor_angles = motor_commands desired_motor_velocities = np . full ( NUM_MOTORS , 0 ) elif motor_control_mode is MOTOR_CONTROL_HYBRID : # The input should be a 60 dimension vector assert len ( motor_commands ) == MOTOR_COMMAND_DIMENSION * NUM_MOTORS kp = motor_commands [ POSITION_GAIN_INDEX : : MOTOR_COMMAND_DIMENSION ] kd = motor_commands [ VELOCITY_GAIN_INDEX : : MOTOR_COMMAND_DIMENSION ] desired_motor_angles = motor_commands [ POSITION_INDEX : : MOTOR_COMMAND_DIMENSION ] desired_motor_velocities = motor_commands [ VELOCITY_INDEX : : MOTOR_COMMAND_DIMENSION ] additional_torques = motor_commands [ TORQUE_INDEX : : MOTOR_COMMAND_DIMENSION ] motor_torques = - 1 * ( kp * ( motor_angle - desired_motor_angles ) ) - kd * ( motor_velocity - desired_motor_velocities ) + additional_torques motor_torques = self . _strength_ratios * motor_torques if self . _torque_limits is not None : if len ( self . _torque_limits ) != len ( motor_torques ) : raise ValueError ( "Torque limits dimension does not match the number of motors." ) motor_torques = np . clip ( motor_torques , - 1.0 * self . _torque_limits , self . _torque_limits ) return motor_torques , motor_torques