Skip to content

lie

Lie algebra operations for rigid body dynamics.

This module provides symbolic expression nodes for Lie algebra operations commonly used in 6-DOF rigid body dynamics, robotics, and geometric mechanics. These operations enable elegant formulations of Newton-Euler dynamics using spatial vectors (twists and wrenches).

The module provides two tiers of functionality:

Built-in operators work out of the box and include adjoint/coadjoint operators for dynamics (e.g. Adjoint, AdjointDual) and frame transformations (e.g. SE3Adjoint, SE3AdjointDual).

jaxlie-backed operators require pip install openscvx[lie] and provide exponential/logarithm maps for SO(3) and SE(3) groups (e.g. SO3Exp, SO3Log, SE3Exp, SE3Log).

Conventions
  • Twist (spatial velocity): ξ = [v; ω] where v ∈ ℝ³ is linear velocity and ω ∈ ℝ³ is angular velocity (both in body frame)
  • Wrench (spatial force): F = [f; τ] where f ∈ ℝ³ is force and τ ∈ ℝ³ is torque (both in body frame)
Note

The twist convention [v; ω] (linear first, angular second) matches jaxlie's SE3 tangent parameterization, so no reordering is needed during lowering.

Example

Newton-Euler dynamics for a rigid body using the coadjoint operator::

import openscvx as ox

twist = ox.State("twist", shape=(6,))
M = ox.Parameter("M", shape=(6, 6), value=spatial_inertia)
wrench = ox.Control("wrench", shape=(6,))

momentum = M @ twist
bias_force = ox.lie.AdjointDual(twist, momentum)
twist_dot = M_inv @ (wrench - bias_force)

Product of Exponentials forward kinematics (requires jaxlie)::

screw_axis = ox.Constant(np.array([0, 0, 0, 0, 0, 1]))
theta = ox.State("theta", shape=(1,))
T_joint = ox.lie.SE3Exp(screw_axis * theta)  # 4×4 matrix
References
  • Murray, Li, Sastry: "A Mathematical Introduction to Robotic Manipulation"
  • Featherstone: "Rigid Body Dynamics Algorithms"
  • Sola et al.: "A micro Lie theory for state estimation in robotics"

Adjoint

Bases: Expr

Adjoint operator ad (Lie bracket) for twist-on-twist action.

Computes the adjoint action ad_ξ₁(ξ₂) which represents the Lie bracket [ξ₁, ξ₂] of two twists. This is used for velocity propagation in kinematic chains and acceleration computations.

For se(3), given twists ξ₁ = [v₁; ω₁] and ξ₂ = [v₂; ω₂]:

ad_ξ₁(ξ₂) = [ξ₁, ξ₂] = [ ω₁ × v₂ - ω₂ × v₁ ]
                        [     ω₁ × ω₂       ]

Equivalently using the adjoint matrix:

ad_ξ = [ [ω]×    0   ]
       [ [v]×   [ω]× ]

where [·]× denotes the 3x3 skew-symmetric (cross product) matrix.

Attributes:

Name Type Description
twist1

First 6D twist vector [v₁; ω₁]
twist2

Second 6D twist vector [v₂; ω₂]
Example

Compute the Lie bracket of two twists::

import openscvx as ox

twist1 = ox.State("twist1", shape=(6,))
twist2 = ox.State("twist2", shape=(6,))

bracket = ox.lie.Adjoint(twist1, twist2)

Velocity propagation in a kinematic chain::

# Child link velocity includes parent velocity plus relative motion
# V_child = Ad_T @ V_parent + joint_twist * q_dot
Note

The adjoint satisfies the Jacobi identity and is antisymmetric: ad_ξ₁(ξ₂) = -ad_ξ₂(ξ₁)

See Also

AdjointDual: The coadjoint operator for momentum dynamics

Source code in openscvx/symbolic/expr/lie/adjoint.py 
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
class Adjoint(Expr):
    """Adjoint operator ad (Lie bracket) for twist-on-twist action.

    Computes the adjoint action ad_ξ₁(ξ₂) which represents the Lie bracket
    [ξ₁, ξ₂] of two twists. This is used for velocity propagation in
    kinematic chains and acceleration computations.

    For se(3), given twists ξ₁ = [v₁; ω₁] and ξ₂ = [v₂; ω₂]:

        ad_ξ₁(ξ₂) = [ξ₁, ξ₂] = [ ω₁ × v₂ - ω₂ × v₁ ]
                                [     ω₁ × ω₂       ]

    Equivalently using the adjoint matrix:

        ad_ξ = [ [ω]×    0   ]
               [ [v]×   [ω]× ]

    where [·]× denotes the 3x3 skew-symmetric (cross product) matrix.

    Attributes:
        twist1: First 6D twist vector [v₁; ω₁]
        twist2: Second 6D twist vector [v₂; ω₂]

    Example:
        Compute the Lie bracket of two twists::

            import openscvx as ox

            twist1 = ox.State("twist1", shape=(6,))
            twist2 = ox.State("twist2", shape=(6,))

            bracket = ox.lie.Adjoint(twist1, twist2)

        Velocity propagation in a kinematic chain::

            # Child link velocity includes parent velocity plus relative motion
            # V_child = Ad_T @ V_parent + joint_twist * q_dot

    Note:
        The adjoint satisfies the Jacobi identity and is antisymmetric:
        ad_ξ₁(ξ₂) = -ad_ξ₂(ξ₁)

    See Also:
        AdjointDual: The coadjoint operator for momentum dynamics
    """

    def __init__(
        self,
        twist1: Union[Expr, float, int, np.ndarray],
        twist2: Union[Expr, float, int, np.ndarray],
    ):
        """Initialize an adjoint operator.

        Args:
            twist1: First 6D twist vector [v; ω] with shape (6,)
            twist2: Second 6D twist vector [v; ω] with shape (6,)
        """
        self.twist1 = to_expr(twist1)
        self.twist2 = to_expr(twist2)

    def children(self):
        return [self.twist1, self.twist2]

    def canonicalize(self) -> "Expr":
        twist1 = self.twist1.canonicalize()
        twist2 = self.twist2.canonicalize()
        return Adjoint(twist1, twist2)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that inputs are 6D vectors and return output shape.

        Returns:
            tuple: Shape (6,) for the resulting Lie bracket

        Raises:
            ValueError: If either twist does not have shape (6,)
        """
        twist1_shape = self.twist1.check_shape()
        twist2_shape = self.twist2.check_shape()

        if twist1_shape != (6,):
            raise ValueError(f"Adjoint expects twist1 with shape (6,), got {twist1_shape}")
        if twist2_shape != (6,):
            raise ValueError(f"Adjoint expects twist2 with shape (6,), got {twist2_shape}")

        return (6,)

    def __repr__(self) -> str:
        return f"ad({self.twist1!r}, {self.twist2!r})"
__init__(twist1: Union[Expr, float, int, np.ndarray], twist2: Union[Expr, float, int, np.ndarray])

Initialize an adjoint operator.

Parameters:

Name Type Description Default
twist1 Union[Expr, float, int, ndarray]

First 6D twist vector [v; ω] with shape (6,)

 required
twist2 Union[Expr, float, int, ndarray]

Second 6D twist vector [v; ω] with shape (6,)

 required
Source code in openscvx/symbolic/expr/lie/adjoint.py 
160
161
162
163
164
165
166
167
168
169
170
171
172
def __init__(
    self,
    twist1: Union[Expr, float, int, np.ndarray],
    twist2: Union[Expr, float, int, np.ndarray],
):
    """Initialize an adjoint operator.

    Args:
        twist1: First 6D twist vector [v; ω] with shape (6,)
        twist2: Second 6D twist vector [v; ω] with shape (6,)
    """
    self.twist1 = to_expr(twist1)
    self.twist2 = to_expr(twist2)
check_shape() -> Tuple[int, ...]

Check that inputs are 6D vectors and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (6,) for the resulting Lie bracket

Raises:

Type Description
ValueError

If either twist does not have shape (6,)
Source code in openscvx/symbolic/expr/lie/adjoint.py 
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
def check_shape(self) -> Tuple[int, ...]:
    """Check that inputs are 6D vectors and return output shape.

    Returns:
        tuple: Shape (6,) for the resulting Lie bracket

    Raises:
        ValueError: If either twist does not have shape (6,)
    """
    twist1_shape = self.twist1.check_shape()
    twist2_shape = self.twist2.check_shape()

    if twist1_shape != (6,):
        raise ValueError(f"Adjoint expects twist1 with shape (6,), got {twist1_shape}")
    if twist2_shape != (6,):
        raise ValueError(f"Adjoint expects twist2 with shape (6,), got {twist2_shape}")

    return (6,)

AdjointDual

Bases: Expr

Coadjoint operator ad* for computing Coriolis and centrifugal forces.

Computes the coadjoint action ad*_ξ(μ) which represents the rate of change of momentum due to body rotation. This is the key term in Newton-Euler dynamics that captures Coriolis and centrifugal effects.

For se(3), given twist ξ = [v; ω] and momentum μ = [f; τ]:

ad*_ξ(μ) = [ ω × f + v × τ ]
           [     ω × τ     ]

This appears in the Newton-Euler equations as:

M @ ξ_dot = F_ext - ad*_ξ(M @ ξ)

where M is the spatial inertia matrix and F_ext is the external wrench.

Attributes:

Name Type Description
twist

6D twist vector [v; ω] (linear velocity, angular velocity)
momentum

6D momentum vector [p; L] or [f; τ] (linear, angular)
Example

Compute the bias force (Coriolis + centrifugal) for rigid body dynamics::

import openscvx as ox

twist = ox.State("twist", shape=(6,))
M = ox.Parameter("M", shape=(6, 6), value=inertia_matrix)

momentum = M @ twist
bias_force = ox.lie.AdjointDual(twist, momentum)

# In dynamics: twist_dot = M_inv @ (wrench - bias_force)
Note

The coadjoint is related to the adjoint by: ad*_ξ = -(ad_ξ)^T

For the special case of pure rotation (v=0) with diagonal inertia, the angular part reduces to the familiar ω × (J @ ω) term.

See Also

Adjoint: The adjoint operator for twist-on-twist action SSM: 3x3 skew-symmetric matrix for cross products

Source code in openscvx/symbolic/expr/lie/adjoint.py 
 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
class AdjointDual(Expr):
    """Coadjoint operator ad* for computing Coriolis and centrifugal forces.

    Computes the coadjoint action ad*_ξ(μ) which represents the rate of change
    of momentum due to body rotation. This is the key term in Newton-Euler
    dynamics that captures Coriolis and centrifugal effects.

    For se(3), given twist ξ = [v; ω] and momentum μ = [f; τ]:

        ad*_ξ(μ) = [ ω × f + v × τ ]
                   [     ω × τ     ]

    This appears in the Newton-Euler equations as:

        M @ ξ_dot = F_ext - ad*_ξ(M @ ξ)

    where M is the spatial inertia matrix and F_ext is the external wrench.

    Attributes:
        twist: 6D twist vector [v; ω] (linear velocity, angular velocity)
        momentum: 6D momentum vector [p; L] or [f; τ] (linear, angular)

    Example:
        Compute the bias force (Coriolis + centrifugal) for rigid body dynamics::

            import openscvx as ox

            twist = ox.State("twist", shape=(6,))
            M = ox.Parameter("M", shape=(6, 6), value=inertia_matrix)

            momentum = M @ twist
            bias_force = ox.lie.AdjointDual(twist, momentum)

            # In dynamics: twist_dot = M_inv @ (wrench - bias_force)

    Note:
        The coadjoint is related to the adjoint by: ad*_ξ = -(ad_ξ)^T

        For the special case of pure rotation (v=0) with diagonal inertia,
        the angular part reduces to the familiar ω × (J @ ω) term.

    See Also:
        Adjoint: The adjoint operator for twist-on-twist action
        SSM: 3x3 skew-symmetric matrix for cross products
    """

    def __init__(
        self,
        twist: Union[Expr, float, int, np.ndarray],
        momentum: Union[Expr, float, int, np.ndarray],
    ):
        """Initialize a coadjoint operator.

        Args:
            twist: 6D twist vector [v; ω] with shape (6,)
            momentum: 6D momentum vector [p; L] with shape (6,)
        """
        self.twist = to_expr(twist)
        self.momentum = to_expr(momentum)

    def children(self):
        return [self.twist, self.momentum]

    def canonicalize(self) -> "Expr":
        twist = self.twist.canonicalize()
        momentum = self.momentum.canonicalize()
        return AdjointDual(twist, momentum)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that inputs are 6D vectors and return output shape.

        Returns:
            tuple: Shape (6,) for the resulting coadjoint vector

        Raises:
            ValueError: If twist or momentum do not have shape (6,)
        """
        twist_shape = self.twist.check_shape()
        momentum_shape = self.momentum.check_shape()

        if twist_shape != (6,):
            raise ValueError(f"AdjointDual expects twist with shape (6,), got {twist_shape}")
        if momentum_shape != (6,):
            raise ValueError(f"AdjointDual expects momentum with shape (6,), got {momentum_shape}")

        return (6,)

    def __repr__(self) -> str:
        return f"ad_dual({self.twist!r}, {self.momentum!r})"
__init__(twist: Union[Expr, float, int, np.ndarray], momentum: Union[Expr, float, int, np.ndarray])

Initialize a coadjoint operator.

Parameters:

Name Type Description Default
twist Union[Expr, float, int, ndarray]

6D twist vector [v; ω] with shape (6,)

 required
momentum Union[Expr, float, int, ndarray]

6D momentum vector [p; L] with shape (6,)

 required
Source code in openscvx/symbolic/expr/lie/adjoint.py 
69
70
71
72
73
74
75
76
77
78
79
80
81
def __init__(
    self,
    twist: Union[Expr, float, int, np.ndarray],
    momentum: Union[Expr, float, int, np.ndarray],
):
    """Initialize a coadjoint operator.

    Args:
        twist: 6D twist vector [v; ω] with shape (6,)
        momentum: 6D momentum vector [p; L] with shape (6,)
    """
    self.twist = to_expr(twist)
    self.momentum = to_expr(momentum)
check_shape() -> Tuple[int, ...]

Check that inputs are 6D vectors and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (6,) for the resulting coadjoint vector

Raises:

Type Description
ValueError

If twist or momentum do not have shape (6,)
Source code in openscvx/symbolic/expr/lie/adjoint.py 
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
def check_shape(self) -> Tuple[int, ...]:
    """Check that inputs are 6D vectors and return output shape.

    Returns:
        tuple: Shape (6,) for the resulting coadjoint vector

    Raises:
        ValueError: If twist or momentum do not have shape (6,)
    """
    twist_shape = self.twist.check_shape()
    momentum_shape = self.momentum.check_shape()

    if twist_shape != (6,):
        raise ValueError(f"AdjointDual expects twist with shape (6,), got {twist_shape}")
    if momentum_shape != (6,):
        raise ValueError(f"AdjointDual expects momentum with shape (6,), got {momentum_shape}")

    return (6,)

SE3Adjoint

Bases: Expr

SE(3) Adjoint representation Ad_T for transforming twists between frames.

Computes the 6×6 adjoint matrix Ad_T that transforms twists from one coordinate frame to another. Given a transformation T_ab from frame A to frame B, the adjoint transforms a twist expressed in frame A to frame B:

ξ_b = Ad_{T_ab} @ ξ_a

For SE(3), given T with rotation R and translation p:

Ad_T = [ R      0   ]
       [ [p]×R  R   ]

where [p]× is the 3×3 skew-symmetric matrix of p.

This is essential for:

  • Velocity propagation through kinematic chains
  • Computing geometric Jacobians for manipulators
  • Recursive Newton-Euler dynamics algorithms

Attributes:

Name Type Description
transform

4×4 homogeneous transformation matrix
Example

Transform a body twist to the world frame::

import openscvx as ox

T_world_body = forward_kinematics(q)  # 4×4 transform
twist_body = ox.State("twist_body", shape=(6,))

# Transform twist to world frame
Ad_T = ox.lie.SE3Adjoint(T_world_body)  # 6×6 matrix
twist_world = Ad_T @ twist_body

Compute geometric Jacobian columns::

# Each column of the geometric Jacobian is Ad_{T_0i} @ ξ_i
J_col_i = ox.lie.SE3Adjoint(T_0_to_i) @ screw_axis_i
Note

The adjoint satisfies: Ad_{T1 @ T2} = Ad_{T1} @ Ad_{T2}

See Also
  • SE3AdjointDual: For transforming wrenches between frames
  • Adjoint: The small adjoint (Lie bracket) for twist-on-twist action
Source code in openscvx/symbolic/expr/lie/adjoint.py 
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
class SE3Adjoint(Expr):
    """SE(3) Adjoint representation Ad_T for transforming twists between frames.

    Computes the 6×6 adjoint matrix Ad_T that transforms twists from one
    coordinate frame to another. Given a transformation T_ab from frame A to
    frame B, the adjoint transforms a twist expressed in frame A to frame B:

        ξ_b = Ad_{T_ab} @ ξ_a

    For SE(3), given T with rotation R and translation p:

        Ad_T = [ R      0   ]
               [ [p]×R  R   ]

    where [p]× is the 3×3 skew-symmetric matrix of p.

    This is essential for:

    - Velocity propagation through kinematic chains
    - Computing geometric Jacobians for manipulators
    - Recursive Newton-Euler dynamics algorithms

    Attributes:
        transform: 4×4 homogeneous transformation matrix

    Example:
        Transform a body twist to the world frame::

            import openscvx as ox

            T_world_body = forward_kinematics(q)  # 4×4 transform
            twist_body = ox.State("twist_body", shape=(6,))

            # Transform twist to world frame
            Ad_T = ox.lie.SE3Adjoint(T_world_body)  # 6×6 matrix
            twist_world = Ad_T @ twist_body

        Compute geometric Jacobian columns::

            # Each column of the geometric Jacobian is Ad_{T_0i} @ ξ_i
            J_col_i = ox.lie.SE3Adjoint(T_0_to_i) @ screw_axis_i

    Note:
        The adjoint satisfies: Ad_{T1 @ T2} = Ad_{T1} @ Ad_{T2}

    See Also:
        - SE3AdjointDual: For transforming wrenches between frames
        - Adjoint: The small adjoint (Lie bracket) for twist-on-twist action
    """

    def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
        """Initialize SE3 Adjoint operator.

        Args:
            transform: 4×4 homogeneous transformation matrix with shape (4, 4)
        """
        self.transform = to_expr(transform)

    def children(self):
        return [self.transform]

    def canonicalize(self) -> "Expr":
        transform = self.transform.canonicalize()
        return SE3Adjoint(transform)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 4×4 matrix and return output shape.

        Returns:
            tuple: Shape (6, 6) for the adjoint matrix

        Raises:
            ValueError: If transform does not have shape (4, 4)
        """
        transform_shape = self.transform.check_shape()
        if transform_shape != (4, 4):
            raise ValueError(
                f"SE3Adjoint expects transform with shape (4, 4), got {transform_shape}"
            )
        return (6, 6)

    def __repr__(self) -> str:
        return f"Ad({self.transform!r})"
__init__(transform: Union[Expr, float, int, np.ndarray])

Initialize SE3 Adjoint operator.

Parameters:

Name Type Description Default
transform Union[Expr, float, int, ndarray]

4×4 homogeneous transformation matrix with shape (4, 4)

 required
Source code in openscvx/symbolic/expr/lie/adjoint.py 
255
256
257
258
259
260
261
def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
    """Initialize SE3 Adjoint operator.

    Args:
        transform: 4×4 homogeneous transformation matrix with shape (4, 4)
    """
    self.transform = to_expr(transform)
check_shape() -> Tuple[int, ...]

Check that input is a 4×4 matrix and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (6, 6) for the adjoint matrix

Raises:

Type Description
ValueError

If transform does not have shape (4, 4)
Source code in openscvx/symbolic/expr/lie/adjoint.py 
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 4×4 matrix and return output shape.

    Returns:
        tuple: Shape (6, 6) for the adjoint matrix

    Raises:
        ValueError: If transform does not have shape (4, 4)
    """
    transform_shape = self.transform.check_shape()
    if transform_shape != (4, 4):
        raise ValueError(
            f"SE3Adjoint expects transform with shape (4, 4), got {transform_shape}"
        )
    return (6, 6)

SE3AdjointDual

Bases: Expr

SE(3) coadjoint representation Ad*_T for transforming wrenches between frames.

Computes the 6×6 coadjoint matrix Ad*_T that transforms wrenches from one coordinate frame to another. Given a transformation T_ab from frame A to frame B, the coadjoint transforms a wrench expressed in frame B to frame A:

F_a = Ad*_{T_ab} @ F_b

For SE(3), given T with rotation R and translation p:

Ad*_T = [ R     [p]×R ]
        [ 0       R   ]

This is the transpose-inverse of Ad_T: Ad*_T = (Ad_T)^{-T}

This is essential for:

  • Force/torque propagation in dynamics
  • Transforming wrenches between end-effector and base frames
  • Recursive Newton-Euler dynamics algorithms

Attributes:

Name Type Description
transform

4×4 homogeneous transformation matrix
Example

Transform a wrench from end-effector to base frame::

import openscvx as ox

T_base_ee = forward_kinematics(q)  # 4×4 transform
wrench_ee = ox.Control("wrench_ee", shape=(6,))

# Transform wrench to base frame
Ad_star_T = ox.lie.SE3AdjointDual(T_base_ee)  # 6×6 matrix
wrench_base = Ad_star_T @ wrench_ee
Note

The coadjoint is related to the adjoint by: Ad*_T = (Ad_T)^{-T}

See Also
  • SE3Adjoint: For transforming twists between frames
  • AdjointDual: The small coadjoint for Coriolis/centrifugal forces
Source code in openscvx/symbolic/expr/lie/adjoint.py 
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
class SE3AdjointDual(Expr):
    """SE(3) coadjoint representation Ad*_T for transforming wrenches between frames.

    Computes the 6×6 coadjoint matrix Ad*_T that transforms wrenches from one
    coordinate frame to another. Given a transformation T_ab from frame A to
    frame B, the coadjoint transforms a wrench expressed in frame B to frame A:

        F_a = Ad*_{T_ab} @ F_b

    For SE(3), given T with rotation R and translation p:

        Ad*_T = [ R     [p]×R ]
                [ 0       R   ]

    This is the transpose-inverse of Ad_T: Ad*_T = (Ad_T)^{-T}

    This is essential for:

    - Force/torque propagation in dynamics
    - Transforming wrenches between end-effector and base frames
    - Recursive Newton-Euler dynamics algorithms

    Attributes:
        transform: 4×4 homogeneous transformation matrix

    Example:
        Transform a wrench from end-effector to base frame::

            import openscvx as ox

            T_base_ee = forward_kinematics(q)  # 4×4 transform
            wrench_ee = ox.Control("wrench_ee", shape=(6,))

            # Transform wrench to base frame
            Ad_star_T = ox.lie.SE3AdjointDual(T_base_ee)  # 6×6 matrix
            wrench_base = Ad_star_T @ wrench_ee

    Note:
        The coadjoint is related to the adjoint by: Ad*_T = (Ad_T)^{-T}

    See Also:
        - SE3Adjoint: For transforming twists between frames
        - AdjointDual: The small coadjoint for Coriolis/centrifugal forces
    """

    def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
        """Initialize SE3 coadjoint operator.

        Args:
            transform: 4×4 homogeneous transformation matrix with shape (4, 4)
        """
        self.transform = to_expr(transform)

    def children(self):
        return [self.transform]

    def canonicalize(self) -> "Expr":
        transform = self.transform.canonicalize()
        return SE3AdjointDual(transform)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 4×4 matrix and return output shape.

        Returns:
            tuple: Shape (6, 6) for the coadjoint matrix

        Raises:
            ValueError: If transform does not have shape (4, 4)
        """
        transform_shape = self.transform.check_shape()
        if transform_shape != (4, 4):
            raise ValueError(
                f"SE3AdjointDual expects transform with shape (4, 4), got {transform_shape}"
            )
        return (6, 6)

    def __repr__(self) -> str:
        return f"Ad_dual({self.transform!r})"
__init__(transform: Union[Expr, float, int, np.ndarray])

Initialize SE3 coadjoint operator.

Parameters:

Name Type Description Default
transform Union[Expr, float, int, ndarray]

4×4 homogeneous transformation matrix with shape (4, 4)

 required
Source code in openscvx/symbolic/expr/lie/adjoint.py 
335
336
337
338
339
340
341
def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
    """Initialize SE3 coadjoint operator.

    Args:
        transform: 4×4 homogeneous transformation matrix with shape (4, 4)
    """
    self.transform = to_expr(transform)
check_shape() -> Tuple[int, ...]

Check that input is a 4×4 matrix and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (6, 6) for the coadjoint matrix

Raises:

Type Description
ValueError

If transform does not have shape (4, 4)
Source code in openscvx/symbolic/expr/lie/adjoint.py 
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 4×4 matrix and return output shape.

    Returns:
        tuple: Shape (6, 6) for the coadjoint matrix

    Raises:
        ValueError: If transform does not have shape (4, 4)
    """
    transform_shape = self.transform.check_shape()
    if transform_shape != (4, 4):
        raise ValueError(
            f"SE3AdjointDual expects transform with shape (4, 4), got {transform_shape}"
        )
    return (6, 6)

SE3Exp

Bases: Expr

Exponential map from se(3) twist to SE(3) transformation matrix.

Maps a 6D twist vector to a 4×4 homogeneous transformation matrix. Uses jaxlie for numerically robust implementation with proper handling of small angles and translations.

The twist ξ = [v; ω] follows the convention:

  • v: 3D linear velocity component
  • ω: 3D angular velocity component

This is the key operation for Product of Exponentials (PoE) forward kinematics in robotic manipulators.

Attributes:

Name Type Description
twist

6D twist vector [v; ω] with shape (6,)
Example

Product of Exponentials forward kinematics::

import openscvx as ox
import numpy as np

# Screw axis for revolute joint about z-axis at origin
screw_axis = np.array([0, 0, 0, 0, 0, 1])  # [v; ω]
theta = ox.State("theta", shape=(1,))

# Joint transformation
T = ox.lie.SE3Exp(ox.Constant(screw_axis) * theta)  # 4×4 matrix

# Chain multiple joints
T_01 = ox.lie.SE3Exp(screw1 * q1)
T_12 = ox.lie.SE3Exp(screw2 * q2)
T_02 = T_01 @ T_12

Extract position from transformation::

T_ee = forward_kinematics(joint_angles)
p_ee = T_ee[:3, 3]  # End-effector position
Note

The twist convention [v; ω] matches jaxlie's SE3 tangent parameterization, so no reordering is performed.

See Also
  • SE3Log: Inverse operation (transformation matrix to twist)
  • SO3Exp: Rotation-only exponential map
  • AdjointDual: For dynamics computations with twists
Source code in openscvx/symbolic/expr/lie/se3.py 
 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
class SE3Exp(Expr):
    """Exponential map from se(3) twist to SE(3) transformation matrix.

    Maps a 6D twist vector to a 4×4 homogeneous transformation matrix.
    Uses jaxlie for numerically robust implementation with proper handling
    of small angles and translations.

    The twist ξ = [v; ω] follows the convention:

    - v: 3D linear velocity component
    - ω: 3D angular velocity component

    This is the key operation for Product of Exponentials (PoE) forward
    kinematics in robotic manipulators.

    Attributes:
        twist: 6D twist vector [v; ω] with shape (6,)

    Example:
        Product of Exponentials forward kinematics::

            import openscvx as ox
            import numpy as np

            # Screw axis for revolute joint about z-axis at origin
            screw_axis = np.array([0, 0, 0, 0, 0, 1])  # [v; ω]
            theta = ox.State("theta", shape=(1,))

            # Joint transformation
            T = ox.lie.SE3Exp(ox.Constant(screw_axis) * theta)  # 4×4 matrix

            # Chain multiple joints
            T_01 = ox.lie.SE3Exp(screw1 * q1)
            T_12 = ox.lie.SE3Exp(screw2 * q2)
            T_02 = T_01 @ T_12

        Extract position from transformation::

            T_ee = forward_kinematics(joint_angles)
            p_ee = T_ee[:3, 3]  # End-effector position

    Note:
        The twist convention [v; ω] matches jaxlie's SE3 tangent
        parameterization, so no reordering is performed.

    See Also:
        - SE3Log: Inverse operation (transformation matrix to twist)
        - SO3Exp: Rotation-only exponential map
        - AdjointDual: For dynamics computations with twists
    """

    def __init__(self, twist: Union[Expr, float, int, np.ndarray]):
        """Initialize SE3 exponential map.

        Args:
            twist: 6D twist vector [v; ω] with shape (6,)
        """
        self.twist = to_expr(twist)

    def children(self):
        return [self.twist]

    def canonicalize(self) -> "Expr":
        twist = self.twist.canonicalize()
        return SE3Exp(twist)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 6D vector and return output shape.

        Returns:
            tuple: Shape (4, 4) for the homogeneous transformation matrix

        Raises:
            ValueError: If twist does not have shape (6,)
        """
        twist_shape = self.twist.check_shape()
        if twist_shape != (6,):
            raise ValueError(f"SE3Exp expects twist with shape (6,), got {twist_shape}")
        return (4, 4)

    def __repr__(self) -> str:
        return f"SE3Exp({self.twist!r})"
__init__(twist: Union[Expr, float, int, np.ndarray])

Initialize SE3 exponential map.

Parameters:

Name Type Description Default
twist Union[Expr, float, int, ndarray]

6D twist vector [v; ω] with shape (6,)

 required
Source code in openscvx/symbolic/expr/lie/se3.py 
74
75
76
77
78
79
80
def __init__(self, twist: Union[Expr, float, int, np.ndarray]):
    """Initialize SE3 exponential map.

    Args:
        twist: 6D twist vector [v; ω] with shape (6,)
    """
    self.twist = to_expr(twist)
check_shape() -> Tuple[int, ...]

Check that input is a 6D vector and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (4, 4) for the homogeneous transformation matrix

Raises:

Type Description
ValueError

If twist does not have shape (6,)
Source code in openscvx/symbolic/expr/lie/se3.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 6D vector and return output shape.

    Returns:
        tuple: Shape (4, 4) for the homogeneous transformation matrix

    Raises:
        ValueError: If twist does not have shape (6,)
    """
    twist_shape = self.twist.check_shape()
    if twist_shape != (6,):
        raise ValueError(f"SE3Exp expects twist with shape (6,), got {twist_shape}")
    return (4, 4)

SE3Log

Bases: Expr

Logarithm map from SE(3) transformation matrix to se(3) twist.

Maps a 4×4 homogeneous transformation matrix to a 6D twist vector. Uses jaxlie for numerically robust implementation.

The output twist ξ = [v; ω] follows the convention:

  • v: 3D linear component
  • ω: 3D angular component (rotation vector)

This is useful for computing error metrics between poses in optimization.

Attributes:

Name Type Description
transform

4×4 homogeneous transformation matrix with shape (4, 4)
Example

Compute pose error for trajectory optimization::

import openscvx as ox

T_current = forward_kinematics(q)
T_target = ox.Parameter("T_target", shape=(4, 4), value=goal_pose)

# Relative transformation
T_error = ox.linalg.inv(T_target) @ T_current

# Convert to twist for error metric
twist_error = ox.lie.SE3Log(T_error)
pose_cost = ox.linalg.Norm(twist_error) ** 2
See Also
  • SE3Exp: Inverse operation (twist to transformation matrix)
  • SO3Log: Rotation-only logarithm map
Source code in openscvx/symbolic/expr/lie/se3.py 
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
class SE3Log(Expr):
    """Logarithm map from SE(3) transformation matrix to se(3) twist.

    Maps a 4×4 homogeneous transformation matrix to a 6D twist vector.
    Uses jaxlie for numerically robust implementation.

    The output twist ξ = [v; ω] follows the convention:

    - v: 3D linear component
    - ω: 3D angular component (rotation vector)

    This is useful for computing error metrics between poses in optimization.

    Attributes:
        transform: 4×4 homogeneous transformation matrix with shape (4, 4)

    Example:
        Compute pose error for trajectory optimization::

            import openscvx as ox

            T_current = forward_kinematics(q)
            T_target = ox.Parameter("T_target", shape=(4, 4), value=goal_pose)

            # Relative transformation
            T_error = ox.linalg.inv(T_target) @ T_current

            # Convert to twist for error metric
            twist_error = ox.lie.SE3Log(T_error)
            pose_cost = ox.linalg.Norm(twist_error) ** 2

    See Also:
        - SE3Exp: Inverse operation (twist to transformation matrix)
        - SO3Log: Rotation-only logarithm map
    """

    def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
        """Initialize SE3 logarithm map.

        Args:
            transform: 4×4 homogeneous transformation matrix with shape (4, 4)
        """
        self.transform = to_expr(transform)

    def children(self):
        return [self.transform]

    def canonicalize(self) -> "Expr":
        transform = self.transform.canonicalize()
        return SE3Log(transform)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 4×4 matrix and return output shape.

        Returns:
            tuple: Shape (6,) for the twist vector

        Raises:
            ValueError: If transform does not have shape (4, 4)
        """
        transform_shape = self.transform.check_shape()
        if transform_shape != (4, 4):
            raise ValueError(f"SE3Log expects transform with shape (4, 4), got {transform_shape}")
        return (6,)

    def __repr__(self) -> str:
        return f"SE3Log({self.transform!r})"
__init__(transform: Union[Expr, float, int, np.ndarray])

Initialize SE3 logarithm map.

Parameters:

Name Type Description Default
transform Union[Expr, float, int, ndarray]

4×4 homogeneous transformation matrix with shape (4, 4)

 required
Source code in openscvx/symbolic/expr/lie/se3.py 
143
144
145
146
147
148
149
def __init__(self, transform: Union[Expr, float, int, np.ndarray]):
    """Initialize SE3 logarithm map.

    Args:
        transform: 4×4 homogeneous transformation matrix with shape (4, 4)
    """
    self.transform = to_expr(transform)
check_shape() -> Tuple[int, ...]

Check that input is a 4×4 matrix and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (6,) for the twist vector

Raises:

Type Description
ValueError

If transform does not have shape (4, 4)
Source code in openscvx/symbolic/expr/lie/se3.py 
158
159
160
161
162
163
164
165
166
167
168
169
170
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 4×4 matrix and return output shape.

    Returns:
        tuple: Shape (6,) for the twist vector

    Raises:
        ValueError: If transform does not have shape (4, 4)
    """
    transform_shape = self.transform.check_shape()
    if transform_shape != (4, 4):
        raise ValueError(f"SE3Log expects transform with shape (4, 4), got {transform_shape}")
    return (6,)

SO3Exp

Bases: Expr

Exponential map from so(3) to SO(3) rotation matrix.

Maps a 3D rotation vector (axis-angle representation) to a 3×3 rotation matrix using the Rodrigues formula. Uses jaxlie for numerically robust implementation with proper handling of small angles.

The rotation vector ω has direction equal to the rotation axis and magnitude equal to the rotation angle in radians.

Attributes:

Name Type Description
omega

3D rotation vector with shape (3,)
Example

Create a rotation about the z-axis::

import openscvx as ox
import numpy as np

# 90 degree rotation about z
omega = ox.Constant(np.array([0, 0, np.pi/2]))
R = ox.lie.SO3Exp(omega)  # 3×3 rotation matrix

Parameterized rotation for optimization::

theta = ox.State("theta", shape=(1,))
axis = ox.Constant(np.array([0, 0, 1]))  # z-axis
R = ox.lie.SO3Exp(axis * theta)
See Also
  • SO3Log: Inverse operation (rotation matrix to rotation vector)
  • SE3Exp: Full rigid body transformation including translation
Source code in openscvx/symbolic/expr/lie/so3.py 
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
class SO3Exp(Expr):
    """Exponential map from so(3) to SO(3) rotation matrix.

    Maps a 3D rotation vector (axis-angle representation) to a 3×3 rotation
    matrix using the Rodrigues formula. Uses jaxlie for numerically robust
    implementation with proper handling of small angles.

    The rotation vector ω has direction equal to the rotation axis and
    magnitude equal to the rotation angle in radians.

    Attributes:
        omega: 3D rotation vector with shape (3,)

    Example:
        Create a rotation about the z-axis::

            import openscvx as ox
            import numpy as np

            # 90 degree rotation about z
            omega = ox.Constant(np.array([0, 0, np.pi/2]))
            R = ox.lie.SO3Exp(omega)  # 3×3 rotation matrix

        Parameterized rotation for optimization::

            theta = ox.State("theta", shape=(1,))
            axis = ox.Constant(np.array([0, 0, 1]))  # z-axis
            R = ox.lie.SO3Exp(axis * theta)

    See Also:
        - SO3Log: Inverse operation (rotation matrix to rotation vector)
        - SE3Exp: Full rigid body transformation including translation
    """

    def __init__(self, omega: Union[Expr, float, int, np.ndarray]):
        """Initialize SO3 exponential map.

        Args:
            omega: 3D rotation vector (axis × angle) with shape (3,)
        """
        self.omega = to_expr(omega)

    def children(self):
        return [self.omega]

    def canonicalize(self) -> "Expr":
        omega = self.omega.canonicalize()
        return SO3Exp(omega)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 3D vector and return output shape.

        Returns:
            tuple: Shape (3, 3) for the rotation matrix

        Raises:
            ValueError: If omega does not have shape (3,)
        """
        omega_shape = self.omega.check_shape()
        if omega_shape != (3,):
            raise ValueError(f"SO3Exp expects omega with shape (3,), got {omega_shape}")
        return (3, 3)

    def __repr__(self) -> str:
        return f"SO3Exp({self.omega!r})"
__init__(omega: Union[Expr, float, int, np.ndarray])

Initialize SO3 exponential map.

Parameters:

Name Type Description Default
omega Union[Expr, float, int, ndarray]

3D rotation vector (axis × angle) with shape (3,)

 required
Source code in openscvx/symbolic/expr/lie/so3.py 
51
52
53
54
55
56
57
def __init__(self, omega: Union[Expr, float, int, np.ndarray]):
    """Initialize SO3 exponential map.

    Args:
        omega: 3D rotation vector (axis × angle) with shape (3,)
    """
    self.omega = to_expr(omega)
check_shape() -> Tuple[int, ...]

Check that input is a 3D vector and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (3, 3) for the rotation matrix

Raises:

Type Description
ValueError

If omega does not have shape (3,)
Source code in openscvx/symbolic/expr/lie/so3.py 
66
67
68
69
70
71
72
73
74
75
76
77
78
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 3D vector and return output shape.

    Returns:
        tuple: Shape (3, 3) for the rotation matrix

    Raises:
        ValueError: If omega does not have shape (3,)
    """
    omega_shape = self.omega.check_shape()
    if omega_shape != (3,):
        raise ValueError(f"SO3Exp expects omega with shape (3,), got {omega_shape}")
    return (3, 3)

SO3Log

Bases: Expr

Logarithm map from SO(3) rotation matrix to so(3) rotation vector.

Maps a 3×3 rotation matrix to a 3D rotation vector (axis-angle representation). Uses jaxlie for numerically robust implementation.

The output rotation vector ω has direction equal to the rotation axis and magnitude equal to the rotation angle in radians.

Attributes:

Name Type Description
rotation

3×3 rotation matrix with shape (3, 3)
Example

Extract rotation vector from a rotation matrix::

import openscvx as ox

R = ox.State("R", shape=(3, 3))  # Rotation matrix state
omega = ox.lie.SO3Log(R)  # 3D rotation vector
See Also
  • SO3Exp: Inverse operation (rotation vector to rotation matrix)
  • SE3Log: Full rigid body transformation logarithm
Source code in openscvx/symbolic/expr/lie/so3.py 
 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
class SO3Log(Expr):
    """Logarithm map from SO(3) rotation matrix to so(3) rotation vector.

    Maps a 3×3 rotation matrix to a 3D rotation vector (axis-angle
    representation). Uses jaxlie for numerically robust implementation.

    The output rotation vector ω has direction equal to the rotation axis
    and magnitude equal to the rotation angle in radians.

    Attributes:
        rotation: 3×3 rotation matrix with shape (3, 3)

    Example:
        Extract rotation vector from a rotation matrix::

            import openscvx as ox

            R = ox.State("R", shape=(3, 3))  # Rotation matrix state
            omega = ox.lie.SO3Log(R)  # 3D rotation vector

    See Also:
        - SO3Exp: Inverse operation (rotation vector to rotation matrix)
        - SE3Log: Full rigid body transformation logarithm
    """

    def __init__(self, rotation: Union[Expr, float, int, np.ndarray]):
        """Initialize SO3 logarithm map.

        Args:
            rotation: 3×3 rotation matrix with shape (3, 3)
        """
        self.rotation = to_expr(rotation)

    def children(self):
        return [self.rotation]

    def canonicalize(self) -> "Expr":
        rotation = self.rotation.canonicalize()
        return SO3Log(rotation)

    def check_shape(self) -> Tuple[int, ...]:
        """Check that input is a 3×3 matrix and return output shape.

        Returns:
            tuple: Shape (3,) for the rotation vector

        Raises:
            ValueError: If rotation does not have shape (3, 3)
        """
        rotation_shape = self.rotation.check_shape()
        if rotation_shape != (3, 3):
            raise ValueError(f"SO3Log expects rotation with shape (3, 3), got {rotation_shape}")
        return (3,)

    def __repr__(self) -> str:
        return f"SO3Log({self.rotation!r})"
__init__(rotation: Union[Expr, float, int, np.ndarray])

Initialize SO3 logarithm map.

Parameters:

Name Type Description Default
rotation Union[Expr, float, int, ndarray]

3×3 rotation matrix with shape (3, 3)

 required
Source code in openscvx/symbolic/expr/lie/so3.py 
109
110
111
112
113
114
115
def __init__(self, rotation: Union[Expr, float, int, np.ndarray]):
    """Initialize SO3 logarithm map.

    Args:
        rotation: 3×3 rotation matrix with shape (3, 3)
    """
    self.rotation = to_expr(rotation)
check_shape() -> Tuple[int, ...]

Check that input is a 3×3 matrix and return output shape.

Returns:

Name Type Description
tuple Tuple[int, ...]

Shape (3,) for the rotation vector

Raises:

Type Description
ValueError

If rotation does not have shape (3, 3)
Source code in openscvx/symbolic/expr/lie/so3.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
def check_shape(self) -> Tuple[int, ...]:
    """Check that input is a 3×3 matrix and return output shape.

    Returns:
        tuple: Shape (3,) for the rotation vector

    Raises:
        ValueError: If rotation does not have shape (3, 3)
    """
    rotation_shape = self.rotation.check_shape()
    if rotation_shape != (3, 3):
        raise ValueError(f"SO3Log expects rotation with shape (3, 3), got {rotation_shape}")
    return (3,)