Skip to content

Manipulator

Manipulator kinematics and dynamics

Manipulator

Bases: Robot

Manipulator kinematics and dynamics

Parameters:

Name Type Description Default
urdf_filename str

Path to the URDF file to load

 required
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 a free-floating base: None (fixed base), "quaternion", or "euler". See Robot for details. Defaults to None.

 None
ee_offset Optional[ArrayLike]

Transformation matrix specifying the end-effector offset from the last joint frame. Defaults to None.

 None
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/manipulator.py 
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
@jax.tree_util.register_static
class Manipulator(Robot):
    """Manipulator kinematics and dynamics

    Args:
        urdf_filename (str): Path to the URDF file to load
        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 a free-floating base: None (fixed base),
            "quaternion", or "euler". See Robot for details. Defaults to None.
        ee_offset (Optional[ArrayLike]): Transformation matrix specifying the end-effector
            offset from the last joint frame. Defaults to None.
        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: str,
        collision_data: Optional[dict] = None,
        joint_ordering: Optional[list[str]] = None,
        floating_base: Optional[str] = None,
        ee_offset: Optional[ArrayLike] = None,
        default_configuration: Optional[ArrayLike] = None,
    ):
        super().__init__(
            urdf_filename,
            collision_data,
            joint_ordering,
            floating_base=floating_base,
            default_configuration=default_configuration,
        )
        # TODO decide if floating base should be an input? Only makes sense for in-space manipulators...
        assert self.is_pure_kinematic_chain

        # TODO REORGANIZE ALL OF THIS BELOW
        self.ee_parent_chain = np.arange(self.nv)

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

        self.ee_offset = ee_offset

    # EE STUFF

    def ee_transform(self, q: Array) -> Array:
        """Transformation matrix of the end effector (EE frame --> world frame)

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

        Returns:
            Array: Transformation matrix, shape (4, 4)
        """
        transforms = self.joint_to_world_transforms(q)
        return self._ee_transform(transforms)

    def _ee_transform(self, joint_transforms: Array) -> Array:
        """Helper function: Compute EE transform given joint transforms"""
        parent_index = self.ee_parent_chain[-1]
        return self._frame_transform(joint_transforms, self.ee_offset, parent_index)

    def ee_jacobian(self, q: Array) -> Array:
        """Jacobian [Jv; Jw] of the end effector given the joint configuration

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

        Returns:
            Array: Jacobian, shape (6, nv). The first 3 rows are the linear Jacobian,
                and the last 3 rows are the angular Jacobian
        """
        transforms = self.joint_to_world_transforms(q)
        return self._ee_jacobian(transforms)

    def _ee_jacobian(self, joint_transforms: Array):
        """Helper function: Compute EE jacobian given joint transforms"""
        return self._frame_jacobian(
            joint_transforms, self.ee_offset, self.ee_parent_chain
        )

    def ee_jacobian_and_derivative(self, q: Array, v: Array) -> Tuple[Array, Array]:
        """End-effector Jacobian and its time derivative (w.r.t world)

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

        Returns:
            Tuple[Array, Array]:
                J (Array): EE Jacobian, shape (6, nv)
                Jdot (Array): Time derivative of the EE Jacobian, shape (6, nv)
        """
        transforms = self.joint_to_world_transforms(q)
        return self._ee_jacobian_and_derivative(v, transforms)

    def _ee_jacobian_and_derivative(
        self, v: Array, joint_transforms: Array
    ) -> Tuple[Array, Array]:
        """Helper function: Compute EE Jacobian and time derivative given joint transforms"""
        return self._frame_jacobian_and_derivative(
            v, joint_transforms, self.ee_offset, self.ee_parent_chain
        )

    def ee_manipulability_index(self, q: Array) -> float:
        """Manipulability index of the end-effector Jacobian

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

        Returns:
            float: Manipulability index
        """
        joint_transforms = self.joint_to_world_transforms(q)
        return self._ee_manipulability_index(joint_transforms)

    def _ee_manipulability_index(self, joint_transforms: Array) -> float:
        """Helper function: Computes EE manipulability index given joint transforms"""
        J_full = self._ee_jacobian(joint_transforms)
        return self._manipulability_index_helper(J_full, self.ee_parent_chain)

    def torque_control_matrices(
        self, q: Array, v: Array
    ) -> Tuple[Array, Array, Array, Array, Array, Array]:
        """Compute the matrices required for operational space torque control
        with just a single evaluation of the kinematics

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

        Returns:
            Tuple[Array, Array, Array, Array, Array, Array]:
                M: Mass matrix, shape (nv, nv)
                M_inv: Inverse of the mass matrix, shape (nv, nv)
                G: Gravity vector, shape (nv,)
                C: Centrifugal/coriolis vector, shape (nv,)
                J: End effector basic Jacobian, shape (6, nv)
                T: End effector transformation matrix, shape (4, 4)
        """
        joint_transforms = self.joint_to_world_transforms(q)
        M = self._mass_matrix(joint_transforms)
        M_inv = self.mass_matrix_inverse(M)
        G = self._gravity_vector(joint_transforms)
        C = self._centrifugal_coriolis_vector(v, joint_transforms)
        J = self._ee_jacobian(joint_transforms)
        T = self._ee_transform(joint_transforms)
        return M, M_inv, G, C, J, T

    def velocity_control_matrices(self, q: Array) -> Tuple[Array, Array]:
        """Compute the matrices required for operational space velocity control
        with just a single evaluation of the kinematics

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

        Returns:
            Tuple[Array, Array]:
                J: End effector basic Jacobian, shape (6, nv)
                T: End effector transformation matrix, shape (4, 4)
        """
        joint_transforms = self.joint_to_world_transforms(q)
        J = self._ee_jacobian(joint_transforms)
        T = self._ee_transform(joint_transforms)
        return J, T

    def dynamically_consistent_velocity_control_matrices(
        self, q: Array
    ) -> Tuple[Array, Array, Array]:
        """Compute the matrices required for operational space velocity control
        with just a single evaluation of the kinematics.

        This version also returns the inverse of the mass matrix, which is required
        to construct the dynamically-consistent generalized Jacobian inverse

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

        Returns:
            Tuple[Array, Array, Array]:
                M_inv: Inverse of the mass matrix, shape (nv, nv)
                J: End effector basic Jacobian, shape (6, nv)
                T: End effector transformation matrix, shape (4, 4)
        """
        joint_transforms = self.joint_to_world_transforms(q)
        J = self._ee_jacobian(joint_transforms)
        T = self._ee_transform(joint_transforms)
        M = self._mass_matrix(joint_transforms)
        M_inv = self.mass_matrix_inverse(M)
        return M_inv, J, T

num_joints property

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

ee_transform(q)

Transformation matrix of the end effector (EE frame --> world frame)

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required

Returns:

Name Type Description
Array Array

Transformation matrix, shape (4, 4)
Source code in frax/core/manipulator.py 
66
67
68
69
70
71
72
73
74
75
76
def ee_transform(self, q: Array) -> Array:
    """Transformation matrix of the end effector (EE frame --> world frame)

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

    Returns:
        Array: Transformation matrix, shape (4, 4)
    """
    transforms = self.joint_to_world_transforms(q)
    return self._ee_transform(transforms)

ee_jacobian(q)

Jacobian [Jv; Jw] of the end effector given the joint configuration

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required

Returns:

Name Type Description
Array Array

Jacobian, shape (6, nv). The first 3 rows are the linear Jacobian, and the last 3 rows are the angular Jacobian
Source code in frax/core/manipulator.py 
83
84
85
86
87
88
89
90
91
92
93
94
def ee_jacobian(self, q: Array) -> Array:
    """Jacobian [Jv; Jw] of the end effector given the joint configuration

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

    Returns:
        Array: Jacobian, shape (6, nv). The first 3 rows are the linear Jacobian,
            and the last 3 rows are the angular Jacobian
    """
    transforms = self.joint_to_world_transforms(q)
    return self._ee_jacobian(transforms)

ee_jacobian_and_derivative(q, v)

End-effector Jacobian and its time derivative (w.r.t world)

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required
v Array

Generalized velocities, shape (nv,)

 required

Returns:

Type Description
Tuple[Array, Array]

Tuple[Array, Array]: J (Array): EE Jacobian, shape (6, nv) Jdot (Array): Time derivative of the EE Jacobian, shape (6, nv)
Source code in frax/core/manipulator.py 
102
103
104
105
106
107
108
109
110
111
112
113
114
115
def ee_jacobian_and_derivative(self, q: Array, v: Array) -> Tuple[Array, Array]:
    """End-effector Jacobian and its time derivative (w.r.t world)

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

    Returns:
        Tuple[Array, Array]:
            J (Array): EE Jacobian, shape (6, nv)
            Jdot (Array): Time derivative of the EE Jacobian, shape (6, nv)
    """
    transforms = self.joint_to_world_transforms(q)
    return self._ee_jacobian_and_derivative(v, transforms)

ee_manipulability_index(q)

Manipulability index of the end-effector Jacobian

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required

Returns:

Name Type Description
float float

Manipulability index
Source code in frax/core/manipulator.py 
125
126
127
128
129
130
131
132
133
134
135
def ee_manipulability_index(self, q: Array) -> float:
    """Manipulability index of the end-effector Jacobian

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

    Returns:
        float: Manipulability index
    """
    joint_transforms = self.joint_to_world_transforms(q)
    return self._ee_manipulability_index(joint_transforms)

torque_control_matrices(q, v)

Compute the matrices required for operational space torque control with just a single evaluation of the kinematics

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required
v Array

Generalized velocities, shape (nv,)

 required

Returns:

Type Description
Tuple[Array, Array, Array, Array, Array, Array]

Tuple[Array, Array, Array, Array, Array, Array]: M: Mass matrix, shape (nv, nv) M_inv: Inverse of the mass matrix, shape (nv, nv) G: Gravity vector, shape (nv,) C: Centrifugal/coriolis vector, shape (nv,) J: End effector basic Jacobian, shape (6, nv) T: End effector transformation matrix, shape (4, 4)
Source code in frax/core/manipulator.py 
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
def torque_control_matrices(
    self, q: Array, v: Array
) -> Tuple[Array, Array, Array, Array, Array, Array]:
    """Compute the matrices required for operational space torque control
    with just a single evaluation of the kinematics

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

    Returns:
        Tuple[Array, Array, Array, Array, Array, Array]:
            M: Mass matrix, shape (nv, nv)
            M_inv: Inverse of the mass matrix, shape (nv, nv)
            G: Gravity vector, shape (nv,)
            C: Centrifugal/coriolis vector, shape (nv,)
            J: End effector basic Jacobian, shape (6, nv)
            T: End effector transformation matrix, shape (4, 4)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    M = self._mass_matrix(joint_transforms)
    M_inv = self.mass_matrix_inverse(M)
    G = self._gravity_vector(joint_transforms)
    C = self._centrifugal_coriolis_vector(v, joint_transforms)
    J = self._ee_jacobian(joint_transforms)
    T = self._ee_transform(joint_transforms)
    return M, M_inv, G, C, J, T

velocity_control_matrices(q)

Compute the matrices required for operational space velocity control with just a single evaluation of the kinematics

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required

Returns:

Type Description
Tuple[Array, Array]

Tuple[Array, Array]: J: End effector basic Jacobian, shape (6, nv) T: End effector transformation matrix, shape (4, 4)
Source code in frax/core/manipulator.py 
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
def velocity_control_matrices(self, q: Array) -> Tuple[Array, Array]:
    """Compute the matrices required for operational space velocity control
    with just a single evaluation of the kinematics

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

    Returns:
        Tuple[Array, Array]:
            J: End effector basic Jacobian, shape (6, nv)
            T: End effector transformation matrix, shape (4, 4)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    J = self._ee_jacobian(joint_transforms)
    T = self._ee_transform(joint_transforms)
    return J, T

dynamically_consistent_velocity_control_matrices(q)

Compute the matrices required for operational space velocity control with just a single evaluation of the kinematics.

This version also returns the inverse of the mass matrix, which is required to construct the dynamically-consistent generalized Jacobian inverse

Parameters:

Name Type Description Default
q Array

Configuration vector, shape (nq,)

 required

Returns:

Type Description
Tuple[Array, Array, Array]

Tuple[Array, Array, Array]: M_inv: Inverse of the mass matrix, shape (nv, nv) J: End effector basic Jacobian, shape (6, nv) T: End effector transformation matrix, shape (4, 4)
Source code in frax/core/manipulator.py 
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
def dynamically_consistent_velocity_control_matrices(
    self, q: Array
) -> Tuple[Array, Array, Array]:
    """Compute the matrices required for operational space velocity control
    with just a single evaluation of the kinematics.

    This version also returns the inverse of the mass matrix, which is required
    to construct the dynamically-consistent generalized Jacobian inverse

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

    Returns:
        Tuple[Array, Array, Array]:
            M_inv: Inverse of the mass matrix, shape (nv, nv)
            J: End effector basic Jacobian, shape (6, nv)
            T: End effector transformation matrix, shape (4, 4)
    """
    joint_transforms = self.joint_to_world_transforms(q)
    J = self._ee_jacobian(joint_transforms)
    T = self._ee_transform(joint_transforms)
    M = self._mass_matrix(joint_transforms)
    M_inv = self.mass_matrix_inverse(M)
    return M_inv, J, T

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 
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
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 
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
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 
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
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 
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
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 
441
442
443
444
445
446
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)

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 
454
455
456
457
458
459
460
461
462
463
464
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)

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 
474
475
476
477
478
479
480
481
482
483
484
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 
495
496
497
498
499
500
501
502
503
504
505
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 
516
517
518
519
520
521
522
523
524
525
526
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)

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 
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
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)

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 
722
723
724
725
726
727
728
729
730
731
732
733
734
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 
931
932
933
934
935
936
937
938
939
940
941
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 
950
951
952
953
954
955
956
957
958
959
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 
961
962
963
964
965
966
967
968
969
970
971
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 
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
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 
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
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 
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
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 
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
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 
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
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)