Skip to content

controllers

controllers

Modules:

Name Description
abstract
base_pose
joint_pos
joint_rel_pos
joint_vel
torso_height

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

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/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

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

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()

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.

Source code in molmo_spaces/controllers/abstract.py 
55
56
57
58
59
60
61
62
@abc.abstractmethod
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, eg. positions, torques etc.
    """
    pass
reset abstractmethod 
reset()

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

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

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.

Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target):
    """
    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.
    """
    pass
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
53
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.
    """
    pass

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

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.
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

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

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

compute_ctrl_inputs abstractmethod 
compute_ctrl_inputs()

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.

Source code in molmo_spaces/controllers/abstract.py 
55
56
57
58
59
60
61
62
@abc.abstractmethod
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, eg. positions, torques etc.
    """
    pass
reset abstractmethod 
reset()

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

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

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.

Source code in molmo_spaces/controllers/abstract.py 
37
38
39
40
41
42
43
44
@abc.abstractmethod
def set_target(self, target):
    """
    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.
    """
    pass
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
53
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.
    """
    pass

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.

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

Whether the controller is in stationary mode
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
def __init__(self, robot_move_group: RobotBaseGroup) -> 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.pose.copy()  # Initial target pose is current base pose

    # TODO: Check if the move_group's actuators support this control type

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

Whether the controller is in stationary mode

target property 
target

The target pose for the controller

compute_base_velocities abstractmethod 
compute_base_velocities(target_base_pose)

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):
    """
    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: The control inputs to be applied to the robot actuators, in this case: positions

Source code in molmo_spaces/controllers/base_pose.py 
60
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
    """
    # velocity control inputs: compute base velocities to achieve target pose
    ctrl_inputs = self.compute_base_velocities(self._target)
    # Clip to control 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/base_pose.py 
82
83
84
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_base_pose) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
43
44
45
46
47
def set_target(self, target_base_pose) -> None:
    """Set the target pose for the controller"""
    self._stationary = False  # Exit stationary mode when target is provided

    self._target = target_base_pose.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
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 base pose to hold it stationary
    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. 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

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

Whether the controller is in stationary mode
target

The target pose for the controller
wheel_base
wheel_radius
Source code in molmo_spaces/controllers/base_pose.py 
88
89
90
91
92
93
94
95
96
97
98
99
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 = n_actuators
ctrl_range instance-attribute 
ctrl_range = ctrl_limits
robot_config instance-attribute 
robot_config = robot_config
robot_move_group instance-attribute 
robot_move_group = robot_move_group
stationary property 
stationary

Whether the controller is in stationary mode

target property 
target

The target pose for the controller

wheel_base instance-attribute 
wheel_base = wheel_base
wheel_radius instance-attribute 
wheel_radius = 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. Args: target_base_pose: Target [x, y, theta] pose in world frame. Returns: np.ndarray: [left_wheel_velocity, right_wheel_velocity]

Source code in molmo_spaces/controllers/base_pose.py 
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
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: Test this controller
    # TODO: Handle case where base_pose frame is not in the center of the wheels

    # Current pose
    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
    -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: The control inputs to be applied to the robot actuators, in this case: positions

Source code in molmo_spaces/controllers/base_pose.py 
60
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
    """
    # velocity control inputs: compute base velocities to achieve target pose
    ctrl_inputs = self.compute_base_velocities(self._target)
    # Clip to control 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/base_pose.py 
82
83
84
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_base_pose) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
43
44
45
46
47
def set_target(self, target_base_pose) -> None:
    """Set the target pose for the controller"""
    self._stationary = False  # Exit stationary mode when target is provided

    self._target = target_base_pose.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
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 base pose to hold it stationary
    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. 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

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

Whether the controller is in stationary mode
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 
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
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 = n_actuators
ctrl_range instance-attribute 
ctrl_range = ctrl_limits
current_steer_angles instance-attribute 
current_steer_angles = zeros(4)
max_wheel_speed instance-attribute 
max_wheel_speed = 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

Whether the controller is in stationary mode

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

The target pose for the controller

wheel_base instance-attribute 
wheel_base = wheel_base
wheel_radius instance-attribute 
wheel_radius = 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. 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
Source code in molmo_spaces/controllers/base_pose.py 
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
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: Test this controller
    # TODO: Handle case where base_pose frame is not in the center of the wheels

    # Current pose
    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: The control inputs to be applied to the robot actuators, in this case: positions

Source code in molmo_spaces/controllers/base_pose.py 
60
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
    """
    # velocity control inputs: compute base velocities to achieve target pose
    ctrl_inputs = self.compute_base_velocities(self._target)
    # Clip to control 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/base_pose.py 
82
83
84
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_base_pose) -> None

Set the target pose for the controller

Source code in molmo_spaces/controllers/base_pose.py 
43
44
45
46
47
def set_target(self, target_base_pose) -> None:
    """Set the target pose for the controller"""
    self._stationary = False  # Exit stationary mode when target is provided

    self._target = target_base_pose.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
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 base pose to hold it stationary
    self._stationary = True
    current_base_pose = self.robot_move_group.pose
    self._target = current_base_pose.copy()

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.

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
robot_move_group
stationary

Returns whether the controller is in stationary mode
target
target_pos ndarray
Source code in molmo_spaces/controllers/joint_pos.py 
14
15
16
17
18
19
20
21
22
23
24
25
26
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
    )  # Initial pos target is current joint positions

    # TODO: Check if the move_group's actuators support this control type
    self.reset()
ctrl_dim instance-attribute 
ctrl_dim = n_actuators
ctrl_range instance-attribute 
ctrl_range = ctrl_limits
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
target_pos property 
target_pos: ndarray
compute_ctrl_inputs 
compute_ctrl_inputs()

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

Source code in molmo_spaces/controllers/joint_pos.py 
61
62
63
64
65
66
67
68
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 
70
71
72
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_joint_positions) -> None

Set the target joint positions for the controller

Source code in molmo_spaces/controllers/joint_pos.py 
45
46
47
48
49
def set_target(self, target_joint_positions) -> None:
    """Set the target joint positions for the controller"""
    self._stationary = False  # Exit stationary mode when target is provided

    self.target = np.clip(target_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_pos.py 
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.
    """
    # 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.

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
target_pos ndarray
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 = n_actuators
ctrl_range instance-attribute 
ctrl_range = ctrl_limits
desired_joint_positions instance-attribute 
desired_joint_positions = 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
target_pos property 
target_pos: ndarray
compute_ctrl_inputs 
compute_ctrl_inputs()

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

Source code in molmo_spaces/controllers/joint_rel_pos.py 
73
74
75
76
77
78
79
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 
81
82
83
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_rel_joint_positions) -> 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
59
60
def set_target(self, target_rel_joint_positions) -> 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_rel_joint_positions) < 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_rel_joint_positions
        self.desired_joint_positions = (
            self.robot_move_group.noop_ctrl + target_rel_joint_positions
        )
        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 
62
63
64
65
66
67
68
69
70
71
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 joint velocities 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
euler_dt
robot_config
robot_move_group
stationary

Returns whether the controller is in stationary mode
target

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
35
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
    # TODO:
    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

    # TODO: Check if the move_group's actuators support this control type

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

Returns whether the controller is in stationary mode

target property 
target

Returns the current target joint velocities

compute_ctrl_inputs 
compute_ctrl_inputs()

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

Source code in molmo_spaces/controllers/joint_vel.py 
62
63
64
65
66
67
68
69
70
71
72
73
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: 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 
75
76
77
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_joint_velocities) -> None

Set the target joint velocities for the controller

Source code in molmo_spaces/controllers/joint_vel.py 
47
48
49
50
def set_target(self, target_joint_velocities) -> None:
    """Set the target joint velocities for the controller"""
    self._stationary = False  # Exit stationary mode when target is provided
    self._target = target_joint_velocities.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 
52
53
54
55
56
57
58
59
60
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.

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
set_target
set_to_stationary

Attributes:

Name Type Description
max_height
robot_move_group
stationary bool
target ndarray
target_pos ndarray
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
target property 
target: ndarray
target_pos property 
target_pos: ndarray
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
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
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
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)