Skip to content

controllers

controllers

Modules:

Name Description
abstract
base_pose
g1_wbc

G1's whole-body PD/ONNX low-level controller: the reference stack's
joint_pos
joint_rel_pos
joint_vel
torso_height

Classes:

Name Description
AbstractPositionController

Base class for (absolute or relative) position controllers.
BasePoseController

Abstract pose controller class for a simple 2D pose controller for a robot base.
Controller

Implementation of a simple, non-blocking controller for the MolmoSpaces environments.
DiffDriveBasePoseController
JointPosController

Generic joint position controller for a robot.
JointRelPosController

Generic joint position controller for a robot.
JointVelController

Generic joint velocity controller for a robot.
SwerveBasePoseController
TorsoHeightJointPosController

Controls the RBY1 torso via a single height scalar.

__all__ module-attribute 

__all__ = ['AbstractPositionController', 'Controller', 'BasePoseController', 'DiffDriveBasePoseController', 'JointPosController', 'JointRelPosController', 'JointVelController', 'SwerveBasePoseController', 'TorsoHeightJointPosController']

AbstractPositionController 

AbstractPositionController(robot_move_group: MoveGroup)

Bases: Controller

Base class for (absolute or relative) position controllers.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/abstract.py 
18
19
20
21
22
23
def __init__(self, robot_move_group: MoveGroup) -> None:
    """
    Args:
        robot_move_group: The move group of the robot to be controlled
    """
    self.robot_move_group = robot_move_group

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary abstractmethod property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target abstractmethod property 

target: ndarray

The target state of the controller, set by the user of the controller.

target_pos abstractmethod property 

target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs abstractmethod 

compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, eg. positions, torques etc.
Source code in molmo_spaces/controllers/abstract.py 
54
55
56
57
58
59
60
61
@abc.abstractmethod
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, eg. positions, torques etc.
    """

reset abstractmethod 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/abstract.py 
63
64
65
@abc.abstractmethod
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets."""

set_target abstractmethod 

set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target: np.ndarray) -> None:
    """
    Set the target state of the controller.

    Args:
        target: The target state to be set, e.g. a joint position, base velocity, or any other state.
    """

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/abstract.py 
46
47
48
49
50
51
52
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    NOTE: If the controller is already stationary, it does not change the target.
    """

BasePoseController 

BasePoseController(robot_move_group: RobotBaseGroup)

Bases: Controller, ABC

Abstract pose controller class for a simple 2D pose controller for a robot base. Computes velocities for the robot base's actuators based on target pose, and clips to control limits. NOTE: Assumes base actuators (eg. wheels) use velocity inputs.

This class cannot be instantiated directly and must be subclassed with an implementation of the "compute_base_velocities" method.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_base_velocities

Abstract method to compute base velocities to achieve the target pose.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target pose for the controller
Source code in molmo_spaces/controllers/base_pose.py 
20
21
22
23
24
25
26
27
28
29
30
31
32
33
def __init__(self, robot_move_group: RobotBaseGroup) -> None:
    super().__init__(robot_move_group)

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = self.robot_move_group.pose.copy()

    self._validate_actuators()

    self.reset()

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 

target

The target pose for the controller

compute_base_velocities abstractmethod 

compute_base_velocities(target_base_pose: ndarray) -> ndarray

Abstract method to compute base velocities to achieve the target pose. Must be implemented by subclasses. Methods could include: Differential-drive, Ackermann steering, Omni-wheel control etc.

Source code in molmo_spaces/controllers/base_pose.py 
73
74
75
76
77
78
79
80
@abstractmethod
def compute_base_velocities(self, target_base_pose: np.ndarray) -> np.ndarray:
    """
    Abstract method to compute base velocities to achieve the target pose.
    Must be implemented by subclasses.
    Methods could include: Differential-drive, Ackermann steering, Omni-wheel control etc.
    """
    pass

compute_ctrl_inputs 

compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()

set_target 

set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

Controller 

Controller(robot_move_group: MoveGroup)

Implementation of a simple, non-blocking controller for the MolmoSpaces environments. This controller is meant to be simple, provide a control law based on stateful targets set by the user, and return the control inputs needed to achieve those targets.

The controller DOES NOT step the simulation, it only computes the control inputs based on the current state and the targets set by the user.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/abstract.py 
18
19
20
21
22
23
def __init__(self, robot_move_group: MoveGroup) -> None:
    """
    Args:
        robot_move_group: The move group of the robot to be controlled
    """
    self.robot_move_group = robot_move_group

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary abstractmethod property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target abstractmethod property 

target: ndarray

The target state of the controller, set by the user of the controller.

compute_ctrl_inputs abstractmethod 

compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, eg. positions, torques etc.
Source code in molmo_spaces/controllers/abstract.py 
54
55
56
57
58
59
60
61
@abc.abstractmethod
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, eg. positions, torques etc.
    """

reset abstractmethod 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/abstract.py 
63
64
65
@abc.abstractmethod
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets."""

set_target abstractmethod 

set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target: np.ndarray) -> None:
    """
    Set the target state of the controller.

    Args:
        target: The target state to be set, e.g. a joint position, base velocity, or any other state.
    """

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/abstract.py 
46
47
48
49
50
51
52
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    NOTE: If the controller is already stationary, it does not change the target.
    """

DiffDriveBasePoseController 

DiffDriveBasePoseController(robot_config, robot_move_group: RobotBaseGroup)

Bases: BasePoseController

Differential drive base pose controller.

Parameters:

Name Type Description Default
robot_config

The configuration of the robot, containing special information about the control

 required
robot_move_group RobotBaseGroup

The move group of the robot base to be controlled

 required

Methods:

Name Description
compute_base_velocities

Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_config
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target pose for the controller
wheel_base
wheel_radius
Source code in molmo_spaces/controllers/base_pose.py 
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
def __init__(self, robot_config, robot_move_group: RobotBaseGroup) -> None:
    """
    Differential drive base pose controller.

    Args:
        robot_config: The configuration of the robot, containing special information about the control
        robot_move_group: The move group of the robot base to be controlled
    """
    self.robot_config = robot_config
    self.wheel_base = robot_config.wheel_base  # Distance between the wheels
    self.wheel_radius = robot_config.wheel_radius  # Radius of the wheels

    super().__init__(robot_move_group)

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

robot_config instance-attribute 

robot_config = robot_config

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 

target

The target pose for the controller

wheel_base instance-attribute 

wheel_base = robot_config.wheel_base

wheel_radius instance-attribute 

wheel_radius = robot_config.wheel_radius

compute_base_velocities 

compute_base_velocities(target_base_pose)

Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.

Parameters:

Name Type Description Default
target_base_pose

Target [x, y, theta] pose in world frame.

 required

Returns:

Type Description

np.ndarray: [left_wheel_velocity, right_wheel_velocity]
Source code in molmo_spaces/controllers/base_pose.py 
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
def compute_base_velocities(self, target_base_pose):
    """
    Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.

    Args:
        target_base_pose: Target [x, y, theta] pose in world frame.

    Returns:
        np.ndarray: [left_wheel_velocity, right_wheel_velocity]
    """
    # TODO(anyone): Test this controller
    # TODO(anyone): Handle case where base_pose frame is not in the center of the wheels

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    current_pose = self.robot_move_group.pose  # [x, y, theta]
    x, y, theta = current_pose
    x_t, y_t, theta_t = target_base_pose

    # Compute pose error in world frame
    dx = x_t - x
    dy = y_t - y

    # Transform error to robot frame
    error_x = np.cos(theta) * dx + np.sin(theta) * dy

    # TODO(wilbert): this part was unused, should it have been error_y = ... ?. Will keep it
    # commented here bc as mentioned in the past comments above this needs to be more tested
    # - np.sin(theta) * dx + np.cos(theta) * dy

    error_theta = np.arctan2(np.sin(theta_t - theta), np.cos(theta_t - theta))

    # Proportional gains
    k_rho = 1.0
    k_alpha = 2.0

    # Compute control signals
    v = k_rho * error_x
    omega = k_alpha * error_theta

    # Convert (v, omega) to wheel velocities
    v_l = (v - (self.wheel_base / 2.0) * omega) / self.wheel_radius
    v_r = (v + (self.wheel_base / 2.0) * omega) / self.wheel_radius

    return np.array([v_l, v_r])

compute_ctrl_inputs 

compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()

set_target 

set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

JointPosController 

JointPosController(robot_move_group: MoveGroup)

Bases: AbstractPositionController

Generic joint position controller for a robot. Computes joint position targets for the joint's actuators, and clips to control limits. NOTE: Assumes joint actuators use position inputs.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/joint_pos.py 
14
15
16
17
18
19
20
21
22
23
24
25
def __init__(self, robot_move_group: MoveGroup) -> None:
    super().__init__(robot_move_group)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = self.robot_move_group.joint_pos

    self._validate_actuators()

    self.reset()

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property writable 

target

The target state of the controller, set by the user of the controller.

target_pos property 

target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 

compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_pos.py 
57
58
59
60
61
62
63
64
65
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    # position control inputs: just pass through the target joint positions
    return self.target.copy()

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets

Source code in molmo_spaces/controllers/joint_pos.py 
67
68
69
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets"""
    self.set_to_stationary()  # Explicit reset to stationary mode

set_target 

set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/joint_pos.py 
43
44
45
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self.target = np.clip(target, self.ctrl_range[:, 0], self.ctrl_range[:, 1])

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_pos.py 
47
48
49
50
51
52
53
54
55
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to current joint positions to hold them stationary
    self._stationary = True
    self.target = self.robot_move_group.noop_ctrl

JointRelPosController 

JointRelPosController(robot_move_group: MoveGroup)

Bases: AbstractPositionController

Generic joint position controller for a robot. Computes delta joint position targets for the joint's actuators, and clips to control limits.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets
set_target

Set the target joint positions for the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
desired_joint_positions
robot_move_group
stationary

Returns whether the controller is in stationary mode
target

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/joint_rel_pos.py 
13
14
15
16
17
18
19
20
21
22
23
24
def __init__(self, robot_move_group: MoveGroup) -> None:
    super().__init__(robot_move_group)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self.target = np.zeros_like(
        self.robot_move_group.noop_ctrl
    )  # Initial pos target is 0 relative to current position
    self.desired_joint_positions = self.robot_move_group.noop_ctrl.copy()
    self.reset()

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

desired_joint_positions instance-attribute 

desired_joint_positions = self.robot_move_group.noop_ctrl.copy()

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary

Returns whether the controller is in stationary mode

target property writable 

target

The target state of the controller, set by the user of the controller.

target_pos property 

target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 

compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_rel_pos.py 
71
72
73
74
75
76
77
78
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    return self.desired_joint_positions.copy()

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets

Source code in molmo_spaces/controllers/joint_rel_pos.py 
80
81
82
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets"""
    self.set_to_stationary()  # Explicit reset to stationary mode

set_target 

set_target(target: ndarray) -> None

Set the target joint positions for the controller. NOTE: For this controller we assume that the controller will be set to stationary every time before the relative target has to be computed

Source code in molmo_spaces/controllers/joint_rel_pos.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
def set_target(self, target: np.ndarray) -> None:
    """Set the target joint positions for the controller.
    NOTE: For this controller we assume that the controller will be set to
    stationary every time before the relative target has to be computed"""

    # Case to avoid drift: if target is close to zero, stay stationary
    if np.linalg.norm(target) < 1e-6:
        if not self._stationary:  # important to avoid re-setting to noop ctrl and drifting
            self.set_to_stationary()
    else:
        self._stationary = False
        self.target = target
        self.desired_joint_positions = self.robot_move_group.noop_ctrl + target
        self.desired_joint_positions = np.clip(
            self.desired_joint_positions, self.ctrl_range[:, 0], self.ctrl_range[:, 1]
        )

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_rel_pos.py 
60
61
62
63
64
65
66
67
68
69
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to current joint positions to hold them stationary
    self._stationary = True
    self.target = np.zeros_like(self.robot_move_group.noop_ctrl)
    self.desired_joint_positions = self.robot_move_group.noop_ctrl.copy()

JointVelController 

JointVelController(robot_config, robot_move_group: MoveGroup)

Bases: Controller

Generic joint velocity controller for a robot. Computes joint position targets for the joint's actuators after euler integration, and clips to control limits. NOTE: Assumes joint actuators use position inputs.

robot_config: The configuration of the robot, containing special information about the control, e.g., delta_t for integration (if applicable) etc. robot_move_group: The move group of the robot to be controlled

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
euler_dt
robot_config
robot_move_group
stationary bool

Returns whether the controller is in stationary mode
target ndarray

Returns the current target joint velocities
Source code in molmo_spaces/controllers/joint_vel.py 
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
def __init__(self, robot_config, robot_move_group: MoveGroup) -> None:
    """
    Args:
    robot_config: The configuration of the robot, containing special information about the control,
                  e.g., delta_t for integration (if applicable) etc.
    robot_move_group: The move group of the robot to be controlled
    """
    super().__init__(robot_move_group)

    self.robot_config = robot_config
    self.euler_dt = self.robot_config.delta_t  # Time step for euler integration

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = np.zeros(self.ctrl_dim)  # Initially zero vel targets

    self._validate_actuators()

    self.reset()

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

euler_dt instance-attribute 

euler_dt = self.robot_config.delta_t

robot_config instance-attribute 

robot_config = robot_config

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Returns whether the controller is in stationary mode

target property 

target: ndarray

Returns the current target joint velocities

compute_ctrl_inputs 

compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_vel.py 
60
61
62
63
64
65
66
67
68
69
70
71
72
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    # position control inputs: perform euler integration on the target velocities
    ctrl_inputs = self.robot_move_group.joint_pos + self._target * self.euler_dt
    # Clip the control inputs to the actuator limits
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])

    return ctrl_inputs

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/joint_vel.py 
74
75
def reset(self) -> None:
    self.set_to_stationary()

set_target 

set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/joint_vel.py 
46
47
48
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self._target = target.copy()

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_vel.py 
50
51
52
53
54
55
56
57
58
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to zero joint velocities to hold joints stationary
    self._stationary = True
    self._target = np.zeros(self.ctrl_dim)

SwerveBasePoseController 

SwerveBasePoseController(robot_config, robot_move_group: RobotBaseGroup)

Bases: BasePoseController

Swerve drive base pose controller.

Parameters:

Name Type Description Default
robot_config

The configuration of the robot, containing special information about the control

 required
robot_move_group RobotBaseGroup

The move group of the robot base to be controlled

 required

Methods:

Name Description
compute_base_velocities

Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
current_steer_angles
max_wheel_speed
robot_config
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
steer_angle_range
steer_track
target

The target pose for the controller
wheel_base
wheel_radius
Source code in molmo_spaces/controllers/base_pose.py 
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
def __init__(self, robot_config, robot_move_group: RobotBaseGroup) -> None:
    """
    Swerve drive base pose controller.

    Args:
        robot_config: The configuration of the robot, containing special information about the control
        robot_move_group: The move group of the robot base to be controlled
    """
    self.robot_config = robot_config
    self.steer_track = robot_config.steer_track
    self.wheel_base = robot_config.wheel_base  # Distance between the wheels
    self.wheel_radius = robot_config.wheel_radius  # Radius of the wheels
    self.steer_angle_range = robot_config.steer_angle_range
    self.max_wheel_speed = robot_config.max_wheel_speed
    self.current_steer_angles = np.zeros(4)

    super().__init__(robot_move_group)

ctrl_dim instance-attribute 

ctrl_dim = robot_move_group.n_actuators

ctrl_range instance-attribute 

ctrl_range = robot_move_group.ctrl_limits

current_steer_angles instance-attribute 

current_steer_angles = np.zeros(4)

max_wheel_speed instance-attribute 

max_wheel_speed = robot_config.max_wheel_speed

robot_config instance-attribute 

robot_config = robot_config

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

steer_angle_range instance-attribute 

steer_angle_range = robot_config.steer_angle_range

steer_track instance-attribute 

steer_track = robot_config.steer_track

target property 

target

The target pose for the controller

wheel_base instance-attribute 

wheel_base = robot_config.wheel_base

wheel_radius instance-attribute 

wheel_radius = robot_config.wheel_radius

compute_base_velocities 

compute_base_velocities(target_base_pose)

Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.

Parameters:

Name Type Description Default
target_base_pose

Target [x, y, theta] pose in world frame.

 required

Returns:

Type Description

np.ndarray: [front_left_steer_angle, front_right_steer_angle, back_left_steer_angle, back_right_steer_angle, front_left_wheel_velocity, front_right_wheel_velocity, back_left_wheel_velocity, back_right_wheel_velocity]

Assert local frame defination:

               ↑  x
               |
       fl      |       fr
        o------|------o
        |      |      |
        |      |      |
        |      |      |
        |      |      |
  y <----------o      |
        |             |
        |             |
        |             |
        |             |
        o-------------o
       rl              rr
Source code in molmo_spaces/controllers/base_pose.py 
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
def compute_base_velocities(self, target_base_pose):
    """
    Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.

    Args:
        target_base_pose: Target [x, y, theta] pose in world frame.

    Returns:
        np.ndarray: [front_left_steer_angle,   front_right_steer_angle,    back_left_steer_angle,    back_right_steer_angle,
                    front_left_wheel_velocity, front_right_wheel_velocity, back_left_wheel_velocity, back_right_wheel_velocity]

    Assert local frame defination:

                       ↑  x
                       |
               fl      |       fr
                o------|------o
                |      |      |
                |      |      |
                |      |      |
                |      |      |
          y <----------o      |
                |             |
                |             |
                |             |
                |             |
                o-------------o
               rl              rr
    """
    # TODO(anyone): Test this controller
    # TODO(anyone): Handle case where base_pose frame is not in the center of the wheels

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    current_pose = self.robot_move_group.pose  # [x, y, theta]
    x, y, theta = current_pose
    x_t, y_t, theta_t = target_base_pose

    # Compute pose error in world frame
    dx = x_t - x
    dy = y_t - y

    # Transform error to robot frame
    error_x = np.cos(theta) * dx + np.sin(theta) * dy
    error_y = -np.sin(theta) * dx + np.cos(theta) * dy
    error_theta = np.arctan2(np.sin(theta_t - theta), np.cos(theta_t - theta))

    # Proportional gains
    k_rho = 1.0
    k_alpha = 2.0

    # Compute control signals
    v_lin_x = k_rho * error_x
    v_lin_y = k_rho * error_y
    v_yaw = k_alpha * error_theta

    if np.abs(v_lin_y) < 0.01:
        v_lin_y = 0.0
    if np.abs(v_lin_x) < 0.01:
        v_lin_x = 0.0
    if np.abs(v_yaw) < 0.01:
        v_yaw = 0.0
    if np.abs(v_lin_y) < 0.01 and np.abs(v_lin_x) < 0.01 and np.abs(v_yaw) < 0.01:
        return np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

    if np.linalg.norm([v_lin_x, v_lin_y]) < 0.03:
        v_lin_x, v_lin_y = 0.0, 0.0
        v_yaw = np.sign(v_yaw) * np.pi * 2

    # Kinematic model: convert vx, vy, vw to steer angles and wheel velocities
    wheel_coords = [
        (-self.wheel_base / 2, self.steer_track / 2),  # fl
        (self.wheel_base / 2, self.steer_track / 2),  # fr
        (-self.wheel_base / 2, -self.steer_track / 2),  # rl
        (self.wheel_base / 2, -self.steer_track / 2),  # rr
    ]

    steer_ang = []
    drive_vel = []
    for wx, wy in wheel_coords:
        vix = v_lin_x + wx * v_yaw
        viy = v_lin_y + wy * v_yaw
        angle = np.arctan2(viy, vix)
        speed = np.hypot(viy, vix) / self.wheel_radius
        steer_ang.append(angle)
        drive_vel.append(speed)
    steer_ang = np.array(steer_ang)
    drive_vel = np.array(drive_vel)

    # Optimize steer angle and wheel speed to find nearest equivalent angle and speed
    optimized_steer_ang, optimized_drive_vel = optimize_all_steer_and_drive(
        self.current_steer_angles,
        steer_ang,
        drive_vel,
        self.steer_angle_range,
        self.max_wheel_speed,
    )
    self.current_steer_angles = optimized_steer_ang.copy()

    # Update the joint velocity and position.
    return np.concatenate([optimized_steer_ang, optimized_drive_vel])

compute_ctrl_inputs 

compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()

set_target 

set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

TorsoHeightJointPosController 

TorsoHeightJointPosController(robot_move_group: MoveGroup, max_height: float = 0.738)

Bases: AbstractPositionController

Controls the RBY1 torso via a single height scalar.

Accepts a 1-element command [h]; compute_ctrl_inputs() returns the full 6-DOF joint position array produced by the constraint torso_1 = torso_3 = h, torso_2 = -2*h.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Return the 6-DOF torso joint positions for the current target height.
height_to_joints

Return the 6-DOF torso joint positions for a given height scalar.
hold_at_height

Set the controller to hold at a specific height without reading current joints.
joints_to_height

Recover the height scalar from a 6-DOF torso joint position array.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
max_height
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/torso_height.py 
33
34
35
36
37
def __init__(self, robot_move_group: MoveGroup, max_height: float = 0.738) -> None:
    super().__init__(robot_move_group)
    self.max_height = max_height
    self._stationary = True
    self._target_height: float = self.joints_to_height(robot_move_group.joint_pos)

max_height instance-attribute 

max_height = max_height

robot_move_group instance-attribute 

robot_move_group = robot_move_group

stationary property 

stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 

target: ndarray

The target state of the controller, set by the user of the controller.

target_pos property 

target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 

compute_ctrl_inputs() -> ndarray

Return the 6-DOF torso joint positions for the current target height.

Source code in molmo_spaces/controllers/torso_height.py 
69
70
71
72
def compute_ctrl_inputs(self) -> np.ndarray:
    """Return the 6-DOF torso joint positions for the current target height."""
    joints = self.height_to_joints(self._target_height)
    return joints

height_to_joints staticmethod 

height_to_joints(height: float) -> ndarray

Return the 6-DOF torso joint positions for a given height scalar.

torso[1] = height, torso[2] = -2*height, torso[3] = height. Indices 0, 4, 5 are left at zero.

Source code in molmo_spaces/controllers/torso_height.py 
15
16
17
18
19
20
21
22
23
24
25
26
@staticmethod
def height_to_joints(height: float) -> np.ndarray:
    """Return the 6-DOF torso joint positions for a given height scalar.

    torso[1] = height, torso[2] = -2*height, torso[3] = height.
    Indices 0, 4, 5 are left at zero.
    """
    joints = np.zeros(6)
    joints[1] = height
    joints[2] = -2.0 * height
    joints[3] = height
    return joints

hold_at_height 

hold_at_height(height: float) -> None

Set the controller to hold at a specific height without reading current joints.

Use this to lock the torso at a desired target after a height adjustment is complete, so that subsequent steps without an explicit torso command will maintain this height rather than drifting to the current joint position.

Source code in molmo_spaces/controllers/torso_height.py 
59
60
61
62
63
64
65
66
67
def hold_at_height(self, height: float) -> None:
    """Set the controller to hold at a specific height without reading current joints.

    Use this to lock the torso at a desired target after a height adjustment is
    complete, so that subsequent steps without an explicit torso command will
    maintain this height rather than drifting to the current joint position.
    """
    self._stationary = True
    self._target_height = float(np.clip(height, 0.0, self.max_height))

joints_to_height staticmethod 

joints_to_height(joints: ndarray) -> float

Recover the height scalar from a 6-DOF torso joint position array.

Source code in molmo_spaces/controllers/torso_height.py 
28
29
30
31
@staticmethod
def joints_to_height(joints: np.ndarray) -> float:
    """Recover the height scalar from a 6-DOF torso joint position array."""
    return float(joints[1])

reset 

reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/torso_height.py 
74
75
def reset(self) -> None:
    self.set_to_stationary()

set_target 

set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/torso_height.py 
51
52
53
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self._target_height = float(np.clip(target[0], 0.0, self.max_height))

set_to_stationary 

set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/torso_height.py 
55
56
57
def set_to_stationary(self) -> None:
    self._stationary = True
    self._target_height = self.joints_to_height(self.robot_move_group.joint_pos)

abstract

Classes:

Name Description
AbstractPositionController

Base class for (absolute or relative) position controllers.
Controller

Implementation of a simple, non-blocking controller for the MolmoSpaces environments.

AbstractPositionController 

AbstractPositionController(robot_move_group: MoveGroup)

Bases: Controller

Base class for (absolute or relative) position controllers.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/abstract.py 
18
19
20
21
22
23
def __init__(self, robot_move_group: MoveGroup) -> None:
    """
    Args:
        robot_move_group: The move group of the robot to be controlled
    """
    self.robot_move_group = robot_move_group
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary abstractmethod property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target abstractmethod property 
target: ndarray

The target state of the controller, set by the user of the controller.

target_pos abstractmethod property 
target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs abstractmethod 
compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, eg. positions, torques etc.
Source code in molmo_spaces/controllers/abstract.py 
54
55
56
57
58
59
60
61
@abc.abstractmethod
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, eg. positions, torques etc.
    """
reset abstractmethod 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/abstract.py 
63
64
65
@abc.abstractmethod
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets."""
set_target abstractmethod 
set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target: np.ndarray) -> None:
    """
    Set the target state of the controller.

    Args:
        target: The target state to be set, e.g. a joint position, base velocity, or any other state.
    """
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/abstract.py 
46
47
48
49
50
51
52
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    NOTE: If the controller is already stationary, it does not change the target.
    """

Controller 

Controller(robot_move_group: MoveGroup)

Implementation of a simple, non-blocking controller for the MolmoSpaces environments. This controller is meant to be simple, provide a control law based on stateful targets set by the user, and return the control inputs needed to achieve those targets.

The controller DOES NOT step the simulation, it only computes the control inputs based on the current state and the targets set by the user.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/abstract.py 
18
19
20
21
22
23
def __init__(self, robot_move_group: MoveGroup) -> None:
    """
    Args:
        robot_move_group: The move group of the robot to be controlled
    """
    self.robot_move_group = robot_move_group
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary abstractmethod property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target abstractmethod property 
target: ndarray

The target state of the controller, set by the user of the controller.

compute_ctrl_inputs abstractmethod 
compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, eg. positions, torques etc.
Source code in molmo_spaces/controllers/abstract.py 
54
55
56
57
58
59
60
61
@abc.abstractmethod
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, eg. positions, torques etc.
    """
reset abstractmethod 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/abstract.py 
63
64
65
@abc.abstractmethod
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets."""
set_target abstractmethod 
set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target: np.ndarray) -> None:
    """
    Set the target state of the controller.

    Args:
        target: The target state to be set, e.g. a joint position, base velocity, or any other state.
    """
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/abstract.py 
46
47
48
49
50
51
52
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    NOTE: If the controller is already stationary, it does not change the target.
    """

base_pose

Classes:

Name Description
BasePoseController

Abstract pose controller class for a simple 2D pose controller for a robot base.
DiffDriveBasePoseController
SwerveBasePoseController

BasePoseController 

BasePoseController(robot_move_group: RobotBaseGroup)

Bases: Controller, ABC

Abstract pose controller class for a simple 2D pose controller for a robot base. Computes velocities for the robot base's actuators based on target pose, and clips to control limits. NOTE: Assumes base actuators (eg. wheels) use velocity inputs.

This class cannot be instantiated directly and must be subclassed with an implementation of the "compute_base_velocities" method.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_base_velocities

Abstract method to compute base velocities to achieve the target pose.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target pose for the controller
Source code in molmo_spaces/controllers/base_pose.py 
20
21
22
23
24
25
26
27
28
29
30
31
32
33
def __init__(self, robot_move_group: RobotBaseGroup) -> None:
    super().__init__(robot_move_group)

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = self.robot_move_group.pose.copy()

    self._validate_actuators()

    self.reset()
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 
target

The target pose for the controller

compute_base_velocities abstractmethod 
compute_base_velocities(target_base_pose: ndarray) -> ndarray

Abstract method to compute base velocities to achieve the target pose. Must be implemented by subclasses. Methods could include: Differential-drive, Ackermann steering, Omni-wheel control etc.

Source code in molmo_spaces/controllers/base_pose.py 
73
74
75
76
77
78
79
80
@abstractmethod
def compute_base_velocities(self, target_base_pose: np.ndarray) -> np.ndarray:
    """
    Abstract method to compute base velocities to achieve the target pose.
    Must be implemented by subclasses.
    Methods could include: Differential-drive, Ackermann steering, Omni-wheel control etc.
    """
    pass
compute_ctrl_inputs 
compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()
set_target 
set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

DiffDriveBasePoseController 

DiffDriveBasePoseController(robot_config, robot_move_group: RobotBaseGroup)

Bases: BasePoseController

Differential drive base pose controller.

Parameters:

Name Type Description Default
robot_config

The configuration of the robot, containing special information about the control

 required
robot_move_group RobotBaseGroup

The move group of the robot base to be controlled

 required

Methods:

Name Description
compute_base_velocities

Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_config
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target pose for the controller
wheel_base
wheel_radius
Source code in molmo_spaces/controllers/base_pose.py 
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
def __init__(self, robot_config, robot_move_group: RobotBaseGroup) -> None:
    """
    Differential drive base pose controller.

    Args:
        robot_config: The configuration of the robot, containing special information about the control
        robot_move_group: The move group of the robot base to be controlled
    """
    self.robot_config = robot_config
    self.wheel_base = robot_config.wheel_base  # Distance between the wheels
    self.wheel_radius = robot_config.wheel_radius  # Radius of the wheels

    super().__init__(robot_move_group)
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
robot_config instance-attribute 
robot_config = robot_config
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 
target

The target pose for the controller

wheel_base instance-attribute 
wheel_base = robot_config.wheel_base
wheel_radius instance-attribute 
wheel_radius = robot_config.wheel_radius
compute_base_velocities 
compute_base_velocities(target_base_pose)

Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.

Parameters:

Name Type Description Default
target_base_pose

Target [x, y, theta] pose in world frame.

 required

Returns:

Type Description

np.ndarray: [left_wheel_velocity, right_wheel_velocity]
Source code in molmo_spaces/controllers/base_pose.py 
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
def compute_base_velocities(self, target_base_pose):
    """
    Compute wheel velocities to drive the robot base to the target pose using a simple proportional controller.

    Args:
        target_base_pose: Target [x, y, theta] pose in world frame.

    Returns:
        np.ndarray: [left_wheel_velocity, right_wheel_velocity]
    """
    # TODO(anyone): Test this controller
    # TODO(anyone): Handle case where base_pose frame is not in the center of the wheels

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    current_pose = self.robot_move_group.pose  # [x, y, theta]
    x, y, theta = current_pose
    x_t, y_t, theta_t = target_base_pose

    # Compute pose error in world frame
    dx = x_t - x
    dy = y_t - y

    # Transform error to robot frame
    error_x = np.cos(theta) * dx + np.sin(theta) * dy

    # TODO(wilbert): this part was unused, should it have been error_y = ... ?. Will keep it
    # commented here bc as mentioned in the past comments above this needs to be more tested
    # - np.sin(theta) * dx + np.cos(theta) * dy

    error_theta = np.arctan2(np.sin(theta_t - theta), np.cos(theta_t - theta))

    # Proportional gains
    k_rho = 1.0
    k_alpha = 2.0

    # Compute control signals
    v = k_rho * error_x
    omega = k_alpha * error_theta

    # Convert (v, omega) to wheel velocities
    v_l = (v - (self.wheel_base / 2.0) * omega) / self.wheel_radius
    v_r = (v + (self.wheel_base / 2.0) * omega) / self.wheel_radius

    return np.array([v_l, v_r])
compute_ctrl_inputs 
compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()
set_target 
set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

SwerveBasePoseController 

SwerveBasePoseController(robot_config, robot_move_group: RobotBaseGroup)

Bases: BasePoseController

Swerve drive base pose controller.

Parameters:

Name Type Description Default
robot_config

The configuration of the robot, containing special information about the control

 required
robot_move_group RobotBaseGroup

The move group of the robot base to be controlled

 required

Methods:

Name Description
compute_base_velocities

Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target pose for the controller
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
current_steer_angles
max_wheel_speed
robot_config
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
steer_angle_range
steer_track
target

The target pose for the controller
wheel_base
wheel_radius
Source code in molmo_spaces/controllers/base_pose.py 
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
def __init__(self, robot_config, robot_move_group: RobotBaseGroup) -> None:
    """
    Swerve drive base pose controller.

    Args:
        robot_config: The configuration of the robot, containing special information about the control
        robot_move_group: The move group of the robot base to be controlled
    """
    self.robot_config = robot_config
    self.steer_track = robot_config.steer_track
    self.wheel_base = robot_config.wheel_base  # Distance between the wheels
    self.wheel_radius = robot_config.wheel_radius  # Radius of the wheels
    self.steer_angle_range = robot_config.steer_angle_range
    self.max_wheel_speed = robot_config.max_wheel_speed
    self.current_steer_angles = np.zeros(4)

    super().__init__(robot_move_group)
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
current_steer_angles instance-attribute 
current_steer_angles = np.zeros(4)
max_wheel_speed instance-attribute 
max_wheel_speed = robot_config.max_wheel_speed
robot_config instance-attribute 
robot_config = robot_config
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

steer_angle_range instance-attribute 
steer_angle_range = robot_config.steer_angle_range
steer_track instance-attribute 
steer_track = robot_config.steer_track
target property 
target

The target pose for the controller

wheel_base instance-attribute 
wheel_base = robot_config.wheel_base
wheel_radius instance-attribute 
wheel_radius = robot_config.wheel_radius
compute_base_velocities 
compute_base_velocities(target_base_pose)

Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.

Parameters:

Name Type Description Default
target_base_pose

Target [x, y, theta] pose in world frame.

 required

Returns:

Type Description

np.ndarray: [front_left_steer_angle, front_right_steer_angle, back_left_steer_angle, back_right_steer_angle, front_left_wheel_velocity, front_right_wheel_velocity, back_left_wheel_velocity, back_right_wheel_velocity]

Assert local frame defination:

               ↑  x
               |
       fl      |       fr
        o------|------o
        |      |      |
        |      |      |
        |      |      |
        |      |      |
  y <----------o      |
        |             |
        |             |
        |             |
        |             |
        o-------------o
       rl              rr
Source code in molmo_spaces/controllers/base_pose.py 
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
def compute_base_velocities(self, target_base_pose):
    """
    Compute wheel velocities and steer angles to drive the robot base to the target pose using a simple proportional controller.

    Args:
        target_base_pose: Target [x, y, theta] pose in world frame.

    Returns:
        np.ndarray: [front_left_steer_angle,   front_right_steer_angle,    back_left_steer_angle,    back_right_steer_angle,
                    front_left_wheel_velocity, front_right_wheel_velocity, back_left_wheel_velocity, back_right_wheel_velocity]

    Assert local frame defination:

                       ↑  x
                       |
               fl      |       fr
                o------|------o
                |      |      |
                |      |      |
                |      |      |
                |      |      |
          y <----------o      |
                |             |
                |             |
                |             |
                |             |
                o-------------o
               rl              rr
    """
    # TODO(anyone): Test this controller
    # TODO(anyone): Handle case where base_pose frame is not in the center of the wheels

    assert isinstance(self.robot_move_group, RobotBaseGroup)

    current_pose = self.robot_move_group.pose  # [x, y, theta]
    x, y, theta = current_pose
    x_t, y_t, theta_t = target_base_pose

    # Compute pose error in world frame
    dx = x_t - x
    dy = y_t - y

    # Transform error to robot frame
    error_x = np.cos(theta) * dx + np.sin(theta) * dy
    error_y = -np.sin(theta) * dx + np.cos(theta) * dy
    error_theta = np.arctan2(np.sin(theta_t - theta), np.cos(theta_t - theta))

    # Proportional gains
    k_rho = 1.0
    k_alpha = 2.0

    # Compute control signals
    v_lin_x = k_rho * error_x
    v_lin_y = k_rho * error_y
    v_yaw = k_alpha * error_theta

    if np.abs(v_lin_y) < 0.01:
        v_lin_y = 0.0
    if np.abs(v_lin_x) < 0.01:
        v_lin_x = 0.0
    if np.abs(v_yaw) < 0.01:
        v_yaw = 0.0
    if np.abs(v_lin_y) < 0.01 and np.abs(v_lin_x) < 0.01 and np.abs(v_yaw) < 0.01:
        return np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

    if np.linalg.norm([v_lin_x, v_lin_y]) < 0.03:
        v_lin_x, v_lin_y = 0.0, 0.0
        v_yaw = np.sign(v_yaw) * np.pi * 2

    # Kinematic model: convert vx, vy, vw to steer angles and wheel velocities
    wheel_coords = [
        (-self.wheel_base / 2, self.steer_track / 2),  # fl
        (self.wheel_base / 2, self.steer_track / 2),  # fr
        (-self.wheel_base / 2, -self.steer_track / 2),  # rl
        (self.wheel_base / 2, -self.steer_track / 2),  # rr
    ]

    steer_ang = []
    drive_vel = []
    for wx, wy in wheel_coords:
        vix = v_lin_x + wx * v_yaw
        viy = v_lin_y + wy * v_yaw
        angle = np.arctan2(viy, vix)
        speed = np.hypot(viy, vix) / self.wheel_radius
        steer_ang.append(angle)
        drive_vel.append(speed)
    steer_ang = np.array(steer_ang)
    drive_vel = np.array(drive_vel)

    # Optimize steer angle and wheel speed to find nearest equivalent angle and speed
    optimized_steer_ang, optimized_drive_vel = optimize_all_steer_and_drive(
        self.current_steer_angles,
        steer_ang,
        drive_vel,
        self.steer_angle_range,
        self.max_wheel_speed,
    )
    self.current_steer_angles = optimized_steer_ang.copy()

    # Update the joint velocity and position.
    return np.concatenate([optimized_steer_ang, optimized_drive_vel])
compute_ctrl_inputs 
compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/base_pose.py 
61
62
63
64
65
66
67
68
69
70
71
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """

    ctrl_inputs = self.compute_base_velocities(self._target)
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
    return ctrl_inputs
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/base_pose.py 
82
83
def reset(self) -> None:
    self.set_to_stationary()
set_target 
set_target(target: ndarray) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
44
45
46
47
def set_target(self, target: np.ndarray) -> None:
    """Set the target pose for the controller"""
    self._stationary = False
    self._target = target.copy()
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/base_pose.py 
49
50
51
52
53
54
55
56
57
58
59
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    assert isinstance(self.robot_move_group, RobotBaseGroup)

    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

g1_wbc

G1's whole-body PD/ONNX low-level controller: the reference stack's G1Controller, renamed here but behaviour unchanged. WbcJointGroup/ MoveGroupController are its own minimal joint-group and control-law holders, one per move group, not robot_views' MoveGroup or controllers/abstract.py's Controller; converging them is still open.

Classes:

Name Description
G1WholeBodyController
LeftArmController

Left arm: zero-gain actuators (falls under gravity, per
LegsWaistController

15-dof legs+waist PD law plus the ONNX WBC residual (one coupled law,
MoveGroupController

The control law for one WbcJointGroup (set_target +
RightArmController

Right arm: plain position-actuator passthrough (PD gains baked into
RightGripperController

Right gripper: single tendon position-actuator passthrough.
WbcJointGroup

Raw qpos/qvel/ctrl I/O for a named subset of G1's actuated DOFs; the

Functions:

Name Description
flat15_to_move_groups

gold's flat-15 action (ACT_* slices) -> molmo_spaces' move-group dict.

Attributes:

Name Type Description
ACTION_DIM
ACT_ARM
ACT_CMD
ACT_GRIP
ACT_HEIGHT
ACT_UPPER
ACT_WAIST
NUM_LEGS_WAIST_TARGET_DIMS

ACTION_DIM module-attribute 

ACTION_DIM = 15

ACT_ARM module-attribute 

ACT_ARM = slice(7, 14)

ACT_CMD module-attribute 

ACT_CMD = slice(0, 3)

ACT_GRIP module-attribute 

ACT_GRIP = 14

ACT_HEIGHT module-attribute 

ACT_HEIGHT = 3

ACT_UPPER module-attribute 

ACT_UPPER = slice(3, 14)

ACT_WAIST module-attribute 

ACT_WAIST = slice(4, 7)

NUM_LEGS_WAIST_TARGET_DIMS module-attribute 

NUM_LEGS_WAIST_TARGET_DIMS = 7

G1WholeBodyController 

G1WholeBodyController(models_dir: Path | str | None = None)

Methods:

Name Description
execute_action

Parse the flat 15-dim action, set_target on each controller, then
set_env
setup
Source code in molmo_spaces/controllers/g1_wbc.py 
263
264
265
266
267
268
269
270
def __init__(self, models_dir: Path | str | None = None):
    d = Path(models_dir) if models_dir is not None else _DEFAULT_MODELS_DIR
    self._stand_sess = ort.InferenceSession(
        str(d / "groot_balance.onnx"), providers=["CPUExecutionProvider"]
    )
    self._walk_sess = ort.InferenceSession(
        str(d / "groot_walk.onnx"), providers=["CPUExecutionProvider"]
    )
execute_action 
execute_action(action)

Parse the flat 15-dim action, set_target on each controller, then compute and write ctrl in gold's order: legs_waist torque from the previous call's _target_lower first, then the WBC inference that updates it. That ordering is load-bearing.

Source code in molmo_spaces/controllers/g1_wbc.py 
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
def execute_action(self, action):
    """Parse the flat 15-dim action, set_target on each controller, then
    compute and write ctrl in gold's order: legs_waist torque from the
    previous call's _target_lower first, then the WBC inference that
    updates it. That ordering is load-bearing.
    """
    cmd, height_cmd, waist = action[ACT_CMD], action[ACT_HEIGHT], action[ACT_WAIST]
    r_arm, r_grip = action[ACT_ARM], action[ACT_GRIP]

    self._legs_waist_ctrl.set_target((cmd, height_cmd, waist))
    self._right_arm_ctrl.set_target(r_arm)
    self._right_gripper_ctrl.set_target(r_grip)

    # Legacy _target_q bookkeeping, kept verbatim even though nothing
    # currently reads _target_q[:15]/[22:29] afterward (only [15:] is
    # read elsewhere, at reset time, by agents/policy_g1ms.py) -- cheap
    # to preserve exactly, so left in the one place gold itself had it.
    self._target_q[:15] = self._target_lower
    self._target_q[12:15] = waist
    self._target_q[15:22] = 0.0
    self._target_q[22:29] = r_arm

    d = self._data
    for controller in self._controllers:
        values = controller.compute_ctrl_inputs(d, self._step_counter)
        controller.move_group.set_ctrl(d, values)
set_env 
set_env(env)
Source code in molmo_spaces/controllers/g1_wbc.py 
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
def set_env(self, env):
    m = env.scene.model
    m.opt.timestep = 0.005
    m.opt.noslip_iterations = 5
    m.opt.impratio = 1.0
    m.opt.cone = mujoco.mjtCone.mjCONE_PYRAMIDAL
    m.opt.gravity[:] = [0, 0, -9.81]
    m.opt.enableflags = int(mujoco.mjtEnableBit.mjENBL_SLEEP)
    env.robot.n_substeps = 1
    for gid in range(m.ngeom):
        gname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_GEOM, gid) or ""
        if m.geom_type[gid] == mujoco.mjtGeom.mjGEOM_PLANE or "floor" in gname.lower():
            m.geom_friction[gid] = [1.0, 0.005, 0.0001]
            m.geom_solref[gid] = [0.02, 1.0]
    prefix = self._prefix if hasattr(self, "_prefix") else "robot_0/"
    for gid in range(m.ngeom):
        bid = m.geom_bodyid[gid]
        bname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, bid) or ""
        if (
            bname.startswith(prefix)
            and "ankle_roll" in bname
            and m.geom_type[gid] == mujoco.mjtGeom.mjGEOM_SPHERE
        ):
            m.geom_friction[gid] = [1.0, 0.005, 0.0001]
setup 
setup(model, data, prefix='robot_0/')
Source code in molmo_spaces/controllers/g1_wbc.py 
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
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
def setup(self, model, data, prefix="robot_0/"):
    self._model, self._data, self._prefix = model, data, prefix

    self._jqpos = np.array(
        [
            model.jnt_qposadr[
                mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"{prefix}{n}")
            ]
            for n in _WBC_BODY
        ],
        dtype=np.int32,
    )
    self._jdof = np.array(
        [
            model.jnt_dofadr[
                mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"{prefix}{n}")
            ]
            for n in _WBC_BODY
        ],
        dtype=np.int32,
    )
    self._fj_id = mujoco.mj_name2id(
        model, mujoco.mjtObj.mjOBJ_JOINT, f"{prefix}floating_base_joint"
    )
    self._fj_qa = model.jnt_qposadr[self._fj_id]
    self._fj_da = model.jnt_dofadr[self._fj_id]
    self._pelvis = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, f"{prefix}pelvis")

    self._act_ids = []
    for i, jname in enumerate(_WBC_BODY):
        aid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{prefix}walk_{jname}")
        if aid < 0:
            aid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{prefix}{jname}")
        self._act_ids.append(aid)
        if aid >= 0:
            if i < 15:
                model.actuator_gainprm[aid, 0] = 1.0
                model.actuator_biasprm[aid, :] = 0.0
                model.actuator_biastype[aid] = mujoco.mjtBias.mjBIAS_NONE
                model.actuator_gaintype[aid] = mujoco.mjtGain.mjGAIN_FIXED
            elif i < 22:
                # Left arm: zero gain so it falls under gravity.
                model.actuator_gainprm[aid, 0] = 0.0
                model.actuator_biasprm[aid, :] = 0.0
                model.actuator_forcerange[aid] = [-400, 400]
            else:
                kp, kd = 2000.0, 60.0
                model.actuator_gainprm[aid, 0] = kp
                model.actuator_biasprm[aid, 0] = 0.0
                model.actuator_biasprm[aid, 1] = -kp
                model.actuator_biasprm[aid, 2] = -kd
                model.actuator_forcerange[aid] = [-400, 400]
    self._act_ids = np.array(self._act_ids)

    for i in range(15, _WBC_N):
        jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"{prefix}{_WBC_BODY[i]}")
        if jid >= 0:
            model.jnt_actfrcrange[jid] = [-400, 400]
            model.dof_damping[model.jnt_dofadr[jid]] = 3.0

    self._rgripper = mujoco.mj_name2id(
        model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{prefix}right_grip"
    )

    # Shared WBC state -- read/written directly by agents/policy_g1ms.py's
    # subclass too (nav command, height smoothing), so these stay owned
    # by this G1WholeBodyController instance, not by LegsWaistController (see its
    # docstring for why).
    self._obs_history = collections.deque(maxlen=_WBC_OBS_HISTORY)
    self._obs_buffer = np.zeros(_WBC_OBS_DIM * _WBC_OBS_HISTORY, dtype=np.float32)
    self._wbc_action = np.zeros(_WBC_NUM_ACTIONS, dtype=np.float32)
    self._target_lower = _WBC_DEFAULT.copy()
    self._cmd = np.zeros(3, dtype=np.float32)
    self._height_cmd = _WBC_HEIGHT_CMD
    self._step_counter = 0

    self._target_q = np.zeros(_WBC_N, dtype=np.float32)
    self._target_q[:15] = _WBC_DEFAULT
    self._waist_target = np.zeros(3, dtype=np.float32)

    # MoveGroups: named slices of the 29-length _WBC_BODY index arrays
    # (+ the separately-resolved right-gripper actuator), matching G1's
    # real actuated groups (see molmo_spaces/robots/robot_views/g1_view.py
    # for the equivalent G1LegsWaistGroup/G1ArmGroup/G1GripperGroup).
    legs_waist_mg = WbcJointGroup(
        "legs_waist", self._jqpos[:15], self._jdof[:15], self._act_ids[:15]
    )
    left_arm_mg = WbcJointGroup(
        "left_arm", self._jqpos[15:22], self._jdof[15:22], self._act_ids[15:22]
    )
    right_arm_mg = WbcJointGroup(
        "right_arm", self._jqpos[22:29], self._jdof[22:29], self._act_ids[22:29]
    )
    right_gripper_mg = WbcJointGroup(
        "right_gripper",
        np.array([], dtype=np.int32),
        np.array([], dtype=np.int32),
        np.array([self._rgripper]),
    )

    self._legs_waist_ctrl = LegsWaistController(legs_waist_mg, owner=self)
    self._left_arm_ctrl = LeftArmController(left_arm_mg)
    self._right_arm_ctrl = RightArmController(right_arm_mg)
    self._right_gripper_ctrl = RightGripperController(right_gripper_mg)
    self._controllers = [
        self._legs_waist_ctrl,
        self._left_arm_ctrl,
        self._right_arm_ctrl,
        self._right_gripper_ctrl,
    ]

LeftArmController 

LeftArmController(move_group)

Bases: MoveGroupController

Left arm: zero-gain actuators (falls under gravity, per G1WholeBodyController.setup's actuator_gainprm[aid,0]=0.0). Target is always 0 -- execute_action never sets a left-arm target, matching gold exactly.

Methods:

Name Description
compute_ctrl_inputs
reset
set_target
set_to_stationary

Attributes:

Name Type Description
move_group
stationary bool
target
Source code in molmo_spaces/controllers/g1_wbc.py 
114
115
116
def __init__(self, move_group):
    self.move_group = move_group
    self.target = None
move_group instance-attribute 
move_group = move_group
stationary property 
stationary: bool
target instance-attribute 
target = None
compute_ctrl_inputs 
compute_ctrl_inputs(data, step_counter)
Source code in molmo_spaces/controllers/g1_wbc.py 
233
234
def compute_ctrl_inputs(self, data, step_counter):
    return np.zeros(len(self.move_group.act_ids), dtype=np.float32)
reset 
reset() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
135
136
def reset(self) -> None:
    self.target = None
set_target 
set_target(target)
Source code in molmo_spaces/controllers/g1_wbc.py 
118
119
def set_target(self, target):
    self.target = target
set_to_stationary 
set_to_stationary() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
132
133
def set_to_stationary(self) -> None:
    self.target = None

LegsWaistController 

LegsWaistController(move_group, owner)

Bases: MoveGroupController

15-dof legs+waist PD law plus the ONNX WBC residual (one coupled law, hence one move group). Its state (_cmd, _height_cmd, _target_lower, _step_counter, ...) lives on the owning G1WholeBodyController, which the pick policy reads and writes directly; this class owns only the math.

Methods:

Name Description
compute_ctrl_inputs
reset
set_target
set_to_stationary

Attributes:

Name Type Description
move_group
stationary bool
target
Source code in molmo_spaces/controllers/g1_wbc.py 
146
147
148
def __init__(self, move_group, owner):
    super().__init__(move_group)
    self._owner = owner
move_group instance-attribute 
move_group = move_group
stationary property 
stationary: bool
target instance-attribute 
target = None
compute_ctrl_inputs 
compute_ctrl_inputs(data, step_counter)
Source code in molmo_spaces/controllers/g1_wbc.py 
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
def compute_ctrl_inputs(self, data, step_counter):
    o = self._owner
    target_q15 = np.empty(15, dtype=np.float32)
    target_q15[:12] = o._target_lower[:12]
    target_q15[12:15] = o._waist_target

    q = self.move_group.joint_pos(data)
    dq = self.move_group.joint_vel(data)
    tau = (target_q15 - q) * _WBC_KP[:15] - dq * _WBC_KD[:15]
    for i in range(12, 15):
        tau[i] += data.qfrc_bias[self.move_group.jdof[i]]

    if step_counter % _WBC_CONTROL_DEC == 0:
        qj = data.qpos[o._jqpos[:29]].astype(np.float32)
        dqj = data.qvel[o._jdof[:29]].astype(np.float32)
        quat = data.qpos[o._fj_qa + 3 : o._fj_qa + 7].astype(np.float32)
        omega = data.qvel[o._fj_da + 3 : o._fj_da + 6].astype(np.float32)
        padded = np.zeros(29, dtype=np.float32)
        padded[:15] = _WBC_DEFAULT
        single_obs = np.zeros(_WBC_OBS_DIM, dtype=np.float32)
        single_obs[0:3] = o._cmd * _WBC_CMD_SCALE
        single_obs[3] = o._height_cmd
        single_obs[4:7] = [o._waist_target[1], o._waist_target[2], o._waist_target[0]]
        single_obs[7:10] = omega * _WBC_ANG_VEL_SCALE
        single_obs[10:13] = _gravity_orientation(quat)
        single_obs[13:42] = (qj - padded) * _WBC_DOF_POS_SCALE
        single_obs[42:71] = dqj * _WBC_DOF_VEL_SCALE
        single_obs[71:86] = o._wbc_action
        o._obs_history.append(single_obs)
        while len(o._obs_history) < _WBC_OBS_HISTORY:
            o._obs_history.appendleft(np.zeros_like(single_obs))
        for i, h in enumerate(o._obs_history):
            o._obs_buffer[i * _WBC_OBS_DIM : (i + 1) * _WBC_OBS_DIM] = h
        obs_tensor = o._obs_buffer[None, :].astype(np.float32)
        if np.linalg.norm(o._cmd) < 0.05:
            o._wbc_action = o._stand_sess.run(
                None, {o._stand_sess.get_inputs()[0].name: obs_tensor}
            )[0][0].astype(np.float32)
        else:
            o._wbc_action = o._walk_sess.run(
                None, {o._walk_sess.get_inputs()[0].name: obs_tensor}
            )[0][0].astype(np.float32)
        o._target_lower = o._wbc_action * _WBC_ACTION_SCALE + _WBC_DEFAULT

    return tau
reset 
reset()
Source code in molmo_spaces/controllers/g1_wbc.py 
150
151
152
153
154
155
156
157
158
159
def reset(self):
    o = self._owner
    o._cmd[:] = 0
    o._wbc_action[:] = 0
    o._target_lower[:] = _WBC_DEFAULT
    o._height_cmd = _WBC_HEIGHT_CMD
    o._waist_target[:] = 0
    o._obs_history.clear()
    for _ in range(_WBC_OBS_HISTORY):
        o._obs_history.append(np.zeros(_WBC_OBS_DIM, dtype=np.float32))
set_target 
set_target(target)
Source code in molmo_spaces/controllers/g1_wbc.py 
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
def set_target(self, target):
    # Two calling conventions, told apart by length: the reference stack's
    # (cmd3, height, waist3) and molmo_spaces' flat [vx, vy, yaw_rate,
    # height, waist(3)].
    if len(target) == NUM_LEGS_WAIST_TARGET_DIMS:
        target = np.asarray(target, dtype=np.float32)
        cmd, height_cmd, waist = target[0:3], float(target[3]), target[4:7]
    elif len(target) == 3:
        cmd, height_cmd, waist = target
    else:
        raise ValueError(
            "legs_waist target must be either (cmd3, height, waist3) or a flat "
            f"{NUM_LEGS_WAIST_TARGET_DIMS}-vector [vx, vy, yaw_rate, height, "
            f"waist(3)]; got length {len(target)}"
        )
    o = self._owner
    o._cmd[:] = cmd
    o._height_cmd = float(height_cmd)
    o._waist_target[:] = waist
set_to_stationary 
set_to_stationary() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
132
133
def set_to_stationary(self) -> None:
    self.target = None

MoveGroupController 

MoveGroupController(move_group)

The control law for one WbcJointGroup (set_target + compute_ctrl_inputs); G1WholeBodyController.execute_action dispatches.

Methods:

Name Description
compute_ctrl_inputs
reset
set_target
set_to_stationary

Attributes:

Name Type Description
move_group
stationary bool
target
Source code in molmo_spaces/controllers/g1_wbc.py 
114
115
116
def __init__(self, move_group):
    self.move_group = move_group
    self.target = None
move_group instance-attribute 
move_group = move_group
stationary property 
stationary: bool
target instance-attribute 
target = None
compute_ctrl_inputs 
compute_ctrl_inputs(data, step_counter)
Source code in molmo_spaces/controllers/g1_wbc.py 
121
122
def compute_ctrl_inputs(self, data, step_counter):
    raise NotImplementedError
reset 
reset() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
135
136
def reset(self) -> None:
    self.target = None
set_target 
set_target(target)
Source code in molmo_spaces/controllers/g1_wbc.py 
118
119
def set_target(self, target):
    self.target = target
set_to_stationary 
set_to_stationary() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
132
133
def set_to_stationary(self) -> None:
    self.target = None

RightArmController 

RightArmController(move_group)

Bases: MoveGroupController

Right arm: plain position-actuator passthrough (PD gains baked into the MJCF actuator, kp=2000/kd=60 -- see G1WholeBodyController.setup).

Methods:

Name Description
compute_ctrl_inputs
reset
set_target
set_to_stationary

Attributes:

Name Type Description
move_group
stationary bool
target
Source code in molmo_spaces/controllers/g1_wbc.py 
114
115
116
def __init__(self, move_group):
    self.move_group = move_group
    self.target = None
move_group instance-attribute 
move_group = move_group
stationary property 
stationary: bool
target instance-attribute 
target = None
compute_ctrl_inputs 
compute_ctrl_inputs(data, step_counter)
Source code in molmo_spaces/controllers/g1_wbc.py 
241
242
def compute_ctrl_inputs(self, data, step_counter):
    return np.asarray(self.target, dtype=np.float32)
reset 
reset() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
135
136
def reset(self) -> None:
    self.target = None
set_target 
set_target(target)
Source code in molmo_spaces/controllers/g1_wbc.py 
118
119
def set_target(self, target):
    self.target = target
set_to_stationary 
set_to_stationary() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
132
133
def set_to_stationary(self) -> None:
    self.target = None

RightGripperController 

RightGripperController(move_group)

Bases: MoveGroupController

Right gripper: single tendon position-actuator passthrough.

Methods:

Name Description
compute_ctrl_inputs
reset
set_target
set_to_stationary

Attributes:

Name Type Description
move_group
stationary bool
target
Source code in molmo_spaces/controllers/g1_wbc.py 
114
115
116
def __init__(self, move_group):
    self.move_group = move_group
    self.target = None
move_group instance-attribute 
move_group = move_group
stationary property 
stationary: bool
target instance-attribute 
target = None
compute_ctrl_inputs 
compute_ctrl_inputs(data, step_counter)
Source code in molmo_spaces/controllers/g1_wbc.py 
248
249
def compute_ctrl_inputs(self, data, step_counter):
    return np.asarray([self.target], dtype=np.float32)
reset 
reset() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
135
136
def reset(self) -> None:
    self.target = None
set_target 
set_target(target)
Source code in molmo_spaces/controllers/g1_wbc.py 
118
119
def set_target(self, target):
    self.target = target
set_to_stationary 
set_to_stationary() -> None
Source code in molmo_spaces/controllers/g1_wbc.py 
132
133
def set_to_stationary(self) -> None:
    self.target = None

WbcJointGroup 

WbcJointGroup(name, jqpos, jdof, act_ids)

Raw qpos/qvel/ctrl I/O for a named subset of G1's actuated DOFs; the index arrays are G1WholeBodyController.setup()'s.

Methods:

Name Description
joint_pos
joint_vel
set_ctrl

Attributes:

Name Type Description
act_ids
jdof
jqpos
name
Source code in molmo_spaces/controllers/g1_wbc.py 
92
93
94
95
96
def __init__(self, name, jqpos, jdof, act_ids):
    self.name = name
    self.jqpos = jqpos
    self.jdof = jdof
    self.act_ids = act_ids
act_ids instance-attribute 
act_ids = act_ids
jdof instance-attribute 
jdof = jdof
jqpos instance-attribute 
jqpos = jqpos
name instance-attribute 
name = name
joint_pos 
joint_pos(data)
Source code in molmo_spaces/controllers/g1_wbc.py 
98
99
def joint_pos(self, data):
    return data.qpos[self.jqpos].astype(np.float32)
joint_vel 
joint_vel(data)
Source code in molmo_spaces/controllers/g1_wbc.py 
101
102
def joint_vel(self, data):
    return data.qvel[self.jdof].astype(np.float32)
set_ctrl 
set_ctrl(data, values)
Source code in molmo_spaces/controllers/g1_wbc.py 
104
105
106
107
def set_ctrl(self, data, values):
    for i, aid in enumerate(self.act_ids):
        if aid >= 0:
            data.ctrl[aid] = values[i]

flat15_to_move_groups 

flat15_to_move_groups(flat, gripper_mg_id: str = 'right_gripper') -> dict

gold's flat-15 action (ACT_* slices) -> molmo_spaces' move-group dict.

Source code in molmo_spaces/controllers/g1_wbc.py 
252
253
254
255
256
257
258
259
def flat15_to_move_groups(flat, gripper_mg_id: str = "right_gripper") -> dict:
    """gold's flat-15 action (ACT_* slices) -> molmo_spaces' move-group dict."""
    flat = np.asarray(flat)
    return {
        "legs_waist": np.concatenate([flat[ACT_CMD], [flat[ACT_HEIGHT]], flat[ACT_WAIST]]),
        "right_arm": flat[ACT_ARM],
        gripper_mg_id: np.array([flat[ACT_GRIP]]),
    }

joint_pos

Classes:

Name Description
JointPosController

Generic joint position controller for a robot.

JointPosController 

JointPosController(robot_move_group: MoveGroup)

Bases: AbstractPositionController

Generic joint position controller for a robot. Computes joint position targets for the joint's actuators, and clips to control limits. NOTE: Assumes joint actuators use position inputs.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/joint_pos.py 
14
15
16
17
18
19
20
21
22
23
24
25
def __init__(self, robot_move_group: MoveGroup) -> None:
    super().__init__(robot_move_group)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = self.robot_move_group.joint_pos

    self._validate_actuators()

    self.reset()
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property writable 
target

The target state of the controller, set by the user of the controller.

target_pos property 
target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 
compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_pos.py 
57
58
59
60
61
62
63
64
65
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    # position control inputs: just pass through the target joint positions
    return self.target.copy()
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets

Source code in molmo_spaces/controllers/joint_pos.py 
67
68
69
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets"""
    self.set_to_stationary()  # Explicit reset to stationary mode
set_target 
set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/joint_pos.py 
43
44
45
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self.target = np.clip(target, self.ctrl_range[:, 0], self.ctrl_range[:, 1])
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_pos.py 
47
48
49
50
51
52
53
54
55
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to current joint positions to hold them stationary
    self._stationary = True
    self.target = self.robot_move_group.noop_ctrl

joint_rel_pos

Classes:

Name Description
JointRelPosController

Generic joint position controller for a robot.

JointRelPosController 

JointRelPosController(robot_move_group: MoveGroup)

Bases: AbstractPositionController

Generic joint position controller for a robot. Computes delta joint position targets for the joint's actuators, and clips to control limits.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets
set_target

Set the target joint positions for the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
desired_joint_positions
robot_move_group
stationary

Returns whether the controller is in stationary mode
target

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/joint_rel_pos.py 
13
14
15
16
17
18
19
20
21
22
23
24
def __init__(self, robot_move_group: MoveGroup) -> None:
    super().__init__(robot_move_group)

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self.target = np.zeros_like(
        self.robot_move_group.noop_ctrl
    )  # Initial pos target is 0 relative to current position
    self.desired_joint_positions = self.robot_move_group.noop_ctrl.copy()
    self.reset()
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
desired_joint_positions instance-attribute 
desired_joint_positions = self.robot_move_group.noop_ctrl.copy()
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary

Returns whether the controller is in stationary mode

target property writable 
target

The target state of the controller, set by the user of the controller.

target_pos property 
target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 
compute_ctrl_inputs()

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_rel_pos.py 
71
72
73
74
75
76
77
78
def compute_ctrl_inputs(self):
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    return self.desired_joint_positions.copy()
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets

Source code in molmo_spaces/controllers/joint_rel_pos.py 
80
81
82
def reset(self) -> None:
    """Reset the controller to its initial state, clearing any internal state or targets"""
    self.set_to_stationary()  # Explicit reset to stationary mode
set_target 
set_target(target: ndarray) -> None

Set the target joint positions for the controller. NOTE: For this controller we assume that the controller will be set to stationary every time before the relative target has to be computed

Source code in molmo_spaces/controllers/joint_rel_pos.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
def set_target(self, target: np.ndarray) -> None:
    """Set the target joint positions for the controller.
    NOTE: For this controller we assume that the controller will be set to
    stationary every time before the relative target has to be computed"""

    # Case to avoid drift: if target is close to zero, stay stationary
    if np.linalg.norm(target) < 1e-6:
        if not self._stationary:  # important to avoid re-setting to noop ctrl and drifting
            self.set_to_stationary()
    else:
        self._stationary = False
        self.target = target
        self.desired_joint_positions = self.robot_move_group.noop_ctrl + target
        self.desired_joint_positions = np.clip(
            self.desired_joint_positions, self.ctrl_range[:, 0], self.ctrl_range[:, 1]
        )
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_rel_pos.py 
60
61
62
63
64
65
66
67
68
69
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to current joint positions to hold them stationary
    self._stationary = True
    self.target = np.zeros_like(self.robot_move_group.noop_ctrl)
    self.desired_joint_positions = self.robot_move_group.noop_ctrl.copy()

joint_vel

Classes:

Name Description
JointVelController

Generic joint velocity controller for a robot.

JointVelController 

JointVelController(robot_config, robot_move_group: MoveGroup)

Bases: Controller

Generic joint velocity controller for a robot. Computes joint position targets for the joint's actuators after euler integration, and clips to control limits. NOTE: Assumes joint actuators use position inputs.

robot_config: The configuration of the robot, containing special information about the control, e.g., delta_t for integration (if applicable) etc. robot_move_group: The move group of the robot to be controlled

Methods:

Name Description
compute_ctrl_inputs

Compute the control inputs based on the current state and the target set by the user.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
ctrl_dim
ctrl_range
euler_dt
robot_config
robot_move_group
stationary bool

Returns whether the controller is in stationary mode
target ndarray

Returns the current target joint velocities
Source code in molmo_spaces/controllers/joint_vel.py 
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
def __init__(self, robot_config, robot_move_group: MoveGroup) -> None:
    """
    Args:
    robot_config: The configuration of the robot, containing special information about the control,
                  e.g., delta_t for integration (if applicable) etc.
    robot_move_group: The move group of the robot to be controlled
    """
    super().__init__(robot_move_group)

    self.robot_config = robot_config
    self.euler_dt = self.robot_config.delta_t  # Time step for euler integration

    self.ctrl_dim = robot_move_group.n_actuators
    self.ctrl_range = robot_move_group.ctrl_limits

    self._stationary = True
    self._target = np.zeros(self.ctrl_dim)  # Initially zero vel targets

    self._validate_actuators()

    self.reset()
ctrl_dim instance-attribute 
ctrl_dim = robot_move_group.n_actuators
ctrl_range instance-attribute 
ctrl_range = robot_move_group.ctrl_limits
euler_dt instance-attribute 
euler_dt = self.robot_config.delta_t
robot_config instance-attribute 
robot_config = robot_config
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Returns whether the controller is in stationary mode

target property 
target: ndarray

Returns the current target joint velocities

compute_ctrl_inputs 
compute_ctrl_inputs() -> ndarray

Compute the control inputs based on the current state and the target set by the user.

Returns:

Type Description
ndarray

The control inputs to be applied to the robot actuators, in this case: positions
Source code in molmo_spaces/controllers/joint_vel.py 
60
61
62
63
64
65
66
67
68
69
70
71
72
def compute_ctrl_inputs(self) -> np.ndarray:
    """
    Compute the control inputs based on the current state and the target set by the user.

    Returns:
        The control inputs to be applied to the robot actuators, in this case: positions
    """
    # position control inputs: perform euler integration on the target velocities
    ctrl_inputs = self.robot_move_group.joint_pos + self._target * self.euler_dt
    # Clip the control inputs to the actuator limits
    ctrl_inputs = np.clip(ctrl_inputs, self.ctrl_range[:, 0], self.ctrl_range[:, 1])

    return ctrl_inputs
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/joint_vel.py 
74
75
def reset(self) -> None:
    self.set_to_stationary()
set_target 
set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/joint_vel.py 
46
47
48
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self._target = target.copy()
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift.

Source code in molmo_spaces/controllers/joint_vel.py 
50
51
52
53
54
55
56
57
58
def set_to_stationary(self) -> None:
    """
    This method sets the robot to stationary mode and computes the targets to hold the
    robot stationary.
    This is useful when the robot needs to be stopped at a certain position and not drift.
    """
    # Set to stationary mode and set target to zero joint velocities to hold joints stationary
    self._stationary = True
    self._target = np.zeros(self.ctrl_dim)

torso_height

Classes:

Name Description
TorsoHeightJointPosController

Controls the RBY1 torso via a single height scalar.

TorsoHeightJointPosController 

TorsoHeightJointPosController(robot_move_group: MoveGroup, max_height: float = 0.738)

Bases: AbstractPositionController

Controls the RBY1 torso via a single height scalar.

Accepts a 1-element command [h]; compute_ctrl_inputs() returns the full 6-DOF joint position array produced by the constraint torso_1 = torso_3 = h, torso_2 = -2*h.

Parameters:

Name Type Description Default
robot_move_group MoveGroup

The move group of the robot to be controlled

 required

Methods:

Name Description
compute_ctrl_inputs

Return the 6-DOF torso joint positions for the current target height.
height_to_joints

Return the 6-DOF torso joint positions for a given height scalar.
hold_at_height

Set the controller to hold at a specific height without reading current joints.
joints_to_height

Recover the height scalar from a 6-DOF torso joint position array.
reset

Reset the controller to its initial state, clearing any internal state or targets.
set_target

Set the target state of the controller.
set_to_stationary

This method sets the robot to stationary mode and computes the targets to hold the

Attributes:

Name Type Description
max_height
robot_move_group
stationary bool

Whether the controller is set to hold the robot stationary, for e.g.
target ndarray

The target state of the controller, set by the user of the controller.
target_pos ndarray

The target absolute position of the controller, set by the user of the controller.
Source code in molmo_spaces/controllers/torso_height.py 
33
34
35
36
37
def __init__(self, robot_move_group: MoveGroup, max_height: float = 0.738) -> None:
    super().__init__(robot_move_group)
    self.max_height = max_height
    self._stationary = True
    self._target_height: float = self.joints_to_height(robot_move_group.joint_pos)
max_height instance-attribute 
max_height = max_height
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary: bool

Whether the controller is set to hold the robot stationary, for e.g. when the robot has to be stopped at a certain position and not drift.

target property 
target: ndarray

The target state of the controller, set by the user of the controller.

target_pos property 
target_pos: ndarray

The target absolute position of the controller, set by the user of the controller.

compute_ctrl_inputs 
compute_ctrl_inputs() -> ndarray

Return the 6-DOF torso joint positions for the current target height.

Source code in molmo_spaces/controllers/torso_height.py 
69
70
71
72
def compute_ctrl_inputs(self) -> np.ndarray:
    """Return the 6-DOF torso joint positions for the current target height."""
    joints = self.height_to_joints(self._target_height)
    return joints
height_to_joints staticmethod 
height_to_joints(height: float) -> ndarray

Return the 6-DOF torso joint positions for a given height scalar.

torso[1] = height, torso[2] = -2*height, torso[3] = height. Indices 0, 4, 5 are left at zero.

Source code in molmo_spaces/controllers/torso_height.py 
15
16
17
18
19
20
21
22
23
24
25
26
@staticmethod
def height_to_joints(height: float) -> np.ndarray:
    """Return the 6-DOF torso joint positions for a given height scalar.

    torso[1] = height, torso[2] = -2*height, torso[3] = height.
    Indices 0, 4, 5 are left at zero.
    """
    joints = np.zeros(6)
    joints[1] = height
    joints[2] = -2.0 * height
    joints[3] = height
    return joints
hold_at_height 
hold_at_height(height: float) -> None

Set the controller to hold at a specific height without reading current joints.

Use this to lock the torso at a desired target after a height adjustment is complete, so that subsequent steps without an explicit torso command will maintain this height rather than drifting to the current joint position.

Source code in molmo_spaces/controllers/torso_height.py 
59
60
61
62
63
64
65
66
67
def hold_at_height(self, height: float) -> None:
    """Set the controller to hold at a specific height without reading current joints.

    Use this to lock the torso at a desired target after a height adjustment is
    complete, so that subsequent steps without an explicit torso command will
    maintain this height rather than drifting to the current joint position.
    """
    self._stationary = True
    self._target_height = float(np.clip(height, 0.0, self.max_height))
joints_to_height staticmethod 
joints_to_height(joints: ndarray) -> float

Recover the height scalar from a 6-DOF torso joint position array.

Source code in molmo_spaces/controllers/torso_height.py 
28
29
30
31
@staticmethod
def joints_to_height(joints: np.ndarray) -> float:
    """Recover the height scalar from a 6-DOF torso joint position array."""
    return float(joints[1])
reset 
reset() -> None

Reset the controller to its initial state, clearing any internal state or targets.

Source code in molmo_spaces/controllers/torso_height.py 
74
75
def reset(self) -> None:
    self.set_to_stationary()
set_target 
set_target(target: ndarray) -> None

Set the target state of the controller.

Parameters:

Name Type Description Default
target ndarray

The target state to be set, e.g. a joint position, base velocity, or any other state.

 required
Source code in molmo_spaces/controllers/torso_height.py 
51
52
53
def set_target(self, target: np.ndarray) -> None:
    self._stationary = False
    self._target_height = float(np.clip(target[0], 0.0, self.max_height))
set_to_stationary 
set_to_stationary() -> None

This method sets the robot to stationary mode and computes the targets to hold the robot stationary. This is useful when the robot needs to be stopped at a certain position and not drift. NOTE: If the controller is already stationary, it does not change the target.

Source code in molmo_spaces/controllers/torso_height.py 
55
56
57
def set_to_stationary(self) -> None:
    self._stationary = True
    self._target_height = self.joints_to_height(self.robot_move_group.joint_pos)