Humanoid

Humanoid kinematics and dynamics

Humanoid

Bases: Robot

Humanoid kinematics and dynamics

Parameters:

Name	Type	Description	Default
urdf_filename	str	Path to the URDF file to load	required
left_hand_parent_joint_name	str	Name of the left hand EE's parent joint	required
right_hand_parent_joint_name	str	Name of the right hand EE's parent joint	required
left_foot_parent_joint_name	str	Name of the left foot EE's parent joint	required
right_foot_parent_joint_name	str	Name of the right foot EE's parent joint	required
left_hand_ee_offset	Optional[ArrayLike]	Transformation matrix specifying the left hand EE offset from the parent joint frame. Defaults to None.	None
right_hand_ee_offset	Optional[ArrayLike]	Transformation matrix specifying the right hand EE offset from the parent joint frame. Defaults to None.	None
left_foot_ee_offset	Optional[ArrayLike]	Transformation matrix specifying the left foot EE offset from the parent joint frame. Defaults to None.	None
right_foot_ee_offset	Optional[ArrayLike]	Transformation matrix specifying the right foot EE offset from the parent joint frame. Defaults to None.	None
collision_data	Optional[dict]	Collision information. Contains info on body/root collision data, as well as self-collision pairs. See collision_utils for more detail. Defaults to None.	None
joint_ordering	Optional[list[str]]	A specific joint ordering to use. Defaults to None (infer ordering from URDF)	None
floating_base	Optional[str]	How to model the free-floating base: None (fixed base), "quaternion", or "euler". See Robot for details. Defaults to "quaternion".	'quaternion'
default_configuration	Optional[ArrayLike]	The default configuration, shape (nq,). See Robot for details. Defaults to None (identity base pose, all joints at zero)	None

Source code in frax/core/humanoid.py

@jax.tree_util.register_static
class Humanoid(Robot):
    """Humanoid kinematics and dynamics

    Args:
        urdf_filename (str): Path to the URDF file to load
        left_hand_parent_joint_name (str): Name of the left hand EE's parent joint
        right_hand_parent_joint_name (str): Name of the right hand EE's parent joint
        left_foot_parent_joint_name (str): Name of the left foot EE's parent joint
        right_foot_parent_joint_name (str): Name of the right foot EE's parent joint
        left_hand_ee_offset (Optional[ArrayLike]): Transformation matrix specifying the left hand EE
            offset from the parent joint frame. Defaults to None.
        right_hand_ee_offset (Optional[ArrayLike]): Transformation matrix specifying the right hand EE
            offset from the parent joint frame. Defaults to None.
        left_foot_ee_offset (Optional[ArrayLike]): Transformation matrix specifying the left foot EE
            offset from the parent joint frame. Defaults to None.
        right_foot_ee_offset (Optional[ArrayLike]): Transformation matrix specifying the right foot EE
            offset from the parent joint frame. Defaults to None.
        collision_data (Optional[dict]): Collision information. Contains info
            on body/root collision data, as well as self-collision pairs.
            See collision_utils for more detail. Defaults to None.
        joint_ordering (Optional[list[str]]): A specific joint ordering to use.
            Defaults to None (infer ordering from URDF)
        floating_base (Optional[str]): How to model the free-floating base: None (fixed base),
            "quaternion", or "euler". See Robot for details. Defaults to "quaternion".
        default_configuration (Optional[ArrayLike]): The default configuration, shape (nq,).
            See Robot for details. Defaults to None (identity base pose, all joints at zero)
    """

    def __init__(
        self,
        urdf_filename,
        left_hand_parent_joint_name: str,
        right_hand_parent_joint_name: str,
        left_foot_parent_joint_name: str,
        right_foot_parent_joint_name: str,
        left_hand_ee_offset: Optional[ArrayLike] = None,
        right_hand_ee_offset: Optional[ArrayLike] = None,
        left_foot_ee_offset: Optional[ArrayLike] = None,
        right_foot_ee_offset: Optional[ArrayLike] = None,
        collision_data: Optional[dict] = None,
        joint_ordering: Optional[list[str]] = None,
        floating_base: Optional[str] = "quaternion",
        default_configuration: Optional[ArrayLike] = None,
    ):
        super().__init__(
            urdf_filename,
            collision_data,
            joint_ordering,
            floating_base,
            default_configuration,
        )

        self.left_hand_parent_chain = np.flatnonzero(
            self.ancestor_mask[self.joint_name_to_index[left_hand_parent_joint_name]]
        )
        self.right_hand_parent_chain = np.flatnonzero(
            self.ancestor_mask[self.joint_name_to_index[right_hand_parent_joint_name]]
        )
        self.left_foot_parent_chain = np.flatnonzero(
            self.ancestor_mask[self.joint_name_to_index[left_foot_parent_joint_name]]
        )
        self.right_foot_parent_chain = np.flatnonzero(
            self.ancestor_mask[self.joint_name_to_index[right_foot_parent_joint_name]]
        )

        if left_hand_ee_offset is None:
            self.left_hand_ee_offset = np.eye(4)
        else:
            left_hand_ee_offset = np.asarray(left_hand_ee_offset, dtype=float)
            assert left_hand_ee_offset.shape == (4, 4)
            self.left_hand_ee_offset = left_hand_ee_offset

        if right_hand_ee_offset is None:
            self.right_hand_ee_offset = np.eye(4)
        else:
            right_hand_ee_offset = np.asarray(right_hand_ee_offset, dtype=float)
            assert right_hand_ee_offset.shape == (4, 4)
            self.right_hand_ee_offset = right_hand_ee_offset

        if left_foot_ee_offset is None:
            self.left_foot_ee_offset = np.eye(4)
        else:
            left_foot_ee_offset = np.asarray(left_foot_ee_offset, dtype=float)
            assert left_foot_ee_offset.shape == (4, 4)
            self.left_foot_ee_offset = left_foot_ee_offset

        if right_foot_ee_offset is None:
            self.right_foot_ee_offset = np.eye(4)
        else:
            right_foot_ee_offset = np.asarray(right_foot_ee_offset, dtype=float)
            assert right_foot_ee_offset.shape == (4, 4)
            self.right_foot_ee_offset = right_foot_ee_offset

    # HANDS AND FEET TRANSFORMS

    def left_hand_transform(self, q: Array) -> Array:
        """Transformation matrix of the left hand (w.r.t world), shape (4, 4)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_hand_transform(transforms)

    def _left_hand_transform(self, joint_transforms: Array) -> Array:
        return self._frame_transform(
            joint_transforms, self.left_hand_ee_offset, self.left_hand_parent_chain[-1]
        )

    def right_hand_transform(self, q: Array) -> Array:
        """Transformation matrix of the right hand (w.r.t world), shape (4, 4)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_hand_transform(transforms)

    def _right_hand_transform(self, joint_transforms: Array) -> Array:
        return self._frame_transform(
            joint_transforms,
            self.right_hand_ee_offset,
            self.right_hand_parent_chain[-1],
        )

    def left_foot_transform(self, q: Array) -> Array:
        """Transformation matrix of the left foot (w.r.t world), shape (4, 4)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_foot_transform(transforms)

    def _left_foot_transform(self, joint_transforms: Array) -> Array:
        return self._frame_transform(
            joint_transforms, self.left_foot_ee_offset, self.left_foot_parent_chain[-1]
        )

    def right_foot_transform(self, q: Array) -> Array:
        """Transformation matrix of the right foot (w.r.t world), shape (4, 4)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_foot_transform(transforms)

    def _right_foot_transform(self, joint_transforms: Array) -> Array:
        return self._frame_transform(
            joint_transforms,
            self.right_foot_ee_offset,
            self.right_foot_parent_chain[-1],
        )

    # HANDS AND FEET JACOBIANS

    def left_hand_jacobian(self, q: Array) -> Array:
        """Left hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_hand_jacobian(transforms)

    def _left_hand_jacobian(self, joint_transforms: Array):
        return self._frame_jacobian(
            joint_transforms, self.left_hand_ee_offset, self.left_hand_parent_chain
        )

    def left_hand_jacobian_and_derivative(
        self, q: Array, v: Array
    ) -> Tuple[Array, Array]:
        """Left hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_hand_jacobian_and_derivative(v, transforms)

    def _left_hand_jacobian_and_derivative(
        self, v: Array, joint_transforms: Array
    ) -> Tuple[Array, Array]:
        return self._frame_jacobian_and_derivative(
            v, joint_transforms, self.left_hand_ee_offset, self.left_hand_parent_chain
        )

    def right_hand_jacobian(self, q: Array) -> Array:
        """Right hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_hand_jacobian(transforms)

    def _right_hand_jacobian(self, joint_transforms: Array):
        return self._frame_jacobian(
            joint_transforms, self.right_hand_ee_offset, self.right_hand_parent_chain
        )

    def right_hand_jacobian_and_derivative(
        self, q: Array, v: Array
    ) -> Tuple[Array, Array]:
        """Right hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_hand_jacobian_and_derivative(v, transforms)

    def _right_hand_jacobian_and_derivative(
        self, v: Array, joint_transforms: Array
    ) -> Tuple[Array, Array]:
        return self._frame_jacobian_and_derivative(
            v,
            joint_transforms,
            self.right_hand_ee_offset,
            self.right_hand_parent_chain,
        )

    def left_foot_jacobian(self, q: Array) -> Array:
        """Left foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_foot_jacobian(transforms)

    def _left_foot_jacobian(self, joint_transforms: Array):
        return self._frame_jacobian(
            joint_transforms, self.left_foot_ee_offset, self.left_foot_parent_chain
        )

    def left_foot_jacobian_and_derivative(
        self, q: Array, v: Array
    ) -> Tuple[Array, Array]:
        """Left foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._left_foot_jacobian_and_derivative(v, transforms)

    def _left_foot_jacobian_and_derivative(
        self, v: Array, joint_transforms: Array
    ) -> Tuple[Array, Array]:
        return self._frame_jacobian_and_derivative(
            v, joint_transforms, self.left_foot_ee_offset, self.left_foot_parent_chain
        )

    def right_foot_jacobian(self, q: Array) -> Array:
        """Right foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_foot_jacobian(transforms)

    def _right_foot_jacobian(self, joint_transforms: Array):
        return self._frame_jacobian(
            joint_transforms, self.right_foot_ee_offset, self.right_foot_parent_chain
        )

    def right_foot_jacobian_and_derivative(
        self, q: Array, v: Array
    ) -> Tuple[Array, Array]:
        """Right foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
        transforms = self.joint_to_world_transforms(q)
        return self._right_foot_jacobian_and_derivative(v, transforms)

    def _right_foot_jacobian_and_derivative(
        self, v: Array, joint_transforms: Array
    ) -> Tuple[Array, Array]:
        return self._frame_jacobian_and_derivative(
            v,
            joint_transforms,
            self.right_foot_ee_offset,
            self.right_foot_parent_chain,
        )

    # MANIPULABILITY INDICES
    # NOTE: Defining hand manipulability based on the actuated joints from pelvis -> hand
    # TODO: Decide if this should only be restricted to the arm joints

    def left_hand_manipulability_index(self, q: Array) -> float:
        joint_transforms = self.joint_to_world_transforms(q)
        return self._left_hand_manipulability_index(joint_transforms)

    def _left_hand_manipulability_index(self, joint_transforms: Array) -> float:
        J_full = self._left_hand_jacobian(joint_transforms)
        chain_idxs = self.left_hand_parent_chain[self.nv_floating :]
        return self._manipulability_index_helper(J_full, chain_idxs)

    def right_hand_manipulability_index(self, q: Array) -> float:
        joint_transforms = self.joint_to_world_transforms(q)
        return self._right_hand_manipulability_index(joint_transforms)

    def _right_hand_manipulability_index(self, joint_transforms: Array) -> float:
        J_full = self._right_hand_jacobian(joint_transforms)
        chain_idxs = self.right_hand_parent_chain[self.nv_floating :]
        return self._manipulability_index_helper(J_full, chain_idxs)

    def left_foot_manipulability_index(self, q: Array) -> float:
        joint_transforms = self.joint_to_world_transforms(q)
        return self._left_foot_manipulability_index(joint_transforms)

    def _left_foot_manipulability_index(self, joint_transforms: Array) -> float:
        J_full = self._left_foot_jacobian(joint_transforms)
        chain_idxs = self.left_foot_parent_chain[self.nv_floating :]
        return self._manipulability_index_helper(J_full, chain_idxs)

    def right_foot_manipulability_index(self, q: Array) -> float:
        joint_transforms = self.joint_to_world_transforms(q)
        return self._right_foot_manipulability_index(joint_transforms)

    def _right_foot_manipulability_index(self, joint_transforms: Array) -> float:
        J_full = self._right_foot_jacobian(joint_transforms)
        chain_idxs = self.right_foot_parent_chain[self.nv_floating :]
        return self._manipulability_index_helper(J_full, chain_idxs)

num_joints property

Deprecated: use nv (velocity dimension) or nq (configuration dimension)

left_hand_transform(q)

Transformation matrix of the left hand (w.r.t world), shape (4, 4)

Source code in frax/core/humanoid.py

def left_hand_transform(self, q: Array) -> Array:
    """Transformation matrix of the left hand (w.r.t world), shape (4, 4)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_hand_transform(transforms)

right_hand_transform(q)

Transformation matrix of the right hand (w.r.t world), shape (4, 4)

Source code in frax/core/humanoid.py

def right_hand_transform(self, q: Array) -> Array:
    """Transformation matrix of the right hand (w.r.t world), shape (4, 4)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_hand_transform(transforms)

left_foot_transform(q)

Transformation matrix of the left foot (w.r.t world), shape (4, 4)

Source code in frax/core/humanoid.py

def left_foot_transform(self, q: Array) -> Array:
    """Transformation matrix of the left foot (w.r.t world), shape (4, 4)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_foot_transform(transforms)

right_foot_transform(q)

Transformation matrix of the right foot (w.r.t world), shape (4, 4)

Source code in frax/core/humanoid.py

def right_foot_transform(self, q: Array) -> Array:
    """Transformation matrix of the right foot (w.r.t world), shape (4, 4)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_foot_transform(transforms)

left_hand_jacobian(q)

Left hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)

Source code in frax/core/humanoid.py

def left_hand_jacobian(self, q: Array) -> Array:
    """Left hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_hand_jacobian(transforms)

left_hand_jacobian_and_derivative(q, v)

Left hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)

Source code in frax/core/humanoid.py

def left_hand_jacobian_and_derivative(
    self, q: Array, v: Array
) -> Tuple[Array, Array]:
    """Left hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_hand_jacobian_and_derivative(v, transforms)

right_hand_jacobian(q)

Right hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)

Source code in frax/core/humanoid.py

def right_hand_jacobian(self, q: Array) -> Array:
    """Right hand Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_hand_jacobian(transforms)

right_hand_jacobian_and_derivative(q, v)

Right hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)

Source code in frax/core/humanoid.py

def right_hand_jacobian_and_derivative(
    self, q: Array, v: Array
) -> Tuple[Array, Array]:
    """Right hand Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_hand_jacobian_and_derivative(v, transforms)

left_foot_jacobian(q)

Left foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)

Source code in frax/core/humanoid.py

def left_foot_jacobian(self, q: Array) -> Array:
    """Left foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_foot_jacobian(transforms)

left_foot_jacobian_and_derivative(q, v)

Left foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)

Source code in frax/core/humanoid.py

def left_foot_jacobian_and_derivative(
    self, q: Array, v: Array
) -> Tuple[Array, Array]:
    """Left foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._left_foot_jacobian_and_derivative(v, transforms)

right_foot_jacobian(q)

Right foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)

Source code in frax/core/humanoid.py

def right_foot_jacobian(self, q: Array) -> Array:
    """Right foot Jacobian (w.r.t world), [Jv; Jw], shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_foot_jacobian(transforms)

right_foot_jacobian_and_derivative(q, v)

Right foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)

Source code in frax/core/humanoid.py

def right_foot_jacobian_and_derivative(
    self, q: Array, v: Array
) -> Tuple[Array, Array]:
    """Right foot Jacobian and its time derivative (w.r.t world), both of shape (6, nv)"""
    transforms = self.joint_to_world_transforms(q)
    return self._right_foot_jacobian_and_derivative(v, transforms)

integrate(q, v, dt)

Integrates a configuration forward in time with a constant velocity

Parameters:

Name	Type	Description	Default
q	Array	Configuration, shape (nq,)	required
v	Array	Velocity, shape (nv,)	required
dt	float	Timestep	required

Returns:

Name	Type	Description
Array	Array	New configuration, shape (nq,)

Source code in frax/core/robot.py

def integrate(self, q: Array, v: Array, dt: float) -> Array:
    """Integrates a configuration forward in time with a constant velocity

    Args:
        q (Array): Configuration, shape (nq,)
        v (Array): Velocity, shape (nv,)
        dt (float): Timestep

    Returns:
        Array: New configuration, shape (nq,)
    """
    if not self.is_quaternion_base:
        return q + v * dt
    pos = q[:3] + v[:3] * dt
    quat = quat_wxyz_multiply(q[3:7], quat_wxyz_exp(v[3:6] * dt))
    quat = quat / jnp.linalg.norm(quat)
    q_act = q[7:] + v[6:] * dt
    return jnp.concatenate([pos, quat, q_act])

difference(q0, q1)

Computes the velocity which takes q0 to q1 in unit time (inverse of integrate)

Parameters:

Name	Type	Description	Default
q0	Array	Starting configuration, shape (nq,)	required
q1	Array	Ending configuration, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Velocity, shape (nv,)

Source code in frax/core/robot.py

def difference(self, q0: Array, q1: Array) -> Array:
    """Computes the velocity which takes q0 to q1 in unit time
    (inverse of integrate)

    Args:
        q0 (Array): Starting configuration, shape (nq,)
        q1 (Array): Ending configuration, shape (nq,)

    Returns:
        Array: Velocity, shape (nv,)
    """
    if not self.is_quaternion_base:
        return q1 - q0
    quat_rel = quat_wxyz_multiply(quat_wxyz_conjugate(q0[3:7]), q1[3:7])
    return jnp.concatenate(
        [q1[:3] - q0[:3], quat_wxyz_log(quat_rel), q1[7:] - q0[7:]]
    )

velocity_to_qdot_map(q)

Matrix E(q) mapping velocities to the time derivative of the configuration: q_dot = E(q) @ v

This is useful when combining autodiff w.r.t. q with velocities, e.g. dh/dt = (dh/dq) @ E(q) @ v. When nq == nv, this is the identity.

Parameters:

Name	Type	Description	Default
q	Array	Configuration, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Velocity map, shape (nq, nv)

Source code in frax/core/robot.py

def velocity_to_qdot_map(self, q: Array) -> Array:
    """Matrix E(q) mapping velocities to the time derivative of the configuration:
    q_dot = E(q) @ v

    This is useful when combining autodiff w.r.t. q with velocities, e.g.
    dh/dt = (dh/dq) @ E(q) @ v. When nq == nv, this is the identity.

    Args:
        q (Array): Configuration, shape (nq,)

    Returns:
        Array: Velocity map, shape (nq, nv)
    """
    if not self.is_quaternion_base:
        return jnp.eye(self.nv)
    w, x, y, z = q[3:7]
    # q_dot = 0.5 * quat * [0, omega_body] (quaternion multiplication)
    quat_map = 0.5 * jnp.array([[-x, -y, -z], [w, -z, y], [z, w, -x], [-y, x, w]])
    E = jnp.zeros((self.nq, self.nv))
    E = E.at[:3, :3].set(jnp.eye(3))
    E = E.at[3:7, 3:6].set(quat_map)
    E = E.at[7:, 6:].set(jnp.eye(self.num_actuated_joints))
    return E

joint_to_world_transforms(q)

Computes the transformation matrices for all joints (Joint frame --> world frame)

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Transformation matrices, shape (nv, 4, 4)

Source code in frax/core/robot.py

def joint_to_world_transforms(self, q: Array) -> Array:
    """Computes the transformation matrices for all joints (Joint frame --> world frame)

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Transformation matrices, shape (nv, 4, 4)
    """
    # Note about different FK methods:
    # Jax's associative scan is O(log(N)) complexity whereas just unrolling
    # the loop is O(N). For a pure kinematic chain like a serial manipulator,
    # associative scan should be best. But for a kinematic tree like a humanoid,
    # it may be simpler to unroll the loop and rely on the parent mapping.
    if self.is_pure_kinematic_chain:
        return self._scanned_fk(q)
    return self._unrolled_fk(q)

base_transform(q)

Transformation matrix of the floating base (w.r.t world), shape (4, 4)

Source code in frax/core/robot.py

def base_transform(self, q: Array) -> Array:
    """Transformation matrix of the floating base (w.r.t world), shape (4, 4)"""
    if not self.floating_base:
        return jnp.eye(4)
    joint_transforms = self.joint_to_world_transforms(q)
    return self._base_transform(joint_transforms)

link_to_world_transforms(q)

Compute the transformation matrices for all link inertial frames (link inertial frame --> world frame)

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Transformation matrices, shape (nv, 4, 4)

Source code in frax/core/robot.py

def link_to_world_transforms(self, q: Array) -> Array:
    """Compute the transformation matrices for all link inertial frames (link inertial frame --> world frame)

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Transformation matrices, shape (nv, 4, 4)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._link_to_world_transforms(joint_transforms)

link_com_positions(q)

Compute the positions of all link COMs in world frame

Parameters:

Name	Type	Description	Default
q	Array	Joint angles, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Link COM positions in world frame, shape (num_links, 3)

Source code in frax/core/robot.py

def link_com_positions(self, q: Array) -> Array:
    """Compute the positions of all link COMs in world frame

    Args:
        q (Array): Joint angles, shape (nq,)

    Returns:
        Array: Link COM positions in world frame, shape (num_links, 3)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._link_com_positions(joint_transforms)

center_of_mass(q)

Compute the center of mass of the robot, in world frame

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Position of the center of mass, shape (3,)

Source code in frax/core/robot.py

def center_of_mass(self, q: Array) -> Array:
    """Compute the center of mass of the robot, in world frame

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Position of the center of mass, shape (3,)
    """
    transforms = self.joint_to_world_transforms(q)
    return self._center_of_mass(transforms)

center_of_mass_jacobian(q)

Computes the linear Jacobian (Jv) for the motion of the COM

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Jv_COM, shape (3, nv)

Source code in frax/core/robot.py

def center_of_mass_jacobian(self, q: Array) -> Array:
    """Computes the linear Jacobian (Jv) for the motion of the COM

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Jv_COM, shape (3, nv)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._center_of_mass_jacobian(joint_transforms)

link_collision_data(q)

Compute collision data for all links given the joint configuration

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Type	Description
Tuple[Array, Array]	Tuple[Array, Array]: positions (Array): Positions of the collision spheres in world frame, shape (num_collision_spheres, 3) radii (Array): Radii of the collision spheres, shape (num_collision_spheres,)

Source code in frax/core/robot.py

def link_collision_data(self, q: Array) -> Tuple[Array, Array]:
    """Compute collision data for all links given the joint configuration

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Tuple[Array, Array]:
            positions (Array): Positions of the collision spheres in world frame,
                shape (num_collision_spheres, 3)
            radii (Array): Radii of the collision spheres, shape (num_collision_spheres,)
    """
    if not self.has_collision_data:
        return jnp.array([]), jnp.array([])
    joint_transforms = self.joint_to_world_transforms(q)
    return self._link_collision_data(joint_transforms)

link_collision_positions(q)

Compute the positions of all collision spheres in world frame

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Collision positions, shape (num_collision_spheres, 3)

Source code in frax/core/robot.py

def link_collision_positions(self, q: Array) -> Array:
    """Compute the positions of all collision spheres in world frame

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Collision positions, shape (num_collision_spheres, 3)
    """
    if not self.has_collision_data:
        return jnp.array([])
    joint_transforms = self.joint_to_world_transforms(q)
    return self._link_collision_positions(joint_transforms)

mass_matrix(q)

Compute the mass matrix for a given joint configuration

Parameters:

Name	Type	Description	Default
q	Array	Array of joint angles, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	The mass matrix, shape (nv, nv)

Source code in frax/core/robot.py

def mass_matrix(self, q: Array) -> Array:
    """Compute the mass matrix for a given joint configuration

    Args:
        q (Array): Array of joint angles, shape (nq,)

    Returns:
        Array: The mass matrix, shape (nv, nv)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._mass_matrix(joint_transforms)

mass_matrix_inverse(M)

Compute the inverse of the mass matrix

Parameters:

Name	Type	Description	Default
M	Array	Mass matrix, shape (nv, nv)	required

Returns:

Name	Type	Description
Array	Array	Inverse of the mass matrix, shape (nv, nv)

Source code in frax/core/robot.py

def mass_matrix_inverse(self, M: Array) -> Array:
    """Compute the inverse of the mass matrix

    Args:
        M (Array): Mass matrix, shape (nv, nv)

    Returns:
        Array: Inverse of the mass matrix, shape (nv, nv)
    """
    return cholesky_spd_inverse(M)

gravity_vector(q)

Compute the gravity vector for a given joint configuration

Parameters:

Name	Type	Description	Default
q	Array	Array of joint angles, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	The gravity vector, shape (nv,)

Source code in frax/core/robot.py

def gravity_vector(self, q: Array) -> Array:
    """Compute the gravity vector for a given joint configuration

    Args:
        q (Array): Array of joint angles, shape (nq,)

    Returns:
        Array: The gravity vector, shape (nv,)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._gravity_vector(joint_transforms)

centrifugal_coriolis_vector(q, v)

Compute the centrifugal and coriolis vector for a given joint configuration

Parameters:

Name	Type	Description	Default
q	Array	Array of joint angles, shape (nq,)	required
v	Array	Array of Generalized velocities, shape (nv,)	required

Returns:

Name	Type	Description
Array	Array	The centrifugal and coriolis vector, shape (nv,)

Source code in frax/core/robot.py

def centrifugal_coriolis_vector(self, q: Array, v: Array) -> Array:
    """Compute the centrifugal and coriolis vector for a given joint configuration

    Args:
        q (Array): Array of joint angles, shape (nq,)
        v (Array): Array of Generalized velocities, shape (nv,)

    Returns:
        Array: The centrifugal and coriolis vector, shape (nv,)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._centrifugal_coriolis_vector(v, joint_transforms)

nonlinear_bias(q, v)

Compute the nonlinear bias vector (Centrifugal/Coriolis + Gravity) in a single pass

b(q, v) = c(q, v) + g(q),

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required
v	Array	Generalized velocities, shape (nv,)	required

Returns:

Name	Type	Description
Array	Array	The nonlinear bias vector, shape (nv,)

Source code in frax/core/robot.py

def nonlinear_bias(self, q: Array, v: Array) -> Array:
    """Compute the nonlinear bias vector (Centrifugal/Coriolis + Gravity) in a single pass
    ```
    b(q, v) = c(q, v) + g(q),
    ```

    Args:
        q (Array): Configuration vector, shape (nq,)
        v (Array): Generalized velocities, shape (nv,)

    Returns:
        Array: The nonlinear bias vector, shape (nv,)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._nonlinear_bias(v, joint_transforms)

rnea(q, v, a, gravity_accel, F_ext)

Recursive Newton-Euler Algorithm (vectorized form)

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required
v	Optional[Array]	Generalized velocities, shape (nv,). None if not considering joint velocities (as is done to compute gravity)	required
a	Optional[Array]	Generalized accelerations, shape (nv,). This is currently not used for most methods and can be set to None.	required
gravity_accel	Optional[Array]	Spatial acceleration from gravity, shape (6,). None if not considering gravity (as is done to compute centrifugal/coriolis)	required
F_ext	Optional[Array]	External wrenches on each link (expressed in the root/world frame), shape (nv, 6). This is currently not used for most methods and can be set to None.	required

Returns:

Name	Type	Description
Array	Array	Joint torques, shape (nv,)

Source code in frax/core/robot.py

def rnea(
    self,
    q: Array,
    v: Optional[Array],
    a: Optional[Array],
    gravity_accel: Optional[Array],
    F_ext: Optional[Array],
) -> Array:
    """Recursive Newton-Euler Algorithm (vectorized form)

    Args:
        q (Array): Configuration vector, shape (nq,)
        v (Optional[Array]): Generalized velocities, shape (nv,). None if not considering
            joint velocities (as is done to compute gravity)
        a (Optional[Array]): Generalized accelerations, shape (nv,). This is currently not used
            for most methods and can be set to None.
        gravity_accel (Optional[Array]): Spatial acceleration from gravity, shape (6,). None if
            not considering gravity (as is done to compute centrifugal/coriolis)
        F_ext (Optional[Array]): External wrenches on each link (expressed in the root/world frame),
            shape (nv, 6). This is currently not used for most methods and can be set to None.

    Returns:
        Array: Joint torques, shape (nv,)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    spatial_axes, spatial_inertias = self._spatial_axes_and_inertias(
        joint_transforms
    )
    return self._rnea_from_spatial_data(
        spatial_axes, spatial_inertias, v, a, gravity_accel, F_ext
    )

crba(q)

Composite Rigid Body Algorithm (vectorized form)

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required

Returns:

Name	Type	Description
Array	Array	Mass matrix, shape (nv, nv)

Source code in frax/core/robot.py

def crba(self, q: Array) -> Array:
    """Composite Rigid Body Algorithm (vectorized form)

    Args:
        q (Array): Configuration vector, shape (nq,)

    Returns:
        Array: Mass matrix, shape (nv, nv)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    spatial_axes, spatial_inertias = self._spatial_axes_and_inertias(
        joint_transforms
    )
    return self._crba_from_spatial_data(spatial_axes, spatial_inertias)

forward_dynamics(q, v, tau, fext)

Compute the joint acceleration resulting from an applied torque (and optionally, any external forces acting on the links), given the joint state

Note: gravity is assumed always applied (for now)

Parameters:

Name	Type	Description	Default
q	Array	Configuration vector, shape (nq,)	required
v	Array	Generalized velocities, shape (nv,)	required
tau	Array	Joint torques, shape (nv,)	required
fext	Optional[Array]	External wrenches on each link (expressed in the root/world frame), shape (nv, 6). Set to None if no external forces are applied	required

Returns:

Name	Type	Description
Array	Array	Joint accelerations, shape (nv,)

Source code in frax/core/robot.py

def forward_dynamics(
    self, q: Array, v: Array, tau: Array, fext: Optional[Array]
) -> Array:
    """Compute the joint acceleration resulting from an applied torque (and optionally,
    any external forces acting on the links), given the joint state

    Note: gravity is assumed always applied (for now)

    Args:
        q (Array): Configuration vector, shape (nq,)
        v (Array): Generalized velocities, shape (nv,)
        tau (Array): Joint torques, shape (nv,)
        fext (Optional[Array]): External wrenches on each link (expressed in the root/world frame),
            shape (nv, 6). Set to None if no external forces are applied

    Returns:
        Array: Joint accelerations, shape (nv,)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._forward_dynamics(joint_transforms, v, tau, fext)