toddlerbot.sim package

Subpackages

Submodules

toddlerbot.sim.motor_control module

Motor control implementations for ToddlerBot simulation.

Provides PD controllers with asymmetric saturation models for both Dynamixel motor control and basic position control.

class toddlerbot.sim.motor_control.MotorController(robot: Robot)

Bases: object

A class for controlling the Dynamixel motors of a robot.

step(q: Array | ndarray[Any, dtype[float32]], q_dot: Array | ndarray[Any, dtype[float32]], q_dot_dot: Array | ndarray[Any, dtype[float32]], a: Array | ndarray[Any, dtype[float32]], noise: Dict[str, Array | ndarray[Any, dtype[float32]]] = {})

Compute torque commands for Dynamixel motors with an asymmetric saturation model, implemented in JAX.

The controller produces a PD torque (kp * (a - q) - kd_min * q_dot) and then clamps it by two different limits:

Acceleration-side limit — a piece-wise linear envelope constant tau_max until |q_dot| > q_dot_tau_max linearly tapers to tau_q_dot_max at |q_dot| = q_dot_max zero beyond q_dot_max.

Brake-side limit — fixed at tau_brake_max.

Positive joint velocity uses [ -tau_brake_max, +tau_acc_limit ]; negative velocity uses [ -tau_acc_limit, +tau_brake_max ].

Any optional input left as None defaults to the corresponding attribute initialised in __init__; otherwise the provided value (scalar or vector) is used.

Parameters:

  • q (ArrayType) – Joint positions (rad or m).

  • q_dot (ArrayType) – Joint velocities (rad/s or m/s).

  • a (ArrayType) – Desired action (reference position or torque proxy).

  • noise (Optional[ArrayType]) – Additive noise applied to tau_acc_limit (useful for system-ID perturbations).

Returns:

Torque command after asymmetric saturation.

Return type:

ArrayType

class toddlerbot.sim.motor_control.PositionController

Bases: object

A class for controlling the position of a robot’s joints.

step(q: Array | ndarray[Any, dtype[float32]], q_dot: Array | ndarray[Any, dtype[float32]], a: Array | ndarray[Any, dtype[float32]])

Advances the system state by one time step using the provided acceleration.

Parameters:

  • q (ArrayType) – The current state vector of the system.

  • q_dot (ArrayType) – The current velocity vector of the system.

  • a (ArrayType) – The acceleration vector to be applied.

Returns:

The acceleration vector a.

Return type:

ArrayType

toddlerbot.sim.mujoco_sim module

class toddlerbot.sim.mujoco_sim.MuJoCoSim(robot: Robot, n_frames: int = 20, dt: float = 0.001, fixed_base: bool = False, xml_path: str = '', vis_type: str = '', controller_type: str = 'torque')

Bases: BaseSim

MuJoCo physics simulation environment for robot control and testing.

This class provides a MuJoCo-based simulation environment for robotics applications, supporting both torque and position control modes. It handles robot model loading, physics stepping, visualization, and observation collection.

The simulation supports various robot configurations including fixed-base and free-floating robots, with customizable control frequencies and visualization options.

add_ee_marker(pos, color)
check_self_collisions()
close()

Closes the visualizer if it is currently open.

forward()

Advances the simulation forward by a specified number of frames and visualizes the result if a visualizer is available.

Iterates through the simulation for the number of frames specified by self.n_frames, updating the model state at each step. If a visualizer is provided, it visualizes the current state of the simulation data.

get_body_transform(body_name: str)

Computes the transformation matrix for a specified body.

Parameters:

body_name (str) – The name of the body for which to compute the transformation matrix.

Returns:

A 4x4 transformation matrix representing the position and orientation of the body.

Return type:

np.ndarray

get_joint_angles(type: str = 'dict') → Dict[str, float] | ndarray[Any, dtype[float32]]

Retrieves the current joint angles of the robot.

Parameters:

type (str) – The format in which to return the joint angles. Options are “dict” for a dictionary format or “array” for a NumPy array. Defaults to “dict”.

Returns:

The joint angles of the robot.

Returns a dictionary with joint names as keys and angles as values if type is “dict”. Returns a NumPy array of joint angles if type is “array”.

Return type:

Dict[str, float] or npt.NDArray[np.float32]

get_motor_angles(type: str = 'dict') → Dict[str, float] | ndarray[Any, dtype[float32]]

Retrieves the current angles of the robot’s motors.

Parameters:

type (str) – The format in which to return the motor angles. Options are “dict” for a dictionary format or “array” for a NumPy array. Defaults to “dict”.

Returns:

The motor angles in the specified format. If “dict”, returns a dictionary with motor names as keys and angles as values. If “array”, returns a NumPy array of motor angles.

Return type:

Dict[str, float] or npt.NDArray[np.float32]

get_observation() → Obs

Retrieves the current observation of the robot’s state, including motor and joint states, and torso dynamics.

Returns:

An observation object.

Return type:

Obs

get_site_transform(site_name: str)

Retrieves the transformation matrix for a specified site.

This method constructs a 4x4 transformation matrix for the given site name, using the site’s position and orientation matrix. The transformation matrix is composed of a 3x3 rotation matrix and a 3x1 translation vector.

Parameters:

site_name (str) – The name of the site for which to retrieve the transformation matrix.

Returns:

A 4x4 transformation matrix representing the site’s position and orientation.

Return type:

numpy.ndarray

save_recording(exp_folder_path: str, dt: float, render_every: int, name: str = 'eval.mp4')

Saves a recording of the current MuJoCo simulation.

Parameters:

  • exp_folder_path (str) – The path to the folder where the recording will be saved.

  • dt (float) – The time step interval for the recording.

  • render_every (int) – The frequency at which frames are rendered.

  • name (str, optional) – The name of the output video file. Defaults to “eval.mp4”.

set_joint_angles(joint_angles: Dict[str, float] | ndarray[Any, dtype[float32]])

Sets the joint angles of the robot.

This method updates the joint positions of the robot based on the provided joint angles. It converts the input joint angles to motor and passive angles and updates the robot’s data structure accordingly.

Parameters:

joint_angles (Dict[str, float] | npt.NDArray[np.float32]) – A dictionary mapping joint names to their respective angles, or a NumPy array of joint angles in the order specified by the robot’s joint ordering.

set_joint_dynamics(joint_dyn: Dict[str, Dict[str, float]])

Sets the dynamics parameters for specified joints in the model.

Parameters:

joint_dyn (Dict[str, Dict[str, float]]) – A dictionary where each key is a joint name and the value is another dictionary containing dynamics parameters and their corresponding values to be set for that joint.

set_motor_angles(motor_angles: Dict[str, float] | ndarray[Any, dtype[float32]])

Sets the motor angles for the robot and updates the corresponding joint and passive angles.

Parameters:

motor_angles (Dict[str, float] | npt.NDArray[np.float32]) – A dictionary mapping motor names to angles or an array of motor angles in the order specified by the robot’s motor ordering.

set_motor_dynamics(motor_dyn: Dict[str, float])

Sets the motor dynamics by updating the controller’s attributes.

Parameters:

motor_dyn (Dict[str, float]) – A dictionary where keys are attribute names and values are the corresponding dynamics values to be set on the controller.

set_motor_kps(motor_kps: Dict[str, float])

Sets the proportional gain (Kp) values for the motors.

This method updates the Kp values for each motor specified in the motor_kps dictionary. If the controller is an instance of MotorController, it sets the Kp value directly in the controller. Otherwise, it adjusts the gain and bias parameters of the actuator model.

Parameters:

motor_kps (Dict[str, float]) – A dictionary where keys are motor names and values are the Kp values to be set.

set_motor_target(motor_angles: Dict[str, float] | ndarray[Any, dtype[float32]])

Sets the target angles for the motors.

Parameters:

motor_angles (Dict[str, float] | npt.NDArray[np.float32]) – A dictionary mapping motor names to their target angles or a NumPy array of target angles. If a dictionary is provided, the values are converted to a NumPy array of type float32.

set_qpos(qpos: ndarray[Any, dtype[float32]])

Set the position of the system’s generalized coordinates.

Parameters:

qpos (npt.NDArray[np.float32]) – An array representing the desired positions of the system’s generalized coordinates.

step()

Advances the simulation by a specified number of frames and updates the visualizer.

This method iterates over the number of frames defined by n_frames, updating the control inputs using the controller’s step method based on the current position and velocity of the motors. It then advances the simulation state using Mujoco’s mj_step function. If a visualizer is provided, it updates the visualization with the current simulation data.

toddlerbot.sim.mujoco_utils module

MuJoCo utilities for visualization, rendering, and recording.

Provides classes for interactive viewing, multi-camera video recording, and basic rendering functionality for MuJoCo simulations.

class toddlerbot.sim.mujoco_utils.MuJoCoRenderer(model: Any, height: int = 360, width: int = 640)

Bases: object

A class for rendering MuJoCo simulation data and saving video recordings.

anim_pose_callback(data: Any)

Processes animation pose data and updates the animation data dictionary.

Parameters:

data (Any) – An object containing pose data for multiple bodies, including body names, positions, orientations, and a timestamp.

Updates:

self.anim_data (dict): A dictionary where each key is a body name and each value is a list of tuples. Each tuple contains a timestamp, position, and orientation for the corresponding body.

close()

Closes the renderer associated with the current instance.

This method ensures that the renderer is properly closed and any resources associated with it are released.

save_recording(exp_folder_path: str, dt: float, render_every: int, name: str = 'eval.mp4', dump_data: bool = False)

Saves a recording of the simulation from multiple camera angles and optionally dumps animation data.

Parameters:

  • exp_folder_path (str) – The path to the folder where the recording and data will be saved.

  • dt (float) – The time step duration for rendering frames.

  • render_every (int) – The interval at which frames are rendered.

  • name (str, optional) – The name of the final video file. Defaults to “eval.mp4”.

  • dump_data (bool, optional) – If True, dumps the animation data to a pickle file. Defaults to False.

visualize(data: Any, vis_data: Dict[str, Any] = {})

Visualizes the given data by updating pose, position, and velocity information.

This method processes the input data to update the animation pose and appends the position and velocity data to their respective lists for further visualization.

Parameters:

  • data (Any) – The input data containing pose, position, and velocity information.

  • vis_data (Dict[str, Any], optional) – Additional visualization data. Defaults to an empty dictionary.

class toddlerbot.sim.mujoco_utils.MuJoCoViewer(robot: Robot, model: Any, data: Any)

Bases: object

A class for visualizing MuJoCo simulation data.

close()

Closes the viewer associated with the current instance.

visualize(data: Any, vis_flags: List[str] = ['com', 'support_poly'])

Visualizes specified components of the data using the viewer.

Parameters:

  • data (Any) – The data to be visualized.

  • vis_flags (List[str], optional) – A list of visualization flags indicating which components to visualize. Defaults to [“com”, “support_poly”].

visualize_com(data: Any)

Visualize the center of mass (COM) of a given body in the simulation environment.

This function adds a visual representation of the center of mass for a specified body to the simulation viewer. The COM is depicted as a small red sphere.

Parameters:

data (Any) – The simulation data object containing information about the bodies, including their center of mass positions.

visualize_support_poly(data: Any)

Visualizes the support polygon of the feet by drawing lines between the transformed bounding box corners in the world coordinates.

Parameters:

data (Any) – The simulation data object containing geometric information about the feet.

toddlerbot.sim.mujoco_utils.mj_render(model, data, lib='plt')

Renders a MuJoCo simulation scene using the specified library.

Parameters:

  • model – The MuJoCo model to be rendered.

  • data – The simulation data associated with the model.

  • lib (str) – The library to use for rendering. Options are “plt” for matplotlib and any other value for OpenCV. Defaults to “plt”.

Raises:

ValueError – If the specified library is not supported.

toddlerbot.sim.real_world module

toddlerbot.sim.robot module

Robot configuration and kinematics for ToddlerBot.

Defines the Robot class with motor configurations, joint limits, forward/inverse kinematics, and transmission mappings.

class toddlerbot.sim.robot.Robot(robot_name: str)

Bases: object

This class defines some data strucutres, FK, IK of ToddlerBot.

arm_fk(arm_motor_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the forward kinematics for an arm by converting motor positions to joint positions.

Parameters:

arm_motor_pos (ArrayType) – An array of motor positions for the arm.

Returns:

An array of joint positions corresponding to the given motor positions.

Return type:

ArrayType

arm_ik(arm_joint_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the motor positions for an arm based on joint positions and gear ratio.

Parameters:

arm_joint_pos (ArrayType) – The joint positions of the arm.

Returns:

The calculated motor positions for the arm.

Return type:

ArrayType

get_transmission(motor_name: str) → str

Determine transmission type based on motor name suffix.

Parameters:

motor_name – Name of the motor.

Returns:

Transmission type string.

joint_to_dof_angles(joint_angles: Dict[str, float]) → Dict[str, float]

Convert joint angles to degree-of-freedom angles.

Parameters:

joint_angles – Dictionary of joint angles.

Returns:

Dictionary including both motor and joint angles.

joint_to_motor_angles(joint_angles: Dict[str, float]) → Dict[str, float]

Converts joint angles to motor angles based on the transmission type specified in the configuration.

Parameters:

joint_angles (Dict[str, float]) – A dictionary mapping joint names to their respective angles.

Returns:

A dictionary mapping motor names to their calculated angles.

Return type:

Dict[str, float]

leg_fk(leg_motor_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Converts leg motor positions to leg joint positions using the gear ratio.

Parameters:

leg_motor_pos (ArrayType) – An array of motor positions for the leg.

Returns:

An array of joint positions for the leg, calculated by applying the gear ratio to the motor positions.

Return type:

ArrayType

leg_ik(leg_joint_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the motor positions for a leg based on joint positions and gear ratio.

Parameters:

leg_joint_pos (ArrayType) – The joint positions of the leg.

Returns:

The calculated motor positions for the leg.

Return type:

ArrayType

motor_to_dof_angles(motor_angles: Dict[str, float]) → Dict[str, float]

Convert motor angles to degree-of-freedom angles.

Parameters:

motor_angles – Dictionary of motor angles.

Returns:

Dictionary including both joint and motor angles.

motor_to_joint_angles(motor_angles: Dict[str, float]) → Dict[str, float]

Convert motor angles to joint angles using transmission ratios.

Parameters:

motor_angles – Dictionary of motor angles.

Returns:

Dictionary of joint angles.

neck_fk(neck_motor_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the neck joint positions from the neck motor positions.

Parameters:

neck_motor_pos (ArrayType) – The positions of the neck motors.

Returns:

The calculated positions of the neck joints.

Return type:

ArrayType

neck_ik(neck_joint_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the motor positions for the neck based on the joint positions.

Parameters:

neck_joint_pos (ArrayType) – The positions of the neck joints.

Returns:

The calculated motor positions for the neck.

Return type:

ArrayType

waist_fk(waist_motor_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the forward kinematics for the waist joint based on motor positions.

Parameters:

waist_motor_pos (ArrayType) – An array containing the positions of the waist motors.

Returns:

An array containing the calculated waist roll and yaw angles.

Return type:

ArrayType

waist_ik(waist_joint_pos: Array | ndarray[Any, dtype[float32]]) → Array | ndarray[Any, dtype[float32]]

Calculates the inverse kinematics for the waist joint actuators.

Parameters:

waist_joint_pos (ArrayType) – The position of the waist joint, represented as an array.

Returns:

An array containing the calculated positions for the two waist joint actuators.

Return type:

ArrayType

Module contents

class toddlerbot.sim.BaseSim(name: str)

Bases: ABC

Base class for simulation environments and real robot interfaces.

This abstract base class defines the common interface for both simulation environments (like MuJoCo) and real robot hardware interfaces. It provides a unified API for motor control, observation collection, and simulation stepping.

All concrete implementations must override the abstract methods to provide specific functionality for their respective environments.

abstractmethod close()

Clean up and close the simulation or robot connection.

This method should properly release resources, close connections, and perform any necessary cleanup operations.

abstractmethod get_observation() → Obs

Retrieve the current observation from the simulation or robot.

Returns:

An Obs dataclass containing the current robot state information including motor positions, velocities, and sensor data.

abstractmethod set_motor_kps(motor_kps: Dict[str, float])

Set proportional gains for motor position control.

Parameters:

motor_kps – Dictionary mapping motor names to proportional gain values.

abstractmethod set_motor_target(motor_angles: Dict[str, float] | ndarray[Any, dtype[float32]])

Set target positions for all motors.

Parameters:

motor_angles – Target motor positions, either as a dictionary mapping motor names to angles, or as a numpy array of motor angles.

abstractmethod step()

Advance the simulation by one timestep or execute one control cycle.

This method should update the simulation state or send control commands to the real robot hardware.

class toddlerbot.sim.Obs(time: float, motor_pos: ndarray[Any, dtype[float32]], motor_vel: ndarray[Any, dtype[float32]], motor_acc: ndarray[Any, dtype[float32]] | None = None, motor_cur: ndarray[Any, dtype[float32]] | None = None, motor_tor: ndarray[Any, dtype[float32]] | None = None, pos: ndarray[Any, dtype[float32]] | None = None, rot: Rotation | None = None, lin_vel: ndarray[Any, dtype[float32]] | None = None, ang_vel: ndarray[Any, dtype[float32]] | None = None, joint_pos: ndarray[Any, dtype[float32]] | None = None, joint_vel: ndarray[Any, dtype[float32]] | None = None)

Bases: object

Observation data structure for robot state information.

This dataclass encapsulates all relevant sensor and state information from the robot simulation or real hardware, including motor states, IMU data, and joint positions/velocities.

time

Timestamp of the observation in seconds.

Type:

float

motor_pos

Motor positions in radians as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]]

motor_vel

Motor velocities in rad/s as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]]

motor_acc

Optional motor accelerations in rad/s² as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

motor_cur

Optional motor currents in Amperes as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

motor_tor

Optional motor torques in N⋅m as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

pos

Optional 3D position [x, y, z] in meters as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

rot

Optional orientation as a scipy Rotation object.

Type:

scipy.spatial.transform._rotation.Rotation | None

lin_vel

Optional linear velocity [vx, vy, vz] in m/s as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

ang_vel

Optional angular velocity [wx, wy, wz] in rad/s as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

joint_pos

Optional joint positions in radians as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

joint_vel

Optional joint velocities in rad/s as a float32 array.

Type:

numpy.ndarray[Any, numpy.dtype[numpy.float32]] | None

ang_vel: ndarray[Any, dtype[float32]] | None = None
joint_pos: ndarray[Any, dtype[float32]] | None = None
joint_vel: ndarray[Any, dtype[float32]] | None = None
lin_vel: ndarray[Any, dtype[float32]] | None = None
motor_acc: ndarray[Any, dtype[float32]] | None = None
motor_cur: ndarray[Any, dtype[float32]] | None = None
motor_pos: ndarray[Any, dtype[float32]]
motor_tor: ndarray[Any, dtype[float32]] | None = None
motor_vel: ndarray[Any, dtype[float32]]
pos: ndarray[Any, dtype[float32]] | None = None
rot: Rotation | None = None
time: float