Skip to content

controller_base

BaseController

Bases: Serializable, Registerable, Recreatable

An abstract class with interface for mapping specific types of commands to deployable control signals.

Source code in omnigibson/controllers/controller_base.py 
 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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
class BaseController(Serializable, Registerable, Recreatable):
    """
    An abstract class with interface for mapping specific types of commands to deployable control signals.
    """

    def __init__(
        self,
        control_freq,
        control_limits,
        dof_idx,
        command_input_limits="default",
        command_output_limits="default",
    ):
        """
        Args:
            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 @control_limits entry corresponding to self.control_type
        """
        # Store arguments
        self._control_freq = control_freq
        self._control_limits = {}
        for motor_type in {"position", "velocity", "effort"}:
            if motor_type not in control_limits:
                continue

            self._control_limits[ControlType.get_type(motor_type)] = [
                np.array(control_limits[motor_type][0]),
                np.array(control_limits[motor_type][1]),
            ]
        assert "has_limit" in control_limits, "Expected has_limit specified in control_limits, but does not exist."
        self._dof_has_limits = control_limits["has_limit"]
        self._dof_idx = np.array(dof_idx, dtype=int)

        # Initialize some other variables that will be filled in during runtime
        self._control = None
        self._command = None
        self._command_scale_factor = None
        self._command_output_transform = None
        self._command_input_transform = None

        # Standardize command input / output limits to be (min_array, max_array)
        command_input_limits = (-1.0, 1.0) if command_input_limits == "default" else command_input_limits
        command_output_limits = (
            (
                np.array(self._control_limits[self.control_type][0])[self.dof_idx],
                np.array(self._control_limits[self.control_type][1])[self.dof_idx],
            )
            if command_output_limits == "default"
            else command_output_limits
        )
        self._command_input_limits = (
            None
            if command_input_limits is None
            else (
                self.nums2array(command_input_limits[0], self.command_dim),
                self.nums2array(command_input_limits[1], self.command_dim),
            )
        )
        self._command_output_limits = (
            None
            if command_output_limits is None
            else (
                self.nums2array(command_output_limits[0], self.command_dim),
                self.nums2array(command_output_limits[1], self.command_dim),
            )
        )

    def _preprocess_command(self, command):
        """
        Clips + scales inputted @command according to self.command_input_limits and self.command_output_limits.
        If self.command_input_limits is None, then no clipping will occur. If either self.command_input_limits
        or self.command_output_limits is None, then no scaling will occur.

        Args:
            command (Array[float] or float): Inputted command vector

        Returns:
            Array[float]: Processed command vector
        """
        # Make sure command is a np.array
        command = np.array([command]) if type(command) in {int, float} else np.array(command)
        # We only clip and / or scale if self.command_input_limits exists
        if self._command_input_limits is not None:
            # Clip
            command = command.clip(*self._command_input_limits)
            if self._command_output_limits is not None:
                # If we haven't calculated how to scale the command, do that now (once)
                if self._command_scale_factor is None:
                    self._command_scale_factor = abs(
                        self._command_output_limits[1] - self._command_output_limits[0]
                    ) / abs(self._command_input_limits[1] - self._command_input_limits[0])
                    self._command_output_transform = (
                        self._command_output_limits[1] + self._command_output_limits[0]
                    ) / 2.0
                    self._command_input_transform = (self._command_input_limits[1] + self._command_input_limits[0]) / 2.0
                # Scale command
                command = (
                    command - self._command_input_transform
                ) * self._command_scale_factor + self._command_output_transform

        # Return processed command
        return command

    def update_command(self, command):
        """
        Updates inputted @command internally.

        Args:
            command (Array[float]): inputted command to store internally in this controller
        """
        # Sanity check the command
        assert len(command) == self.command_dim, "Commands must be dimension {}, got dim {} instead.".format(
            self.command_dim, len(command)
        )
        # Preprocess and store inputted command
        self._command = self._preprocess_command(np.array(command))

    def clip_control(self, control):
        """
        Clips the inputted @control signal based on @control_limits.

        Args:
            control (Array[float]): control signal to clip

        Returns:
            Array[float]: Clipped control signal
        """
        clipped_control = control.clip(
            self._control_limits[self.control_type][0][self.dof_idx],
            self._control_limits[self.control_type][1][self.dof_idx],
        )
        idx = (
            self._dof_has_limits[self.dof_idx]
            if self.control_type == ControlType.POSITION
            else [True] * self.control_dim
        )
        control[idx] = clipped_control[idx]
        return control

    def step(self, control_dict):
        """
        Take a controller step.

        Args:
            control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
                states necessary for controller computation

        Returns:
            Array[float]: numpy array of outputted control signals
        """
        control = self._command_to_control(command=self._command, control_dict=control_dict)
        self._control = self.clip_control(control=control)
        return self._control

    def reset(self):
        """
        Resets this controller. Should be implemented by subclass.
        """
        raise NotImplementedError

    def _dump_state(self):
        # Default is no state (empty dict)
        return dict()

    def _load_state(self, state):
        # Default is no state (empty dict), so this is a no-op
        pass

    def _serialize(self, state):
        # Default is no state, so do nothing
        return np.array([])

    def _deserialize(self, state):
        # Default is no state, so do nothing
        return dict(), 0

    def _command_to_control(self, command, control_dict):
        """
        Converts the (already preprocessed) inputted @command into deployable (non-clipped!) control signal.
        Should be implemented by subclass.

        Args:
            command (Array[float]): desired (already preprocessed) command to convert into control signals
            control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
                states necessary for controller computation

        Returns:
            Array[float]: outputted (non-clipped!) control signal to deploy
        """
        raise NotImplementedError

    @staticmethod
    def nums2array(nums, dim):
        """
        Convert input @nums into numpy array of length @dim. If @nums is a single number, broadcasts it to the
        corresponding dimension size @dim before converting into a numpy array

        Args:
            nums (numeric or Iterable): Either single value or array of numbers
            dim (int): Size of array to broadcast input to

        Returns:
            np.array: Array filled with values specified in @nums
        """
        # First run sanity check to make sure no strings are being inputted
        if isinstance(nums, str):
            raise TypeError("Error: Only numeric inputs are supported for this function, nums2array!")

        # Check if input is an Iterable, if so, we simply convert the input to np.array and return
        # Else, input is a single value, so we map to a numpy array of correct size and return
        return np.array(nums) if isinstance(nums, Iterable) else np.ones(dim) * nums

    @property
    def state_size(self):
        # Default is no state, so return 0
        return 0

    @property
    def control(self):
        """
        Returns:
            n-array: Array of most recent controls deployed by this controller
        """
        return self._control

    @property
    def control_freq(self):
        """
        Returns:
            float: Control frequency (Hz) of this controller
        """
        return self._control_freq

    @property
    def control_dim(self):
        """
        Returns:
            int: Expected size of outputted controls
        """
        return len(self.dof_idx)

    @property
    def control_type(self):
        """
        Returns:
            ControlType: Type of control returned by this controller
        """
        raise NotImplementedError

    @property
    def command_input_limits(self):
        """
        Returns:
            None or 2-tuple: If specified, returns (min, max) command input limits for this controller, where
                @min and @max are numpy float arrays of length self.command_dim. Otherwise, returns None
        """
        return self._command_input_limits

    @property
    def command_output_limits(self):
        """
        Returns:
            None or 2-tuple: If specified, returns (min, max) command output limits for this controller, where
                @min and @max are numpy float arrays of length self.command_dim. Otherwise, returns None
        """
        return self._command_output_limits

    @property
    def command_dim(self):
        """
        Returns:
            int: Expected size of inputted commands
        """
        raise NotImplementedError

    @property
    def dof_idx(self):
        """
        Returns:
            Array[int]: DOF indices corresponding to the specific DOFs being controlled by this robot
        """
        return np.array(self._dof_idx)

    @classproperty
    def _do_not_register_classes(cls):
        # Don't register this class since it's an abstract template
        classes = super()._do_not_register_classes
        classes.add("BaseController")
        return classes

    @classproperty
    def _cls_registry(cls):
        # Global registry
        global REGISTERED_CONTROLLERS
        return REGISTERED_CONTROLLERS

command_dim property

Returns:

Name Type Description
int

Expected size of inputted commands

command_input_limits property

Returns:

Type Description

None or 2-tuple: If specified, returns (min, max) command input limits for this controller, where @min and @max are numpy float arrays of length self.command_dim. Otherwise, returns None

command_output_limits property

Returns:

Type Description

None or 2-tuple: If specified, returns (min, max) command output limits for this controller, where @min and @max are numpy float arrays of length self.command_dim. Otherwise, returns None

control property

Returns:

Type Description

n-array: Array of most recent controls deployed by this controller

control_dim property

Returns:

Name Type Description
int

Expected size of outputted controls

control_freq property

Returns:

Name Type Description
float

Control frequency (Hz) of this controller

control_type property

Returns:

Name Type Description
ControlType

Type of control returned by this controller

dof_idx property

Returns:

Type Description

Array[int]: DOF indices corresponding to the specific DOFs being controlled by this robot

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

Parameters:

Name Type Description Default
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 @control_limits entry corresponding to self.control_type

 'default'
Source code in omnigibson/controllers/controller_base.py 
 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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
def __init__(
    self,
    control_freq,
    control_limits,
    dof_idx,
    command_input_limits="default",
    command_output_limits="default",
):
    """
    Args:
        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 @control_limits entry corresponding to self.control_type
    """
    # Store arguments
    self._control_freq = control_freq
    self._control_limits = {}
    for motor_type in {"position", "velocity", "effort"}:
        if motor_type not in control_limits:
            continue

        self._control_limits[ControlType.get_type(motor_type)] = [
            np.array(control_limits[motor_type][0]),
            np.array(control_limits[motor_type][1]),
        ]
    assert "has_limit" in control_limits, "Expected has_limit specified in control_limits, but does not exist."
    self._dof_has_limits = control_limits["has_limit"]
    self._dof_idx = np.array(dof_idx, dtype=int)

    # Initialize some other variables that will be filled in during runtime
    self._control = None
    self._command = None
    self._command_scale_factor = None
    self._command_output_transform = None
    self._command_input_transform = None

    # Standardize command input / output limits to be (min_array, max_array)
    command_input_limits = (-1.0, 1.0) if command_input_limits == "default" else command_input_limits
    command_output_limits = (
        (
            np.array(self._control_limits[self.control_type][0])[self.dof_idx],
            np.array(self._control_limits[self.control_type][1])[self.dof_idx],
        )
        if command_output_limits == "default"
        else command_output_limits
    )
    self._command_input_limits = (
        None
        if command_input_limits is None
        else (
            self.nums2array(command_input_limits[0], self.command_dim),
            self.nums2array(command_input_limits[1], self.command_dim),
        )
    )
    self._command_output_limits = (
        None
        if command_output_limits is None
        else (
            self.nums2array(command_output_limits[0], self.command_dim),
            self.nums2array(command_output_limits[1], self.command_dim),
        )
    )

clip_control(control)

Clips the inputted @control signal based on @control_limits.

Parameters:

Name Type Description Default
control Array[float]

control signal to clip

 required

Returns:

Type Description

Array[float]: Clipped control signal
Source code in omnigibson/controllers/controller_base.py 
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
def clip_control(self, control):
    """
    Clips the inputted @control signal based on @control_limits.

    Args:
        control (Array[float]): control signal to clip

    Returns:
        Array[float]: Clipped control signal
    """
    clipped_control = control.clip(
        self._control_limits[self.control_type][0][self.dof_idx],
        self._control_limits[self.control_type][1][self.dof_idx],
    )
    idx = (
        self._dof_has_limits[self.dof_idx]
        if self.control_type == ControlType.POSITION
        else [True] * self.control_dim
    )
    control[idx] = clipped_control[idx]
    return control

nums2array(nums, dim) staticmethod

Convert input @nums into numpy array of length @dim. If @nums is a single number, broadcasts it to the corresponding dimension size @dim before converting into a numpy array

Parameters:

Name Type Description Default
nums numeric or Iterable

Either single value or array of numbers

 required
dim int

Size of array to broadcast input to

 required

Returns:

Type Description

np.array: Array filled with values specified in @nums
Source code in omnigibson/controllers/controller_base.py 
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
@staticmethod
def nums2array(nums, dim):
    """
    Convert input @nums into numpy array of length @dim. If @nums is a single number, broadcasts it to the
    corresponding dimension size @dim before converting into a numpy array

    Args:
        nums (numeric or Iterable): Either single value or array of numbers
        dim (int): Size of array to broadcast input to

    Returns:
        np.array: Array filled with values specified in @nums
    """
    # First run sanity check to make sure no strings are being inputted
    if isinstance(nums, str):
        raise TypeError("Error: Only numeric inputs are supported for this function, nums2array!")

    # Check if input is an Iterable, if so, we simply convert the input to np.array and return
    # Else, input is a single value, so we map to a numpy array of correct size and return
    return np.array(nums) if isinstance(nums, Iterable) else np.ones(dim) * nums

reset()

Resets this controller. Should be implemented by subclass.

Source code in omnigibson/controllers/controller_base.py 
237
238
239
240
241
def reset(self):
    """
    Resets this controller. Should be implemented by subclass.
    """
    raise NotImplementedError

step(control_dict)

Take a controller step.

Parameters:

Name Type Description Default
control_dict Dict[str, Any]

dictionary that should include any relevant keyword-mapped states necessary for controller computation

 required

Returns:

Type Description

Array[float]: numpy array of outputted control signals
Source code in omnigibson/controllers/controller_base.py 
222
223
224
225
226
227
228
229
230
231
232
233
234
235
def step(self, control_dict):
    """
    Take a controller step.

    Args:
        control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
            states necessary for controller computation

    Returns:
        Array[float]: numpy array of outputted control signals
    """
    control = self._command_to_control(command=self._command, control_dict=control_dict)
    self._control = self.clip_control(control=control)
    return self._control

update_command(command)

Updates inputted @command internally.

Parameters:

Name Type Description Default
command Array[float]

inputted command to store internally in this controller

 required
Source code in omnigibson/controllers/controller_base.py 
186
187
188
189
190
191
192
193
194
195
196
197
198
def update_command(self, command):
    """
    Updates inputted @command internally.

    Args:
        command (Array[float]): inputted command to store internally in this controller
    """
    # Sanity check the command
    assert len(command) == self.command_dim, "Commands must be dimension {}, got dim {} instead.".format(
        self.command_dim, len(command)
    )
    # Preprocess and store inputted command
    self._command = self._preprocess_command(np.array(command))

ControlType

Source code in omnigibson/controllers/controller_base.py 
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
class ControlType:
    NONE = -1
    POSITION = 0
    VELOCITY = 1
    EFFORT = 2
    _MAPPING = {
        "none": NONE,
        "position": POSITION,
        "velocity": VELOCITY,
        "effort": EFFORT,
    }
    VALID_TYPES = set(_MAPPING.values())
    VALID_TYPES_STR = set(_MAPPING.keys())

    @classmethod
    def get_type(cls, type_str):
        """
        Args:
            type_str (str): One of "position", "velocity", or "effort" (any case), and maps it
                to the corresponding type

        Returns:
            ControlType: control type corresponding to the associated string
        """
        assert_valid_key(key=type_str.lower(), valid_keys=cls._MAPPING, name="control type")
        return cls._MAPPING[type_str.lower()]

get_type(type_str) classmethod

Parameters:

Name Type Description Default
type_str str

One of "position", "velocity", or "effort" (any case), and maps it to the corresponding type

 required

Returns:

Name Type Description
ControlType

control type corresponding to the associated string
Source code in omnigibson/controllers/controller_base.py 
51
52
53
54
55
56
57
58
59
60
61
62
@classmethod
def get_type(cls, type_str):
    """
    Args:
        type_str (str): One of "position", "velocity", or "effort" (any case), and maps it
            to the corresponding type

    Returns:
        ControlType: control type corresponding to the associated string
    """
    assert_valid_key(key=type_str.lower(), valid_keys=cls._MAPPING, name="control type")
    return cls._MAPPING[type_str.lower()]

GripperController

Bases: BaseController

Controller to control a gripper. All implemented controllers that encompass gripper capabilities should extend from this class.

Source code in omnigibson/controllers/controller_base.py 
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
class GripperController(BaseController):
    """
    Controller to control a gripper. All implemented controllers that encompass gripper capabilities
    should extend from this class.
    """

    def __init_subclass__(cls, **kwargs):
        # Register as part of gripper controllers
        super().__init_subclass__(**kwargs)
        register_gripper_controller(cls)

    def is_grasping(self):
        """
        Checks whether the current state of this gripper being controlled is in a grasping state.
        Should be implemented by subclass.

        Returns:
            IsGraspingState: Grasping state of gripper
        """
        raise NotImplementedError()

    @classproperty
    def _do_not_register_classes(cls):
        # Don't register this class since it's an abstract template
        classes = super()._do_not_register_classes
        classes.add("GripperController")
        return classes

is_grasping()

Checks whether the current state of this gripper being controlled is in a grasping state. Should be implemented by subclass.

Returns:

Name Type Description
IsGraspingState

Grasping state of gripper
Source code in omnigibson/controllers/controller_base.py 
429
430
431
432
433
434
435
436
437
def is_grasping(self):
    """
    Checks whether the current state of this gripper being controlled is in a grasping state.
    Should be implemented by subclass.

    Returns:
        IsGraspingState: Grasping state of gripper
    """
    raise NotImplementedError()

LocomotionController

Bases: BaseController

Controller to control locomotion. All implemented controllers that encompass locomotion capabilities should extend from this class.

Source code in omnigibson/controllers/controller_base.py 
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
class LocomotionController(BaseController):
    """
    Controller to control locomotion. All implemented controllers that encompass locomotion capabilities should extend
    from this class.
    """

    def __init_subclass__(cls, **kwargs):
        # Register as part of locomotion controllers
        super().__init_subclass__(**kwargs)
        register_locomotion_controller(cls)

    @classproperty
    def _do_not_register_classes(cls):
        # Don't register this class since it's an abstract template
        classes = super()._do_not_register_classes
        classes.add("LocomotionController")
        return classes

ManipulationController

Bases: BaseController

Controller to control manipulation. All implemented controllers that encompass manipulation capabilities should extend from this class.

Source code in omnigibson/controllers/controller_base.py 
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
class ManipulationController(BaseController):
    """
    Controller to control manipulation. All implemented controllers that encompass manipulation capabilities
    should extend from this class.
    """

    def __init_subclass__(cls, **kwargs):
        # Register as part of manipulation controllers
        super().__init_subclass__(**kwargs)
        register_manipulation_controller(cls)

    @classproperty
    def _do_not_register_classes(cls):
        # Don't register this class since it's an abstract template
        classes = super()._do_not_register_classes
        classes.add("ManipulationController")
        return classes