def convert_to_motor_action(
robot: typing.Any,
action: typing.Sequence[float],
control_mode: robot_config.MotorControlMode,
):
"""Converts the input action to generic MotorAction classes.
Args:
robot: An robot instance.
action: The motor commands sent to the robot.
control_mode: The motor control mode.
Returns:
The list of converted MotorAction instances.
"""
motor_action_list = []
if control_mode == robot_config.MotorControlMode.POSITION:
for motor_id, position in enumerate(action):
motor_action_list.append(
data_types.MotorAction(
timestamp=robot.last_action_time,
position=position,
position_gain=robot.GetMotorPositionGains()[motor_id],
velocity=0,
velocity_gain=robot.GetMotorVelocityGains()[motor_id],
torque=0))
if (control_mode == robot_config.MotorControlMode.TORQUE
or control_mode == robot_config.MotorControlMode.PWM):
for motor_id, torque in enumerate(action):
motor_action_list.append(
data_types.MotorAction(timestamp=robot.last_action_time,
position=0,
position_gain=0,
velocity=0,
velocity_gain=0,
torque=torque))
if control_mode == robot_config.MotorControlMode.HYBRID:
for motor_id in range(robot.num_motors):
position_index = (motor_id * robot_config.HYBRID_ACTION_DIMENSION +
robot_config.HybridActionIndex.POSITION.value)
position_gain_index = (
motor_id * robot_config.HYBRID_ACTION_DIMENSION +
robot_config.HybridActionIndex.POSITION_GAIN.value)
velocity_index = (motor_id * robot_config.HYBRID_ACTION_DIMENSION +
robot_config.HybridActionIndex.VELOCITY.value)
velocity_gain_index = (
motor_id * robot_config.HYBRID_ACTION_DIMENSION +
robot_config.HybridActionIndex.VELOCITY_GAIN.value)
torque_index = (motor_id * robot_config.HYBRID_ACTION_DIMENSION +
robot_config.HybridActionIndex.TORQUE.value)
motor_action_list.append(
data_types.MotorAction(
timestamp=robot.last_action_time,
position=action[position_index],
position_gain=action[position_gain_index],
velocity=action[velocity_index],
velocity_gain=action[velocity_gain_index],
torque=action[torque_index]))
return motor_action_list
