API¤
RING¤
RING ¤
RING(lam, Ts, **kwargs)
Creates the RING network.
Parameters: |
|
---|
Returns: |
|
---|
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: |
|
---|
Raises: |
|
---|
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_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: |
|
---|
Methods:
Name | Description |
---|---|
num_links |
Returns the number of links in the system. |
q_size |
Returns the total number of generalized coordinates ( |
qd_size |
Returns the total number of generalized velocities ( |
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 |
idx_map |
str) -> dict:
Returns a dictionary mapping link names to their indices for a specified type
( |
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 |
coordinate_vector_to_q |
jax.Array, custom_joints: dict[str, Callable] = {}) -> jax.Array:
Converts a coordinate vector to minimal coordinates ( |
Raises: |
|
---|
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.
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"
).
change_model_name ¤
change_model_name(new_name=None, prefix=None, suffix=None)
Changes the name of the system model.
add_prefix_suffix ¤
add_prefix_suffix(prefix=None, suffix=None)
Adds either or, or both a prefix and suffix to all link names.
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_bodies_to_world ¤
findall_bodies_to_world(names=False)
Returns all bodies directly 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: |
|
---|
Returns: |
|
---|
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: |
---|
Returns: |
|
---|
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: |
---|
Returns: |
|
---|
morph_system ¤
morph_system(new_parents=None, new_anchor=None)
Re-orders the graph underlying the system. Returns a new system.
Parameters: |
|
---|
Returns: |
|
---|
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: |
|
---|
Methods:
Name | Description |
---|---|
is_feasible |
Checks if the motion configuration satisfies all constraints. |
to_nomotion_config |
Returns a new |
overwrite_for_joint_type |
Applies specific configuration changes for a given joint type.
Note: These changes affect all instances of |
overwrite_for_subsystem |
Modifies the motion configuration for all joints in a subsystem rooted at |
from_register |
Retrieves a predefined |
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: |
|
---|
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: |
|
---|
Returns: |
|
---|
Raises: |
|
---|
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: |
|
---|
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: |
|
---|
Raises: |
|
---|
Notes
- The function updates global dictionaries that store joint properties, including:
_joint_types
: Maps joint type names toJointModel
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: |
|
---|
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 |
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: |
|
---|
Returns: |
|
---|
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: |
|
---|
Returns: |
|
---|
Raises: |
|
---|
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: |
|
---|
Methods:
Name | Description |
---|---|
create |
Optional[Vector] = None, rot: Optional[Quaternion] = None) -> Transform:
Creates a |
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: |
|
---|
Returns: |
|
---|
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: |
|
---|
Returns: |
|
---|
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: |
|
---|
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