dair_pll.drake_state_converter

Utility classes for interfacing with Drake’s internal state format.

Classes herein mainly support the implementation of DrakeStateConverter. In order to make Drake states compatible with available StateSpace inheriting classes, users must define the drake system by a collection of URDF files, each of which contains a model for exactly one floating- or fixed-base rigid multibody chain. This allows for the system to be modeled as having a ProductSpace state space, where each factor space is a FloatingBaseSpace or FixedBaseSpace.

For flexible usage, the conversion is between contexts and numpy ndarray types. This is particularly useful as it allows pydrake symbolic types to be used, facilitating differentiable geometric analysis of the relationship between the coordinate systems in multibody_terms.py.

dair_pll.drake_state_converter.state_ndarray_reformat(x)

Resizes Drake coordinates to StateSpace batch.

Return type:

ndarray

dair_pll.drake_state_converter.drake_ndarray_reformat(x)

Resizes StateSpace batch to Drake coordinates.

Return type:

ndarray

class dair_pll.drake_state_converter.DrakeFloatingBaseStateConverter

Bases: object

Converts between the np.ndarray state coordinates of a Drake MultibodyPlant model instance and a floating-base open kinematic chain.

When a Drake model instance is a single floating-base rigid chain, it represents the configuration in tangent bundle of SE(3) x R^n_joints, with coordinates as a quaternion; world-frame floating base c.o.m., joint positions/angles, world-axes floating base angular/linear velocity, and joint velocities.

Conversion between coordinate sets is then simply a frame transformation on the angular velocity between world and floating base frame.

static drake_to_state(q_drake, v_drake)

Formats configuration and velocity into row vectors, and rotates angular velocity into body frame.

Return type:

Tuple[ndarray, ndarray]

static state_to_drake(q, v)

Formats configuration and velocity into squeezed vectors, and rotates angular velocity into world frame.

Return type:

Tuple[ndarray, ndarray]

class dair_pll.drake_state_converter.DrakeFixedBaseStateConverter

Bases: object

Converts between the np.ndarray state coordinates of a Drake MultibodyPlant model instance and a fixed-base open kinematic chain.

When a Drake model instance is a single fixed-base rigid chain, it represents the configuration in tangent bundle of R^n_joints, with the same exact coordinate system that FixedBaseSpace uses. Therefore, conversion between these types is a simple passthrough that copies the coordinates in memory.

static drake_to_state(q_drake, v_drake)

Formats configuration and velocity into row vectors.

Return type:

Tuple[ndarray, ndarray]

static state_to_drake(q, v)

Formats configuration and velocity into squeezed vectors.

Return type:

Tuple[ndarray, ndarray]

class dair_pll.drake_state_converter.DrakeModelStateConverterFactory

Bases: object

Factory class for selecting Drake-to-BaseSpace coordinate conversions.

static state_to_drake(q, v, space)

Selects state_to_drake method based on presence of floating base from DrakeFloatingBaseStateConverter or DrakeFixedBaseStateConverter.

Return type:

Tuple[ndarray, ndarray]

static drake_to_state(q_drake, v_drake, space)

Selects drake_to_state method based on presence of floating base from DrakeFloatingBaseStateConverter or DrakeFixedBaseStateConverter.

Return type:

Tuple[ndarray, ndarray]

class dair_pll.drake_state_converter.DrakeStateConverter

Bases: object

Utility namespace for conversion between Drake state format and ProductSpace formats.

Given a MultibodyPlant and a complete list of its models, DrakeStateConverter converts between a numpy ndarray and Context representation of the state space. This class leverages the one-open-kinematic-chain-per-model assumption to iterate over the models, and convert the factor space coordinates with DrakeModelStateConverterFactory.

static context_to_state(plant, plant_context, model_ids, space)

Retrieves ProductSpace-formatted state from plant’s context.

Parameters:

• plant (MultibodyPlant) – plant from which to retrieve state.

• plant_context (Context) – plant’s context which stores its state.

• model_ids (List[ModelInstanceIndex]) – List of plant’s models

• space (ProductSpace) – state space of output state.

Return type:

ndarray

Returns:

(space.n_x,) current state of plant.

static state_to_context(plant, plant_context, x, model_ids, space)

Transforms and assigns ProductSpace-formatted state in plant’s mutable context.

Parameters:

• plant (MultibodyPlant) – plant in which to store state.

• plant_context (Context) – plant’s context which stores its state.

• x (ndarray) – (1, space.n_x) or (space.n_x,) state.

• model_ids (List[ModelInstanceIndex]) – Mapping from plant’s model names to instances

• space (ProductSpace) – state space of output state.

Return type:

None