How to Add Custom Controller#

This tutorial guides you on how to add a custom controller for a robot.

Note that the controller cannot be operated independently. It must be used with robot to enable robot to act in the environment.

To add a custom controller, you need to:

Create a config class for controller config, inheriting from the internutopia.core.config.robot.ControllerCfg.
Create a class for controller, inheriting from the internutopia.core.robot.controller.BaseController.

In this tutorial we take Differential Drive Controller as an example to show how to add a custom controller.

Create Config Class#

Here’s an example of a config class for a controller:

from typing import Optional

from internutopia.core.config.robot import ControllerCfg


class DifferentialDriveControllerCfg(ControllerCfg):

    type: Optional[str] = 'DifferentialDriveController'
    wheel_radius: float
    wheel_base: float

For differential drive controller, we specify the type and add new fields wheel_radius and wheel_base to specify the radius and base of the wheels.

Create Controller Class#

In the simplest scenario, the following methods are required to be implemented in your controller class:

import numpy as np
from typing import List, Union

from internutopia.core.robot.articulation_action import ArticulationAction
from internutopia.core.robot.controller import BaseController
from internutopia.core.robot.robot import BaseRobot


@BaseController.register('DifferentialDriveController')
class DifferentialDriveController(BaseController):
    def __init__(self, config: DifferentialDriveControllerCfg, robot: BaseRobot, scene: IScene) -> None:
        """Initialize the controller with the given configuration and its owner robot.

        Args:
            config (DifferentialDriveControllerCfg): controller configuration.
            robot (BaseRobot): robot owning the controller.
            scene (IScene): scene interface.
        """

    def action_to_control(self, action: Union[np.ndarray, List]) -> ArticulationAction:
        """Convert input action (in 1d array format) to joint signals to apply.

        Args:
            action (Union[np.ndarray, List]): input control action.

        Returns:
            ArticulationAction: joint signals to apply
        """

The action_to_control method translates the input action into joint signals to apply in each step.

For complete list of controller methods, please refer to the Controller API documentation.

Please note that the registration of the controller class is done through the @BaseController.register decorator, and the registered name MUST match the value of type field within the corresponding controller config class (here is DifferentialDriveController).

Sometimes the calculation logic is defined in a method named forward to show the input parameters the controller accepts (which is common in our implementations), making it more human-readable. In this case, the action_to_control method itself only expands the parameters, and invokes forward method to calculate the joint signals.

An example of controller class implementation is shown as following:

from typing import List

import numpy as np

from internutopia.core.robot.articulation_action import ArticulationAction
from internutopia.core.robot.controller import BaseController
from internutopia.core.robot.robot import BaseRobot
from internutopia.core.scene.scene import IScene
from internutopia_extension.configs.controllers import DifferentialDriveControllerCfg


@BaseController.register('DifferentialDriveController')
class DifferentialDriveController(BaseController):
    def __init__(self, config: DifferentialDriveControllerCfg, robot: BaseRobot, scene: IScene) -> None:
        super().__init__(config=config, robot=robot, scene=scene)
        self._robot_scale = self.robot.get_robot_scale()[0]
        self._wheel_base = config.wheel_base * self._robot_scale
        self._wheel_radius = config.wheel_radius * self._robot_scale

    def forward(
        self,
        forward_speed: float = 0,
        rotation_speed: float = 0,
    ) -> ArticulationAction:
        left_wheel_vel = ((2 * forward_speed) - (rotation_speed * self._wheel_base)) / (2 * self._wheel_radius)
        right_wheel_vel = ((2 * forward_speed) + (rotation_speed * self._wheel_base)) / (2 * self._wheel_radius)
        # A controller has to return an ArticulationAction
        return ArticulationAction(joint_velocities=[left_wheel_vel, right_wheel_vel])

    def action_to_control(self, action: List | np.ndarray) -> ArticulationAction:
        """
        Args:
            action (List | np.ndarray): n-element 1d array containing:
              0. forward_speed (float)
              1. rotation_speed (float)
        """
        assert len(action) == 2, 'action must contain 2 elements'
        return self.forward(
            forward_speed=action[0],
            rotation_speed=action[1],
        )

You can check implementations of our controllers under internutopia_extension/controllers/, within which you can find:

Rule-based controllers: such as ik_controller, dd_controller.
Learning-based controllers: such as g1_move_by_speed_controller, h1_move_by_speed_controller.
Composite controllers: controllers chained with other controller, such as move_to_point_by_speed_controller, rotate_controller.

How to Add Custom Controller#

Create Config Class#

Create Controller Class#

This Page