API¤

RING¤

RING ¤

RING(lam, Ts, **kwargs)

Creates the RING network.

Parameters:
  • lam (list[int] | None) –

    parent array, if None must be given via ringnet.apply(..., lam=lam)

  • Ts

    sampling interval of IMU data; time delta in seconds

Returns:
  • AbstractFilter

    ring.ml.AbstractFilter: An instantiation of ring.ml.ringnet.RING with trained parameters.

Examples:

>>> import ring
>>> import numpy as np
>>>
>>> T  : int       = 30        # sequence length     [s]
>>> Ts : float     = 0.01      # sampling interval   [s]
>>> B  : int       = 1         # batch size
>>> lam: list[int] = [0, 1, 2] # parent array
>>> N  : int       = len(lam)  # number of bodies
>>> T_i: int       = int(T/Ts) # number of timesteps
>>>
>>> X = np.zeros((B, T_i, N, 9))
>>> # where X is structured as follows:
>>> # X[..., :3]  = acc
>>> # X[..., 3:6] = gyr
>>> # X[..., 6:9] = jointaxis
>>>
>>> # let's assume we have an IMU on each outer segment of the
>>> # three-segment kinematic chain
>>> X[:, :, 0, :3]  = acc_segment1
>>> X[:, :, 2, :3]  = acc_segment3
>>> X[:, :, 0, 3:6] = gyr_segment1
>>> X[:, :, 2, 3:6] = gyr_segment3
>>>
>>> ringnet = ring.RING(lam, Ts)
>>>
>>> yhat, _ = ringnet.apply(X)
>>> # yhat : unit quaternions, shape = (B, T_i, N, 4)
>>> # yhat[b, :, i] is the orientation from body `i` to parent body `lam[i]`
>>>
>>> # use `jax.jit` to compile the forward pass
>>> jit_apply = jax.jit(ringnet.apply)
>>> yhat, _ = jit_apply(X)
>>>
>>> # manually pass in and out the hidden state like so
>>> initial_state = None
>>> yhat, state = ringnet.apply(X, state=initial_state)
>>> # state: final hidden state, shape = (B, N, 2*H)

RING ¤

__init__ ¤
__init__(params=None, lam=None, jit=True, name=None, forward_factory=make_ring, **kwargs)

Untrained RING network

RCMG¤

RCMG ¤

__init__ ¤
__init__(sys, config=jcalc.MotionConfig(), setup_fn=None, finalize_fn=None, add_X_imus=False, add_X_imus_kwargs=dict(), add_X_jointaxes=False, add_X_jointaxes_kwargs=dict(), add_y_relpose=False, add_y_rootincl=False, add_y_rootincl_kwargs=dict(), add_y_rootfull=False, add_y_rootfull_kwargs=dict(), sys_ml=None, randomize_positions=False, randomize_motion_artifacts=False, randomize_joint_params=False, randomize_hz=False, randomize_hz_kwargs=dict(), imu_motion_artifacts=False, imu_motion_artifacts_kwargs=dict(), dynamic_simulation=False, dynamic_simulation_kwargs=dict(), output_transform=None, keep_output_extras=False, use_link_number_in_Xy=False, cor=False, disable_tqdm=False)

Initializes the Random Chain Motion Generator (RCMG).

The RCMG generates synthetic joint motion sequences for kinematic and dynamic systems based on predefined motion configurations. It allows for system randomization, augmentation with IMU and joint axis data, and optional dynamic simulation.

Parameters:
  • sys (System | list[System]) –

    The system(s) for which motion should be generated.

  • config (MotionConfig | list[MotionConfig], default: MotionConfig() ) –

    Motion configuration(s) defining velocity limits, interpolation methods, and range constraints. Defaults to jcalc.MotionConfig().

  • setup_fn (Optional[SETUP_FN], default: None ) –

    A function to modify the system before motion generation. Defaults to None.

  • finalize_fn (Optional[FINALIZE_FN], default: None ) –

    A function to modify outputs after motion generation. Defaults to None.

  • add_X_imus (bool, default: False ) –

    Whether to add IMU sensor data to the output. Defaults to False.

  • add_X_imus_kwargs (dict, default: dict() ) –

    Additional keyword arguments for IMU data processing. Defaults to {}.

  • add_X_jointaxes (bool, default: False ) –

    Whether to add joint axis data to the output. Defaults to False.

  • add_X_jointaxes_kwargs (dict, default: dict() ) –

    Additional keyword arguments for joint axis data processing. Defaults to {}.

  • add_y_relpose (bool, default: False ) –

    Whether to add relative pose targets to the output. Defaults to False.

  • add_y_rootincl (bool, default: False ) –

    Whether to add root inclination targets to the output. Defaults to False.

  • add_y_rootincl_kwargs (dict, default: dict() ) –

    Additional keyword arguments for root inclination processing. Defaults to {}.

  • add_y_rootfull (bool, default: False ) –

    Whether to add full root state targets to the output. Defaults to False.

  • add_y_rootfull_kwargs (dict, default: dict() ) –

    Additional keyword arguments for full root state processing. Defaults to {}.

  • sys_ml (Optional[System], default: None ) –

    System that defines the graph and naming structure of the X and y outputs. Defaults to None which then uses the first provided system.

  • randomize_positions (bool, default: False ) –

    Whether to randomised positions based on pos_min and pos_max. Defaults to False.

  • randomize_motion_artifacts (bool, default: False ) –

    Whether to randomize the IMU motion artifact simulation. This randomises the spring stiffness and spring damping parameters of the passive free joint that is added between nonrigid and rigid IMU. Defaults to False.

  • randomize_joint_params (bool, default: False ) –

    Whether to randomize joint parameters by calling JointModel.init_joint_params before every sequence generation. Defaults to False.

  • randomize_hz (bool, default: False ) –

    Whether to randomize the sampling frequency of the generated data. Defaults to False.

  • randomize_hz_kwargs (dict, default: dict() ) –

    Additional keyword arguments for sampling frequency randomization. Defaults to {}.

  • imu_motion_artifacts (bool, default: False ) –

    Whether to simulate nonrigid IMU motion artifacts. Defaults to False.

  • imu_motion_artifacts_kwargs (dict, default: dict() ) –

    Additional keyword arguments for IMU motion artifact simulation. Defaults to {}.

  • dynamic_simulation (bool, default: False ) –

    Whether to use a physics-based simulation to generate motion instead of purely kinematic methods. Defaults to False.

  • dynamic_simulation_kwargs (dict, default: dict() ) –

    Additional keyword arguments for dynamic simulation. Defaults to {}.

  • output_transform (Optional[Callable], default: None ) –

    A function to transform the generated output data. Defaults to None.

  • keep_output_extras (bool, default: False ) –

    Whether to keep additional output metadata. Defaults to False.

  • use_link_number_in_Xy (bool, default: False ) –

    Whether to replace joint names with numerical indices in the output. Defaults to False.

  • cor (bool, default: False ) –

    Whether to replace free joints with center-of-rotation (COR) 9D free joint. Defaults to False.

  • disable_tqdm (bool, default: False ) –

    Whether to disable progress bars during generation. Defaults to False.

Raises:
  • AssertionError

    If any of the provided MotionConfig instances are infeasible.

Notes
  • This class supports batch generation, lazy and eager data loading, and motion augmentation.
  • If randomize_hz=True, the time step (dt) varies according to the specified sampling rates.
  • When cor=True, free joints are replaced with center-of-rotation models, affecting joint motion behavior.
to_lazy_gen ¤
to_lazy_gen(sizes=1, jit=True)

Returns a generator X, y = gen(key) that lazily generates batched sequences.

to_list ¤
to_list(sizes=1, seed=1)

Returns list of unbatched sequences as numpy arrays.

to_folder ¤
to_folder(path, sizes=1, seed=1, overwrite=True, file_prefix='seq', save_fn=partial(utils.pickle_save, overwrite=True), verbose=True)

Stores unbatched sequences as numpy arrays into folder.

to_eager_gen ¤
to_eager_gen(batchsize=1, sizes=1, seed=1, shuffle=True, transform=None)

Returns a generator X, y = gen(key) that returns precomputed batched sequences.

System¤

System ¤

Represents a robotic system consisting of interconnected links and joints. Create it using System.create(...)

The System class models the kinematic and dynamic properties of a multibody system, providing methods for state representation, transformations, joint configuration management, and rendering. It supports both minimal and maximal coordinate representations and can be parsed from or saved to XML files.

Attributes:
  • link_parents (list[int]) –

    A list specifying the parent index for each link. The root link has a parent index of -1.

  • links (Link) –

    A data structure containing information about all links in the system.

  • link_types (list[str]) –

    A list specifying the joint type for each link (e.g., "free", "hinge", "prismatic").

  • link_damping (Array) –

    Joint damping coefficients for each link.

  • link_armature (Array) –

    Armature inertia values for each joint.

  • link_spring_stiffness (Array) –

    Stiffness values for joint springs.

  • link_spring_zeropoint (Array) –

    Rest position of joint springs.

  • dt (float) –

    Simulation time step size.

  • geoms (list[Geometry]) –

    List of geometries associated with the system.

  • gravity (Array) –

    Gravity vector applied to the system (default: [0, 0, -9.81]).

  • integration_method (str) –

    Integration method for simulation (default: "semi_implicit_euler").

  • mass_mat_iters (int) –

    Number of iterations for mass matrix calculations.

  • link_names (list[str]) –

    Names of the links in the system.

  • model_name (Optional[str]) –

    Name of the system model (if available).

  • omc (list[MaxCoordOMC | None]) –

    List of optional Maximal Coordinate representations.

Methods:

Name Description
num_links

Returns the number of links in the system.

q_size

Returns the total number of generalized coordinates (q) in the system.

qd_size

Returns the total number of generalized velocities (qd) in the system.

name_to_idx

str) -> int: Returns the index of a link given its name.

idx_to_name

int, allow_world: bool = False) -> str: Returns the name of a link given its index. If allow_world is True, returns "world" for index -1.

idx_map

str) -> dict: Returns a dictionary mapping link names to their indices for a specified type ("l", "q", or "d").

parent_name

str) -> str: Returns the name of the parent link for a given link.

change_model_name

Optional[str] = None, prefix: Optional[str] = None, suffix: Optional[str] = None) -> "System": Changes the name of the system model.

change_link_name

str, new_name: str) -> "System": Renames a specific link.

add_prefix_suffix

Optional[str] = None, suffix: Optional[str] = None) -> "System": Adds both a prefix and suffix to all link names.

freeze

str | list[str]) -> "System": Freezes the specified link(s), making them immovable.

unfreeze

str, new_joint_type: str) -> "System": Unfreezes a frozen link and assigns it a new joint type.

change_joint_type

str, new_joint_type: str, **kwargs) -> "System": Changes the joint type of a specified link.

joint_type_simplification

str) -> str: Returns a simplified representation of the given joint type.

joint_type_is_free_or_cor

str) -> bool: Checks if a joint type is either "free" or "cor".

joint_type_is_spherical

str) -> bool: Checks if a joint type is "spherical".

joint_type_is_free_or_cor_or_spherical

str) -> bool: Checks if a joint type is "free", "cor", or "spherical".

findall_imus

bool = True) -> list[str] | list[int]: Finds all IMU sensors in the system.

findall_segments

bool = True) -> list[str] | list[int]: Finds all non-IMU segments in the system.

findall_bodies_to_world

bool = False) -> list[int] | list[str]: Returns all bodies directly connected to the world.

find_body_to_world

bool = False) -> int | str: Returns the root body connected to the world.

findall_bodies_with_jointtype

str, names: bool = False) -> list[int] | list[str]: Returns all bodies with the specified joint type.

children

str, names: bool = False) -> list[int] | list[str]: Returns the direct children of a given body.

findall_bodies_subsystem

str, names: bool = False) -> list[int] | list[str]: Finds all bodies in the subsystem rooted at a given link.

scan

Callable, in_types: str, *args, reverse: bool = False): Iterates over system elements while applying a function.

parse

Parses the system, performing consistency checks and computing spatial inertia tensors.

render

Optional[Transform | list[Transform]] = None, **kwargs) -> list[np.ndarray]: Renders frames of the system using maximal coordinates.

render_prediction

Transform | list[Transform], yhat: dict | jax.Array | np.ndarray, **kwargs): Renders a predicted state transformation.

delete_system

str | list[str], strict: bool = True): Removes a subsystem from the system.

make_sys_noimu

Optional[list[str]] = None): Returns a version of the system without IMU sensors.

inject_system

"System", at_body: Optional[str] = None): Merges another system into this one.

morph_system

Optional[list[int | str]] = None, new_anchor: Optional[int | str] = None): Reorders the system’s link hierarchy.

from_xml

str, seed: int = 1) -> "System": Loads a system from an XML file.

from_str

str, seed: int = 1) -> "System": Loads a system from an XML string.

to_str

bool = True) -> str: Serializes the system to an XML string.

to_xml

str) -> None: Saves the system as an XML file.

create

str, seed: int = 1) -> "System": Creates a System instance from an XML file or string.

coordinate_vector_to_q

jax.Array, custom_joints: dict[str, Callable] = {}) -> jax.Array: Converts a coordinate vector to minimal coordinates (q), applying constraints such as quaternion normalization.

Raises:
  • AssertionError

    If the system structure is invalid (e.g., duplicate link names, incorrect parent-child relationships).

  • InvalidSystemError

    If an operation results in an inconsistent system state.

Notes
  • The system must be parsed before use to ensure consistency.
  • The system supports batch operations using JAX for efficient computations.
  • Joint types include revolute ("rx", "ry", "rz"), prismatic ("px", "py", "pz"), spherical, free, and more.
  • Inertial properties of links are computed automatically from associated geometries.
num_links()

Returns the number of links in the system.

q_size ¤
q_size()

Returns the total number of generalized coordinates (q) in the system.

qd_size ¤
qd_size()

Returns the total number of generalized velocities (qd) in the system.

name_to_idx ¤
name_to_idx(name)

Returns the index of a link given its name.

idx_to_name ¤
idx_to_name(idx, allow_world=False)

Returns the name of a link given its index. If allow_world is True, returns "world" for index -1.

idx_map ¤
idx_map(type)

Returns a dictionary mapping link names to their indices for a specified type ("l", "q", or "d").

parent_name ¤
parent_name(name)

Returns the name of the parent link for a given link.

change_model_name ¤
change_model_name(new_name=None, prefix=None, suffix=None)

Changes the name of the system model.

change_link_name(old_name, new_name)

Renames a specific link.

add_prefix_suffix ¤
add_prefix_suffix(prefix=None, suffix=None)

Adds either or, or both a prefix and suffix to all link names.

freeze ¤
freeze(name)

Freezes the specified link(s), making them immovable (uses frozen joint)

unfreeze ¤
unfreeze(name, new_joint_type)

Unfreezes a frozen link and assigns it a new joint type.

change_joint_type ¤
change_joint_type(name, new_joint_type, new_arma=None, new_damp=None, new_stif=None, new_zero=None, seed=1, warn=True)

Changes the joint type of a specified link. By default damping, stiffness are set to zero.

joint_type_simplification staticmethod ¤
joint_type_simplification(typ)

Returns a simplified name of the given joint type.

joint_type_is_free_or_cor staticmethod ¤
joint_type_is_free_or_cor(typ)

Checks if a joint type is either "free" or "cor".

joint_type_is_spherical staticmethod ¤
joint_type_is_spherical(typ)

Checks if a joint type is "spherical".

joint_type_is_free_or_cor_or_spherical staticmethod ¤
joint_type_is_free_or_cor_or_spherical(typ)

Checks if a joint type is "free", "cor", or "spherical".

findall_imus ¤
findall_imus(names=True)

Finds all IMU sensors in the system.

findall_segments ¤
findall_segments(names=True)

Finds all non-IMU segments in the system.

findall_bodies_to_world ¤
findall_bodies_to_world(names=False)

Returns all bodies directly connected to the world.

find_body_to_world ¤
find_body_to_world(name=False)

Returns the root body connected to the world.

findall_bodies_with_jointtype ¤
findall_bodies_with_jointtype(typ, names=False)

Returns all bodies with the specified joint type.

children ¤
children(name, names=False)

List all direct children of body, does not include body itself

findall_bodies_subsystem ¤
findall_bodies_subsystem(name, names=False)

List all children and children's children; does not include body itself

scan ¤
scan(f, in_types, *args, reverse=False)

Scan f along each link in system whilst carrying along state.

Parameters:
  • f (Callable[..., Y]) –

    f(y: Y, *args) -> y

  • in_types (str) –

    string specifying the type of each input arg: 'l' is an input to be split according to link ranges 'q' is an input to be split according to q ranges 'd' is an input to be split according to qd ranges

  • args

    Arguments passed to f, and split to match the link.

  • reverse (bool, default: False ) –

    If true from leaves to root. Defaults to False.

Returns:
  • ys

    Stacked output y of f.

parse ¤
parse()

Initial setup of system. System object does not work unless it is parsed. Currently it does: - some consistency checks - populate the spatial inertia tensors - check that all names are unique - check that names are strings - check that all pos_min <= pos_max (unless traced) - order geoms in ascending order based on their parent link idx - check that all links have the correct size of - damping - armature - stiffness - zeropoint - check that n_links == len(sys.omc)

render ¤
render(xs=None, camera=None, show_pbar=True, backend='mujoco', render_every_nth=1, **scene_kwargs)

Render frames from system and trajectory of maximal coordinates xs.

Parameters:
  • sys (System) –

    System to render.

  • xs (Transform | list[Transform], default: None ) –

    Single or time-series

  • show_pbar (bool, default: True ) –

    Whether or not to show a progress bar.

Returns:
  • list[ndarray]

    list[np.ndarray]: Stacked rendered frames. Length == len(xs).

render_prediction ¤
render_prediction(xs, yhat, transparent_segment_to_root=True, **kwargs)

xs matches sys. yhat matches sys_noimu. yhat are child-to-parent. Note that the body in yhat that connects to -1, is parent-to-child!

delete_system ¤
delete_system(link_name, strict=True)

Cut subsystem starting at link_name (inclusive) from tree.

make_sys_noimu ¤
make_sys_noimu(imu_link_names=None)

Returns, e.g., imu_attachment = {'imu1': 'seg1', 'imu2': 'seg3'}

inject_system ¤
inject_system(other_system, at_body=None)

Combine two systems into one.

Parameters:
  • sys (System) –

    Large system.

  • sub_sys (System) –

    Small system that will be included into the large system sys.

  • at_body (Optional[str], default: None ) –

    Into which body of the large system small system will be included. Defaults to worldbody.

Returns:
  • base.System: description

morph_system ¤
morph_system(new_parents=None, new_anchor=None)

Re-orders the graph underlying the system. Returns a new system.

Parameters:
  • sys (System) –

    System to be modified.

  • new_parents (list[int], default: None ) –

    Let the i-th entry have value j. Then, after morphing the system the system will be such that the link corresponding to the i-th link in the old system will have as parent the link corresponding to the j-th link in the old system.

Returns:
  • base.System: Modified system.

from_xml staticmethod ¤
from_xml(path, seed=1)

Loads a system from an XML file.

from_str staticmethod ¤
from_str(xml, seed=1)

Loads a system from an XML string.

to_str ¤
to_str(warn=True)

Serializes the system to an XML string.

to_xml ¤
to_xml(path)

Saves the system to an XML file.

create classmethod ¤
create(path_or_str, seed=1)

Creates a System instance from an XML file or string.

coordinate_vector_to_q ¤
coordinate_vector_to_q(q, custom_joints={})

Converts a coordinate vector to minimal coordinates (q), applying constraints such as quaternion normalization.

MotionConfig¤

MotionConfig dataclass ¤

Configuration for joint motion generation in kinematic and dynamic simulations.

This class defines the constraints and parameters for generating random joint motions, including angular and positional velocity limits, interpolation methods, and range restrictions for various joint types.

Attributes:
  • T (float) –

    Total duration of the motion sequence (in seconds).

  • t_min (float) –

    Minimum time interval between two generated joint states.

  • t_max (float | TimeDependentFloat) –

    Maximum time interval between two generated joint states.

  • dang_min (float | TimeDependentFloat) –

    Minimum angular velocity (rad/s).

  • dang_max (float | TimeDependentFloat) –

    Maximum angular velocity (rad/s).

  • dang_min_free_spherical (float | TimeDependentFloat) –

    Minimum angular velocity for free and spherical joints.

  • dang_max_free_spherical (float | TimeDependentFloat) –

    Maximum angular velocity for free and spherical joints.

  • delta_ang_min (float | TimeDependentFloat) –

    Minimum allowed change in joint angle (radians).

  • delta_ang_max (float | TimeDependentFloat) –

    Maximum allowed change in joint angle (radians).

  • delta_ang_min_free_spherical (float | TimeDependentFloat) –

    Minimum allowed change in angle for free/spherical joints.

  • delta_ang_max_free_spherical (float | TimeDependentFloat) –

    Maximum allowed change in angle for free/spherical joints.

  • dpos_min (float | TimeDependentFloat) –

    Minimum translational velocity.

  • dpos_max (float | TimeDependentFloat) –

    Maximum translational velocity.

  • pos_min (float | TimeDependentFloat) –

    Minimum position constraint.

  • pos_max (float | TimeDependentFloat) –

    Maximum position constraint.

  • pos_min_p3d_x (float | TimeDependentFloat) –

    Minimum position in x-direction for P3D joints.

  • pos_max_p3d_x (float | TimeDependentFloat) –

    Maximum position in x-direction for P3D joints.

  • pos_min_p3d_y (float | TimeDependentFloat) –

    Minimum position in y-direction for P3D joints.

  • pos_max_p3d_y (float | TimeDependentFloat) –

    Maximum position in y-direction for P3D joints.

  • pos_min_p3d_z (float | TimeDependentFloat) –

    Minimum position in z-direction for P3D joints.

  • pos_max_p3d_z (float | TimeDependentFloat) –

    Maximum position in z-direction for P3D joints.

  • cdf_bins_min (int) –

    Minimum number of bins for cumulative distribution function (CDF)-based random sampling.

  • cdf_bins_max (Optional[int]) –

    Maximum number of bins for CDF-based sampling.

  • randomized_interpolation_angle (bool) –

    Whether to use randomized interpolation for angular motion.

  • randomized_interpolation_position (bool) –

    Whether to use randomized interpolation for positional motion.

  • interpolation_method (str) –

    Interpolation method to be used (default: "cosine").

  • range_of_motion_hinge (bool) –

    Whether to enforce range-of-motion constraints on hinge joints.

  • range_of_motion_hinge_method (str) –

    Method used for range-of-motion enforcement (e.g., "uniform", "sigmoid").

  • rom_halfsize (float | TimeDependentFloat) –

    Half-size of the range of motion restriction.

  • ang0_min (float) –

    Minimum initial joint angle.

  • ang0_max (float) –

    Maximum initial joint angle.

  • pos0_min (float) –

    Minimum initial joint position.

  • pos0_max (float) –

    Maximum initial joint position.

  • cor_t_min (float) –

    Minimum time step for center-of-rotation (COR) joints.

  • cor_t_max (float | TimeDependentFloat) –

    Maximum time step for COR joints.

  • cor_dpos_min (float | TimeDependentFloat) –

    Minimum velocity for COR translation.

  • cor_dpos_max (float | TimeDependentFloat) –

    Maximum velocity for COR translation.

  • cor_pos_min (float | TimeDependentFloat) –

    Minimum position for COR translation.

  • cor_pos_max (float | TimeDependentFloat) –

    Maximum position for COR translation.

  • cor_pos0_min (float) –

    Initial minimum position for COR translation.

  • cor_pos0_max (float) –

    Initial maximum position for COR translation.

  • joint_type_specific_overwrites (dict[str, dict[str, Any]]) –

    A dictionary mapping joint types to specific motion configuration overrides.

Methods:

Name Description
is_feasible

Checks if the motion configuration satisfies all constraints.

to_nomotion_config

Returns a new MotionConfig where all velocities and angle changes are set to zero.

overwrite_for_joint_type

Applies specific configuration changes for a given joint type. Note: These changes affect all instances of MotionConfig for this joint type.

overwrite_for_subsystem

Modifies the motion configuration for all joints in a subsystem rooted at link_name.

from_register

Retrieves a predefined MotionConfig from the global registry.

overwrite_for_joint_type staticmethod ¤
overwrite_for_joint_type(joint_type, **changes)

Changes values of the MotionConfig used by the draw_fn for only a specific joint.

Note

This applies these changes to all MotionConfigs for this joint type! This takes precedence over Motionconfig.joint_type_specific_overwrites!

overwrite_for_subsystem staticmethod ¤
overwrite_for_subsystem(sys, link_name, **changes)

Modifies motionconfig of all joints in subsystem with root link_name. Note that if the subsystem contains a free joint then the jointtype will will be re-named to free_<link_name>, then the RCMG flag cor will potentially not work as expected because it searches for all joints of type free to replace with cor. The workaround here is to change the type already from free to cor in the xml file. This takes precedence *over*Motionconfig.joint_type_specific_overwrites`!

Parameters:
  • sys (System) –

    System object that gets updated

  • link_name (str) –

    Root node of subsystem

  • changes

    Changes to apply to the motionconfig

Return

base.System: Updated system with new jointtypes


join_motionconfigs ¤

join_motionconfigs(configs, boundaries)

Joins multiple MotionConfig objects in time, transitioning between them at specified boundaries.

This function takes a list of MotionConfig instances and a corresponding list of boundary times, and constructs a new MotionConfig that varies in time according to the provided segments.

Parameters:
  • configs (list[MotionConfig]) –

    A list of MotionConfig objects to be joined.

  • boundaries (list[float]) –

    A list of time values where transitions between configs occur. Must have one element less than configs, as each boundary defines the transition point between two consecutive configurations.

Returns:
  • MotionConfig( MotionConfig ) –

    A new MotionConfig object where time-dependent fields transition based on the

  • MotionConfig

    specified boundaries.

Raises:
  • AssertionError

    If the number of boundaries does not match len(configs) - 1.

  • AssertionError

    If time-independent fields have differing values across configs.

Notes
  • Only fields that are time-dependent (float | TimeDependentFloat) will change over time.
  • Time-independent fields must be the same in all configs, or an error is raised.

Extending the RCMG¤

JointModel dataclass ¤

Represents the kinematic and dynamic properties of a joint type.

A JointModel defines the mathematical functions required to compute joint transformations, motion, control terms, and inverse kinematics. It is used to describe the behavior of various joint types, including revolute, prismatic, spherical, and free joints.

Attributes:
  • transform (Callable[[Array, Array], Transform]) –

    Computes the transformation (position and orientation) of the joint given the joint state q and joint parameters.

  • motion (list[Motion | Callable[[Array], Motion]]) –

    Defines the joint motion model. It can be a list of Motion objects or callables that return Motion based on joint parameters.

  • rcmg_draw_fn (Optional[DRAW_FN]) –

    Function used to generate a reference motion trajectory for the joint using Randomized Control Motion Generation (RCMG).

  • p_control_term (Optional[P_CONTROL_TERM]) –

    Function that computes the proportional control term for the joint.

  • qd_from_q (Optional[QD_FROM_Q]) –

    Function to compute joint velocity (qd) from joint positions (q).

  • coordinate_vector_to_q (Optional[COORDINATE_VECTOR_TO_Q]) –

    Function that maps a coordinate vector to a valid joint state q, ensuring constraints (e.g., wrapping angles or normalizing quaternions).

  • inv_kin (Optional[INV_KIN]) –

    Function that computes the inverse kinematics for the joint, mapping a desired transform to joint coordinates q.

  • init_joint_params (Optional[INIT_JOINT_PARAMS]) –

    Function that initializes joint-specific parameters.

  • utilities (Optional[dict[str, Any]]) –

    Additional utility functions or metadata related to the joint model.

Notes
  • The transform function is essential for computing the joint's spatial transformation based on its generalized coordinates.
  • The motion attribute describes how forces and torques affect the joint.
  • The rcmg_draw_fn is used for RCMG motion generation.
  • The coordinate_vector_to_q is critical for maintaining valid joint states.

register_new_joint_type ¤

register_new_joint_type(joint_type, joint_model, q_width, qd_width=None, overwrite=False)

Registers a new joint type with its corresponding JointModel and kinematic properties.

This function allows the addition of custom joint types to the system by associating them with a JointModel, specifying their state and velocity dimensions, and optionally overwriting existing joint definitions.

Parameters:
  • joint_type (str) –

    Name of the new joint type to register.

  • joint_model (JointModel) –

    The JointModel instance defining the kinematic and dynamic properties of the joint.

  • q_width (int) –

    Number of generalized coordinates (degrees of freedom) required to represent the joint.

  • qd_width (Optional[int], default=None, default: None ) –

    Number of velocity coordinates associated with the joint. Defaults to q_width.

  • overwrite (bool, default=False, default: False ) –

    If True, allows overwriting an existing joint type. Otherwise, raises an error if the joint type already exists.

Raises:
  • AssertionError
    • If joint_type is "default" (reserved name).
    • If joint_type already exists and overwrite=False.
    • If qd_width is not provided and does not default to q_width.
    • If joint_model.motion length does not match qd_width.
Notes
  • The function updates global dictionaries that store joint properties, including:
  • _joint_types: Maps joint type names to JointModel instances.
  • base.Q_WIDTHS: Stores the number of state coordinates for each joint type.
  • base.QD_WIDTHS: Stores the number of velocity coordinates for each joint type.
  • If overwrite=True, existing entries are removed before adding the new joint type.
  • Ensures consistency between motion definitions and velocity coordinate dimensions.
Example
new_joint = JointModel(
    transform=my_transform_fn,
    motion=[base.Motion.create(ang=jnp.array([1, 0, 0]))],
)
register_new_joint_type("custom_hinge", new_joint, q_width=1)

Simulation¤

State ¤

Represents the state of a dynamic system in minimal and maximal coordinates.

The State class encapsulates both the configuration (q) and velocity (qd) of the system in minimal coordinates, as well as the corresponding transforms (x) in maximal coordinates.

Attributes:
  • q (Array) –

    The joint positions (generalized coordinates) of the system. The size of q matches sys.q_size().

  • qd (Array) –

    The joint velocities (generalized velocities) of the system. The size of qd matches sys.qd_size().

  • x (Transform) –

    The maximal coordinate representation of all system links, expressed as a Transform object.

Methods:

Name Description
create

System, q: Optional[jax.Array] = None, qd: Optional[jax.Array] = None, x: Optional[Transform] = None, key: Optional[jax.Array] = None, custom_joints: dict[str, Callable] = {}) -> State: Creates a State instance for a given system with optional initial conditions.

Usage

sys = System.create("model.xml") state = State.create(sys) print(state.q.shape) # Should match sys.q_size() print(state.qd.shape) # Should match sys.qd_size()

create classmethod ¤
create(sys, q=None, qd=None, x=None, key=None, custom_joints={})

Creates a State instance for the given system with optional initial conditions.

If no initial values are provided, joint positions (q) and velocities (qd) are initialized to zero, except for free and spherical joints, which have unit quaternions.

Parameters:
  • sys (System) –

    The system for which to create a state.

  • q (Optional[jax.Array], default=None, default: None ) –

    Initial joint positions. If None, defaults to zeros, with unit quaternion initialization for free and spherical joints.

  • qd (Optional[jax.Array], default=None, default: None ) –

    Initial joint velocities. If None, defaults to zeros.

  • x (Optional[Transform], default=None, default: None ) –

    Initial maximal coordinates of the system links. If None, defaults to zero transforms.

  • key (Optional[jax.Array], default=None, default: None ) –

    Random key for initializing q if no values are provided.

  • custom_joints (dict[str, Callable], default={}, default: {} ) –

    Custom joint functions for mapping coordinate vectors to minimal coordinates.

Returns:
  • State( State ) –

    A new instance of the State class representing the initialized system state.

Example

sys = System.create("model.xml") state = State.create(sys) print(state.q.shape) # Should match sys.q_size() print(state.qd.shape) # Should match sys.qd_size()


step ¤

step(sys, state, taus=None, n_substeps=1)

Advances the system dynamics by a single timestep using semi-implicit Euler integration.

This function updates the system's state by integrating the equations of motion over a timestep, potentially with multiple substeps for improved numerical stability. The method ensures that the system's kinematics are updated before each integration step.

Parameters:
  • sys (System) –

    The system to simulate, containing link information, joint dynamics, and integration parameters.

  • state (State) –

    The current state of the system, including joint positions (q), velocities (qd), and transforms (x).

  • taus (Optional[Array], default: None ) –

    The control torques applied to the system joints. If None, zero torques are applied. Defaults to None.

  • n_substeps (int, default: 1 ) –

    The number of integration substeps per timestep to improve numerical accuracy. Defaults to 1.

Returns:
  • State

    base.State: The updated state of the system after integration.

Raises:
  • AssertionError

    If the system's degrees of freedom (q and qd) do not match expectations.

  • AssertionError

    If an unsupported integration method is specified in sys.integration_method.


Transform ¤

Represents a spatial transformation between two coordinate frames using Plücker coordinates.

The Transform class defines the relative position and orientation of one frame (B) with respect to another frame (A). The position (pos) is given in the coordinate frame of A, and the rotation (rot) is expressed as a unit quaternion representing the relative rotation from frame A to frame B.

Attributes:
  • pos (Vector) –

    The translation vector (position of B relative to A) in the coordinate frame of A. Shape: (..., 3), where ... represents optional batch dimensions.

  • rot (Quaternion) –

    The unit quaternion representing the orientation of B relative to A. Shape: (..., 4), where ... represents optional batch dimensions.

Methods:

Name Description
create

Optional[Vector] = None, rot: Optional[Quaternion] = None) -> Transform: Creates a Transform instance with optional position and rotation.

zero

Sequence[int] = ()) -> Transform: Returns a zero transform with a given batch shape.

as_matrix

Returns the 4x4 homogeneous transformation matrix representation of this transform.

Usage

pos = jnp.array([1.0, 2.0, 3.0]) rot = jnp.array([1.0, 0.0, 0.0, 0.0]) # Identity quaternion T = Transform.create(pos, rot) print(T.pos) # Output: [1. 2. 3.] print(T.rot) # Output: [1. 0. 0. 0.] print(T.as_matrix()) # 4x4 transformation matrix

create classmethod ¤
create(pos=None, rot=None)

Creates a Transform instance with the specified position and rotation.

At least one of pos or rot must be provided. If only pos is given, the rotation defaults to the identity quaternion [1, 0, 0, 0]. If only rot is given, the position defaults to [0, 0, 0].

Parameters:
  • pos (Optional[Vector], default=None, default: None ) –

    The position of frame B relative to frame A, expressed in frame A coordinates. If None, defaults to a zero vector of shape (3,).

  • rot (Optional[Quaternion], default=None, default: None ) –

    The unit quaternion representing the orientation of B relative to A. If None, defaults to the identity quaternion (1, 0, 0, 0).

Returns:
  • Transform

    A new Transform instance with the specified position and rotation.

Example

pos = jnp.array([1.0, 2.0, 3.0]) rot = jnp.array([1.0, 0.0, 0.0, 0.0]) # Identity quaternion T = Transform.create(pos, rot) print(T.pos) # Output: [1. 2. 3.] print(T.rot) # Output: [1. 0. 0. 0.]

zero classmethod ¤
zero(shape=())

Returns a zero transform with a given batch shape.

This creates a transform with position (0, 0, 0) and an identity quaternion (1, 0, 0, 0), which represents no translation or rotation.

Parameters:
  • shape (Sequence[int], default=, default: () ) –

    The batch shape for the transform. Defaults to a scalar transform.

Returns:
  • Transform( Transform ) –

    A zero transform with the specified batch shape.

Example

T = Transform.zero() print(T.pos) # Output: [0. 0. 0.] print(T.rot) # Output: [1. 0. 0. 0.]

as_matrix ¤
as_matrix()

Returns the 4x4 homogeneous transformation matrix representation of this transform.

The homogeneous transformation matrix is defined as:

[ R  t ]
[ 0  1 ]

where R is the 3x3 rotation matrix converted from the quaternion and t is the 3x1 position vector.

Returns:
  • Array

    jax.Array: A (4, 4) homogeneous transformation matrix.

Example

T = Transform.create(jnp.array([1.0, 2.0, 3.0]), jnp.array([1.0, 0.0, 0.0, 0.0])) print(T.as_matrix()) # Output: 4x4 matrix