Skip to content

dd_controller

DifferentialDriveController

Bases: LocomotionController

Differential drive (DD) controller for controlling two independently controlled wheeled joints.

Each controller step consists of the following
  1. Clip + Scale inputted command according to @command_input_limits and @command_output_limits
  2. Convert desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control signals
  3. Clips the resulting command by the joint velocity limits
Source code in omnigibson/controllers/dd_controller.py 
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
class DifferentialDriveController(LocomotionController):
    """
    Differential drive (DD) controller for controlling two independently controlled wheeled joints.

    Each controller step consists of the following:
        1. Clip + Scale inputted command according to @command_input_limits and @command_output_limits
        2. Convert desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control signals
        3. Clips the resulting command by the joint velocity limits
    """

    def __init__(
        self,
        wheel_radius,
        wheel_axle_length,
        control_freq,
        control_limits,
        dof_idx,
        command_input_limits="default",
        command_output_limits="default",
    ):
        """
        Args:
            wheel_radius (float): radius of the wheels (both assumed to be same radius)
            wheel_axle_length (float): perpendicular distance between the two wheels
            control_freq (int): controller loop frequency
            control_limits (Dict[str, Tuple[Array[float], Array[float]]]): The min/max limits to the outputted
                control signal. Should specify per-dof type limits, i.e.:

                "position": [[min], [max]]
                "velocity": [[min], [max]]
                "effort": [[min], [max]]
                "has_limit": [...bool...]

                Values outside of this range will be clipped, if the corresponding joint index in has_limit is True.
            dof_idx (Array[int]): specific dof indices controlled by this robot. Used for inferring
                controller-relevant values during control computations
            command_input_limits (None or "default" or Tuple[float, float] or Tuple[Array[float], Array[float]]):
                if set, is the min/max acceptable inputted command. Values outside this range will be clipped.
                If None, no clipping will be used. If "default", range will be set to (-1, 1)
            command_output_limits (None or "default" or Tuple[float, float] or Tuple[Array[float], Array[float]]):
                if set, is the min/max scaled command. If both this value and @command_input_limits is not None,
                then all inputted command values will be scaled from the input range to the output range.
                If either is None, no scaling will be used. If "default", then this range will automatically be set
                to the maximum linear and angular velocities calculated from @wheel_radius, @wheel_axle_length, and
                @control_limits velocity limits entry
        """
        # Store internal variables
        self._wheel_radius = wheel_radius
        self._wheel_axle_halflength = wheel_axle_length / 2.0

        # If we're using default command output limits, map this to maximum linear / angular velocities
        if command_output_limits == "default":
            min_vels = control_limits["velocity"][0][dof_idx]
            assert (
                min_vels[0] == min_vels[1]
            ), "Differential drive requires both wheel joints to have same min velocities!"
            max_vels = control_limits["velocity"][1][dof_idx]
            assert (
                max_vels[0] == max_vels[1]
            ), "Differential drive requires both wheel joints to have same max velocities!"
            assert abs(min_vels[0]) == abs(
                max_vels[0]
            ), "Differential drive requires both wheel joints to have same min and max absolute velocities!"
            max_lin_vel = max_vels[0] * wheel_radius
            max_ang_vel = max_lin_vel * 2.0 / wheel_axle_length
            command_output_limits = ((-max_lin_vel, -max_ang_vel), (max_lin_vel, max_ang_vel))

        # Run super init
        super().__init__(
            control_freq=control_freq,
            control_limits=control_limits,
            dof_idx=dof_idx,
            command_input_limits=command_input_limits,
            command_output_limits=command_output_limits,
        )

    def _update_goal(self, command, control_dict):
        # Directly store command as the velocity goal
        return dict(vel=command)

    def compute_control(self, goal_dict, control_dict):
        """
        Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal.
        This processes converts the desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control
        signals.

        Args:
            goal_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
                goals necessary for controller computation. Must include the following keys:
                    vel: desired (lin_vel, ang_vel) of the controlled body
            control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
                states necessary for controller computation. Must include the following keys:

        Returns:
            Array[float]: outputted (non-clipped!) velocity control signal to deploy
                to the [left, right] wheel joints
        """
        lin_vel, ang_vel = goal_dict["vel"]

        # Convert to wheel velocities
        left_wheel_joint_vel = (lin_vel - ang_vel * self._wheel_axle_halflength) / self._wheel_radius
        right_wheel_joint_vel = (lin_vel + ang_vel * self._wheel_axle_halflength) / self._wheel_radius

        # Return desired velocities
        return np.array([left_wheel_joint_vel, right_wheel_joint_vel])

    def compute_no_op_goal(self, control_dict):
        # This is zero-vector, since we want zero linear / angular velocity
        return dict(vel=np.zeros(2))

    def _get_goal_shapes(self):
        # Add (2, )-array representing linear, angular velocity
        return dict(vel=(2,))

    @property
    def control_type(self):
        return ControlType.VELOCITY

    @property
    def command_dim(self):
        # [lin_vel, ang_vel]
        return 2

__init__(wheel_radius, wheel_axle_length, control_freq, control_limits, dof_idx, command_input_limits='default', command_output_limits='default')

Parameters:

Name Type Description Default
wheel_radius float

radius of the wheels (both assumed to be same radius)

 required
wheel_axle_length float

perpendicular distance between the two wheels

 required
control_freq int

controller loop frequency

 required
control_limits Dict[str, Tuple[Array[float], Array[float]]]

The min/max limits to the outputted control signal. Should specify per-dof type limits, i.e.:

"position": [[min], [max]] "velocity": [[min], [max]] "effort": [[min], [max]] "has_limit": [...bool...]

Values outside of this range will be clipped, if the corresponding joint index in has_limit is True.

 required
dof_idx Array[int]

specific dof indices controlled by this robot. Used for inferring controller-relevant values during control computations

 required
command_input_limits None or default or Tuple[float, float] or Tuple[Array[float], Array[float]]

if set, is the min/max acceptable inputted command. Values outside this range will be clipped. If None, no clipping will be used. If "default", range will be set to (-1, 1)

 'default'
command_output_limits None or default or Tuple[float, float] or Tuple[Array[float], Array[float]]

if set, is the min/max scaled command. If both this value and @command_input_limits is not None, then all inputted command values will be scaled from the input range to the output range. If either is None, no scaling will be used. If "default", then this range will automatically be set to the maximum linear and angular velocities calculated from @wheel_radius, @wheel_axle_length, and @control_limits velocity limits entry

 'default'
Source code in omnigibson/controllers/dd_controller.py 
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
def __init__(
    self,
    wheel_radius,
    wheel_axle_length,
    control_freq,
    control_limits,
    dof_idx,
    command_input_limits="default",
    command_output_limits="default",
):
    """
    Args:
        wheel_radius (float): radius of the wheels (both assumed to be same radius)
        wheel_axle_length (float): perpendicular distance between the two wheels
        control_freq (int): controller loop frequency
        control_limits (Dict[str, Tuple[Array[float], Array[float]]]): The min/max limits to the outputted
            control signal. Should specify per-dof type limits, i.e.:

            "position": [[min], [max]]
            "velocity": [[min], [max]]
            "effort": [[min], [max]]
            "has_limit": [...bool...]

            Values outside of this range will be clipped, if the corresponding joint index in has_limit is True.
        dof_idx (Array[int]): specific dof indices controlled by this robot. Used for inferring
            controller-relevant values during control computations
        command_input_limits (None or "default" or Tuple[float, float] or Tuple[Array[float], Array[float]]):
            if set, is the min/max acceptable inputted command. Values outside this range will be clipped.
            If None, no clipping will be used. If "default", range will be set to (-1, 1)
        command_output_limits (None or "default" or Tuple[float, float] or Tuple[Array[float], Array[float]]):
            if set, is the min/max scaled command. If both this value and @command_input_limits is not None,
            then all inputted command values will be scaled from the input range to the output range.
            If either is None, no scaling will be used. If "default", then this range will automatically be set
            to the maximum linear and angular velocities calculated from @wheel_radius, @wheel_axle_length, and
            @control_limits velocity limits entry
    """
    # Store internal variables
    self._wheel_radius = wheel_radius
    self._wheel_axle_halflength = wheel_axle_length / 2.0

    # If we're using default command output limits, map this to maximum linear / angular velocities
    if command_output_limits == "default":
        min_vels = control_limits["velocity"][0][dof_idx]
        assert (
            min_vels[0] == min_vels[1]
        ), "Differential drive requires both wheel joints to have same min velocities!"
        max_vels = control_limits["velocity"][1][dof_idx]
        assert (
            max_vels[0] == max_vels[1]
        ), "Differential drive requires both wheel joints to have same max velocities!"
        assert abs(min_vels[0]) == abs(
            max_vels[0]
        ), "Differential drive requires both wheel joints to have same min and max absolute velocities!"
        max_lin_vel = max_vels[0] * wheel_radius
        max_ang_vel = max_lin_vel * 2.0 / wheel_axle_length
        command_output_limits = ((-max_lin_vel, -max_ang_vel), (max_lin_vel, max_ang_vel))

    # Run super init
    super().__init__(
        control_freq=control_freq,
        control_limits=control_limits,
        dof_idx=dof_idx,
        command_input_limits=command_input_limits,
        command_output_limits=command_output_limits,
    )

compute_control(goal_dict, control_dict)

Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal. This processes converts the desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control signals.

Parameters:

Name Type Description Default
goal_dict Dict[str, Any]

dictionary that should include any relevant keyword-mapped goals necessary for controller computation. Must include the following keys: vel: desired (lin_vel, ang_vel) of the controlled body

 required
control_dict Dict[str, Any]

dictionary that should include any relevant keyword-mapped states necessary for controller computation. Must include the following keys:

 required

Returns:

Type Description

Array[float]: outputted (non-clipped!) velocity control signal to deploy to the [left, right] wheel joints
Source code in omnigibson/controllers/dd_controller.py 
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
def compute_control(self, goal_dict, control_dict):
    """
    Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal.
    This processes converts the desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control
    signals.

    Args:
        goal_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
            goals necessary for controller computation. Must include the following keys:
                vel: desired (lin_vel, ang_vel) of the controlled body
        control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
            states necessary for controller computation. Must include the following keys:

    Returns:
        Array[float]: outputted (non-clipped!) velocity control signal to deploy
            to the [left, right] wheel joints
    """
    lin_vel, ang_vel = goal_dict["vel"]

    # Convert to wheel velocities
    left_wheel_joint_vel = (lin_vel - ang_vel * self._wheel_axle_halflength) / self._wheel_radius
    right_wheel_joint_vel = (lin_vel + ang_vel * self._wheel_axle_halflength) / self._wheel_radius

    # Return desired velocities
    return np.array([left_wheel_joint_vel, right_wheel_joint_vel])