dair_pll.multibody_terms

Mathematical implementation of multibody dynamics terms calculations.

This file implements the MultibodyTerms type, which interprets a list of urdfs as a learnable Lagrangian system with contact, taking the state space from the corresponding MultibodyPlantDiagram as a given, and interpreting the various inertial and geometric terms stored within it as initial conditions of learnable parameters.

Multibody dynamics can be derived from four functions of state [q,v]:

• M(q), the generalized mass-matrix

• F(q), the non-contact/Lagrangian force terms.

• phi(q), the signed distance between collision candidates.

• J(q), the contact-frame velocity Jacobian between collision candidates.

The first two terms depend solely on state and inertial properties, and parameterize the contact-free Lagrangian dynamics as:

dv/dt = (M(q) ** (-1)) * F(q)

These terms are accordingly encapsulated in a LagrangianTerms instance.

The latter two terms depend solely on the geometry of bodies coming into contact, and are encapsulated in a ContactTerms instance.

For both sets of terms, we derive their functional form either directly or in part through symbolic analysis of the MultibodyPlant of the associated MultibodyPlantDiagram. The MultibodyTerms object manages the symbolic calculation and has corresponding LagrangianTerms and ContactTerms members.

dair_pll.multibody_terms.init_symbolic_plant_context_and_state(plant_diagram)

Generates a symbolic interface for a MultibodyPlantDiagram.

Generates a new Drake Expression data type state in StateSpace format, and sets this state inside a new context for a symbolic version of the diagram’s MultibodyPlant.

Parameters:

plant_diagram (MultibodyPlantDiagram) – Drake MultibodyPlant diagram to convert to symbolic.

Return type:

Tuple[MultibodyPlant_𝓣Expression𝓤, Context, ndarray, ndarray]

Returns:

New symbolic plant. New plant’s context, with symbolic states set. (n_q,) symbolic StateSpace configuration. (n_v,) symbolic StateSpace velocity.

class dair_pll.multibody_terms.LagrangianTerms(plant_diagram)

Bases: Module

Container class for non-contact/Lagrangian dynamics terms.

Accepts batched pytorch callback functions for M(q) and F(q) and related contact terms in theta format (see inertia.py).

Inits LagrangianTerms with prescribed parameters and functional forms.

Parameters:

plant_diagram (MultibodyPlantDiagram) – Drake MultibodyPlant diagram to extract terms from.

mass_matrix: typing.Optional[typing.Callable[[Tensor, Tensor], Tensor]]

lagrangian_forces: typing.Optional[typing.Callable[[Tensor, Tensor, Tensor, Tensor], Tensor]]

inertial_parameters: torch.nn.parameter.Parameter

static extract_body_parameters_and_variables(plant, model_ids, context)

Generates parameterization and symbolic variables for all bodies.

For a multibody plant, finds all bodies that should have inertial properties; extracts the current values as an initial condition for theta-format learnable parameters, and sets new symbolic versions of these variables.

Parameters:

• plant (MultibodyPlant_𝓣Expression𝓤) – Symbolic plant from which to extract parameterization.

• model_ids (List[ModelInstanceIndex]) – List of models in plant.

• context (Context) – Plant’s symbolic context.

Return type:

Tuple[Tensor, ndarray]

Returns:

(n_bodies, 10) theta parameters initial conditions. (n_bodies, 10) symbolic inertial variables.

pi_cm()

Returns inertial parameters in human-understandable pi_cm- format

Return type:

Tensor

forward(q, v, u)

Evaluates Lagrangian dynamics terms at given state and input.

Parameters:

• q (Tensor) – (*, n_q) configuration batch.

• v (Tensor) – (*, n_v) velocity batch.

• u (Tensor) – (*, n_u) input batch.

Return type:

Tuple[Tensor, Tensor]

Returns:

(*, n_v, n_v) mass matrix batch M(q) (*, n_v) Lagrangian contact-free acceleration inv(M(q)) F(q)

dair_pll.multibody_terms.make_configuration_callback(expression, q)

Converts drake symbolic expression to pytorch function via drake_pytorch.

Return type:

Callable[[Tensor], Tensor]

class dair_pll.multibody_terms.ContactTerms(plant_diagram)

Bases: Module

Container class for contact-related dynamics terms.

Derives batched pytorch callback functions for collision geometry position and velocity kinematics from a MultibodyPlantDiagram.

Inits ContactTerms with prescribed kinematics and geometries.

phi(q) and J(q) are calculated implicitly from kinematics and n_g == len(geometries) collision geometries C.

Parameters:

plant_diagram (MultibodyPlantDiagram) – Drake MultibodyPlant diagram to extract terms from.

geometry_local_poses: torch.nn.parameter.Parameter

geometry_rotations: typing.Optional[typing.Callable[[Tensor], Tensor]]

geometry_translations: typing.Optional[typing.Callable[[Tensor], Tensor]]

geometry_spatial_jacobians: typing.Optional[typing.Callable[[Tensor], Tensor]]

geometries: torch.nn.modules.container.ModuleList

friction_params: torch.nn.parameter.Parameter

collision_candidates: torch.Tensor

get_friction_coefficients()

From the stored friction_params, compute the friction coefficient as its absolute value.

Return type:

Tensor

static extract_geometries_and_kinematics(plant, inspector, geometry_ids, context)

Extracts modules and kinematics of list of geometries G.

Parameters:

• plant (MultibodyPlant_𝓣Expression𝓤) – Multibody plant from which terms are extracted.

• inspector (SceneGraphInspector) – Scene graph inspector associated with plant.

• geometry_ids (List[GeometryId]) – List of geometries to model.

• context (Context) – Plant’s context with symbolic state.

Return type:

Tuple[List[CollisionGeometry], List[ndarray], List[ndarray], List[ndarray]]

Returns:

List of CollisionGeometry models with one-to-one correspondence with provided geometries. List[(3,3)] of corresponding rotation matrices R_WG List[(3,)] of corresponding geometry frame origins p_WoGo_W List[(6,n_v)] of geometry spatial jacobians w.r.t. drake velocity coordinates, J(v_drake)_V_WG_W

static assemble_velocity_jacobian(R_CW, Jv_V_WC_W, p_CoCc_C)

Helper method to generate velocity jacobian from contact information.

Parameters:

• R_CW – (*, n_c, 3, 3) Rotation of world frame w.r.t. geometry frame.

• Jv_V_WC_W – (*, 1, 6, n_v) Geometry spatial velocity Jacobian.

• p_CoCc_C – (*, n_c, 3) Geometry-frame contact points.

Returns:

(*, n_c, 3, n_v) World-frame contact point translational velocity Jacobian.

static relative_velocity_to_contact_jacobian(Jv_v_W_BcAc_F, mu)

Helper method to reorder contact Jacobian columns.

Parameters:

• Jv_v_W_BcAc_F (Tensor) – (*, n_collisions, 3, n_v) collection of

• Jacobians. (contact-frame relative velocity) –

• mu (Tensor) – (n_collisions,) list of

Return type:

Tensor

Returns:

(*, 3 * n_collisions, n_v) contact jacobian J(q) in [J_n; mu * J_t] ordering.

forward(q)

Evaluates Lagrangian dynamics terms at given state and input.

Uses GeometryCollider and kinematics to construct signed distance phi(q) and the corresponding Jacobian J(q).

phi(q) and J(q) are calculated implicitly from kinematics and collision geometries.

Parameters:

• q (Tensor) – (*, n_q) configuration batch.

• collide. (indices that can) –

Return type:

Tuple[Tensor, Tensor]

Returns:

(*, n_collisions) signed distance phi(q). (*, 3 * n_collisions, n_v) contact Jacobian J(q).

class dair_pll.multibody_terms.MultibodyTerms(urdfs)

Bases: Module

Derives and manages computation of terms of multibody dynamics with contact.

Primarily

Inits MultibodyTerms for system described in URDFs

Interpretation is performed as a thin wrapper around LagrangianTerms and ContactTerms.

As this module is also responsible for evaluating updated URDF representations, the associations between bodies and geometries is also tracked to enable URDF rendering in MultibodyTerms.EvalUrdfRepresentation and Tensorboard logging in MultibodyTerms.scalars.

Parameters:

• urdfs (Dict[str, str]) – Dictionary of named URDF XML file names, containing

• system. (description of multibody) –

scalars_and_meshes()

Generates summary statistics for inertial and geometric quantities.

Return type:

Tuple[Dict[str, float], Dict[str, MeshSummary]]

forward(q, v, u)

Evaluates multibody system dynamics terms at given state and input.

Calculation is performed as a thin wrapper around LagrangianTerms and ContactTerms. For convenience, this function also returns the Delassus operator D(q) = J(q)^T inv(M(q)) J(q).

Parameters:

• q (Tensor) – (*, n_q) configuration batch.

• v (Tensor) – (*, n_v) velocity batch.

• u (Tensor) – (*, n_u) input batch.

Return type:

Tuple[Tensor, Tensor, Tensor, Tensor, Tensor]

Returns:

(*, 3 * n_collisions, 3 * n_collisions) Delassus operator D(q). (*, n_v, n_v) mass matrix batch M(q). (*, 3 * n_collisions, n_v) contact Jacobian J(q). (*, n_collisions) signed distance phi(q). (*, n_v) Contact-free acceleration inv(M(q)) * F(q).

lagrangian_terms: dair_pll.multibody_terms.LagrangianTerms

contact_terms: dair_pll.multibody_terms.ContactTerms

geometry_body_assignment: typing.Dict[str, typing.List[int]]

plant_diagram: dair_pll.drake_utils.MultibodyPlantDiagram

urdfs: typing.Dict[str, str]