MechanismState

struct MechanismState{X, M, C, JointCollection}

A MechanismState stores state information for an entire Mechanism. It contains the joint configuration and velocity vectors $q$ and $v$, and a vector of additional states $s$. In addition, it stores cache variables that depend on $q$ and $v$ and are aimed at preventing double work.

Type parameters:

• X: the scalar type of the $q$, $v$, and $s$ vectors.
• M: the scalar type of the Mechanism
• C: the scalar type of the cache variables (== promote_type(X, M))
• JointCollection: the type of the treejoints and nontreejoints members (a TypeSortedCollection subtype)

copyto!(dest, src)

Copy state information in state dest to vector src (ordered [q; v; s]).

copyto!(dest, src)

Copy state information in vector src (ordered [q; v; s]) to state dest.

copyto!(dest, src)

Copy (minimal representation of) state src to state dest.

rand!(state)

Randomize both the configuration and velocity. Invalidates cache variables.

additional_state(state)

Return the vector of additional states $s$.

bias_acceleration(state, joint)
bias_acceleration(state, joint, safe)

Return the bias acceleration across the given joint, i.e. the spatial acceleration of frame_after(joint) with respect to frame_before(joint), expressed in the root frame of the mechanism when all joint accelerations are zero.

bias_acceleration(state, body)
bias_acceleration(state, body, safe)

Return the bias acceleration of the given body with respect to the world, i.e. the spatial acceleration of default_frame(body) with respect to the root frame of the mechanism, expressed in the root frame, when all joint accelerations are zero.

configuration(state, joint)

Return the part of the configuration vector $q$ associated with joint.

configuration(state)

Return the configuration vector $q$.

Note that this returns a reference to the underlying data in state. The user is responsible for calling setdirty! after modifying this vector to ensure that dependent cache variables are invalidated.

configuration_index_to_joint_id(state, qindex)

Return the JointID of the joint associated with the given index into the configuration vector $q$.

configuration_range(state, joint)

Return the range of indices into the joint configuration vector $q$ corresponding to joint joint.

constraint_range(state, joint)

Return the range of row indices into the constraint Jacobian corresponding to joint joint.

crb_inertia(state, body)
crb_inertia(state, body, safe)

Return the composite rigid body inertia body expressed in the root frame of the mechanism.

global_coordinates!(state, q0, ϕ)

Convert local coordinates $\phi$ centered around $q_0$ to (global) configuration vector $q$.

gravitational_potential_energy(state, body)
gravitational_potential_energy(state, body, safe)

Return the gravitational potential energy in the given state, computed as the negation of the dot product of the gravitational force and the center of mass expressed in the Mechanism's root frame.

joint_transform(state, joint)
joint_transform(state, joint, safe)

Return the joint transform for the given joint, i.e. the transform from frame_after(joint) to frame_before(joint).

local_coordinates!(ϕ, ϕd, state, q0)

Compute local coordinates $\phi$ centered around (global) configuration vector $q_0$, as well as their time derivatives $\dot{\phi}$.

normalize_configuration!(state)

Project the configuration vector $q$ onto the configuration manifold.

For example:

• for a part of $q$ corresponding to a revolute joint, this method is a no-op;
• for a part of $q$ corresponding to a spherical joint that uses a unit quaternion

to parameterize the orientation of its successor with respect to its predecessor, normalize_configuration! will renormalize the quaternion so that it is indeed of unit length.

This method does not ensure that the configuration or velocity satisfy joint configuration or velocity limits/bounds.

num_additional_states(state)

Return the length of the vector of additional states $s$ (currently used for stateful contact models).

num_positions(state)

Return the length of the joint configuration vector $q$.

num_velocities(state)

Return the length of the joint velocity vector $v$.

principal_value!(state)

Applies the principalvalue functions from [Rotations.jl](https://github.com/FugroRoames/Rotations.jl/blob/d080990517f89b56c37962ad53a7fd24bd94b9f7/src/principalvalue.jl) to joint angles. This currently only applies to SPQuatFloating joints.

For example:

• for a part of $q$ corresponding to a revolute joint, this method is a no-op;
• for a part of $q$ corresponding to a SPQuatFloating joint this function applies

`principal_value the orientation.

rand_configuration!(state)

Randomize the configuration vector $q$. The distribution depends on the particular joint types present in the associated Mechanism. The resulting $q$ is guaranteed to be on the Mechanism's configuration manifold. Invalidates cache variables.

rand_velocity!(state)

Randomize the velocity vector $v$. Invalidates cache variables.

relative_transform(state, from, to)

Return the homogeneous transform from from to to.

relative_twist(state, body_frame, base_frame)

Return the twist of body_frame with respect to base_frame, expressed in the Mechanism's root frame.

relative_twist(state, body, base)

Return the twist of body with respect to base, expressed in the Mechanism's root frame.

reset_contact_state!(state)

Reset all contact state variables.

set_additional_state!(state, s)

Set the vector of additional states $s$.

set_configuration!(state, q)

Set the configuration vector $q$. Invalidates cache variables.

set_configuration!(state, joint, config)

Set the part of the configuration vector associated with joint. Invalidates cache variables.

set_velocity!(state, v)

Set the velocity vector $v$. Invalidates cache variables.

set_velocity!(state, joint, vel)

Set the part of the velocity vector associated with joint. Invalidates cache variables.

setdirty!(state)

Invalidate all cache variables.

spatial_inertia(state, body)
spatial_inertia(state, body, safe)

Return the spatial inertia of body expressed in the root frame of the mechanism.

supports(joint, body, state)

Return whether joint supports body, i.e., joint is a tree joint on the path between body and the root.

transform_to_root(state, body)
transform_to_root(state, body, safe)

Return the transform from default_frame(body) to the root frame of the mechanism.

twist(state, joint)
twist(state, joint, safe)

Return the joint twist for the given joint, i.e. the twist of frame_after(joint) with respect to frame_before(joint), expressed in the root frame of the mechanism.

twist_wrt_world(state, body)
twist_wrt_world(state, body, safe)

Return the twist of default_frame(body) with respect to the root frame of the mechanism, expressed in the root frame.

velocity(state, joint)

Return the part of the velocity vector $v$ associated with joint.

velocity(state)

Return the velocity vector $v$.

Note that this function returns a read-write reference to a field in state. The user is responsible for calling setdirty! after modifying this vector to ensure that dependent cache variables are invalidated.

velocity_index_to_joint_id(state, qindex)

Return the JointID of the joint associated with the given index into the velocity vector $v$.

velocity_range(state, joint)

Return the range of indices into the joint velocity vector $v$ corresponding to joint joint.

zero!(state)

Zero both the configuration and velocity. Invalidates cache variables.

See zero_configuration!, zero_velocity!.

zero_configuration!(state)

'Zero' the configuration vector $q$. Invalidates cache variables.

Note that when the Mechanism contains e.g. quaternion-parameterized joints, $q$ may not actually be set to all zeros; the quaternion part of the configuration vector would be set to identity. The contract is that each of the joint transforms should be an identity transform.

zero_velocity!(state)

Zero the velocity vector $v$. Invalidates cache variables.