Skip to content

Formulas: screws.symbolic

Symbolic twins of the chapter functions, in SymPy: the product of exponentials, the Jacobians, the adjoint and Newton-Euler dynamics as formulas in the joint variables.

import screws.symbolic as sym, or just screws.symbolic, imported on first use. Each function has the name and the arguments of its numerical twin (sym.fk_space(M, S, theta) beside screws.fk_space(M, S, theta), MR's names too: sym.FKinSpace) and returns SymPy matrices; a vector is a column. Two differences:

  • theta may be left out (or None): the joint variables are then the real symbols theta1 ... thetan, and dtheta, ddtheta in the dynamics thetadot1 ..., thetaddot1 ... (see joint_symbols).
  • A Python float is read as sympy.nsimplify(x, rational=True) reads it: as the decimal it prints as (0.425 is 17/40, 0.1 + 0.2 is 3/10), so a Robot's float arrays give clean formulas. np.pi / 2 is read as 1.5707963267949, so write sympy.pi / 2 where pi/2 is meant: SymPy objects pass through untouched (a sympy.Float stays a float).

Results are simplified (sympy.simplify, entry by entry) unless simplify=False, or by the function given instead (simplify=sympy.trigsimp is sometimes quicker). The kinematics of a six-joint arm take seconds, its gravity forces half a minute, its mass matrix far longer; and a robot read from floats that carry round-off simplifies slowly or not at all, which is what Robot.symbolic(decimals=...) is for.

Not here, being numerical by nature: the logarithms, inverse kinematics, manipulability, projections and distances, time stepping and trajectories. MR chapters 3, 4, 5 and 8; notes 3.5-3.7, 4.1-4.2, 5.1, 8.3.

SymbolicRobot dataclass

A serial chain whose description may hold symbols: the symbolic twin of screws.Robot.

M: 4x4 home configuration of {b} in {s}. S: 6xn space-frame screw axes, one per column. link_frames, link_inertias: MR's M_{i-1,i} and G_i, or None. gravity: the gravity vector in {s}, (0, 0, -g) with g a positive symbol unless given. Every method takes the joint values as optional (theta1 ... thetan when left out) and simplify=.

Build one from a numerical robot with Robot.symbolic() (its floats read as decimals), or by hand with from_screw_axes. MR 4.1, 8.3; notes chapter 4.

Source code in screws/symbolic.py 
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
@dataclass(frozen=True, eq=False)
class SymbolicRobot:
    """A serial chain whose description may hold symbols: the symbolic twin of screws.Robot.

    M: 4x4 home configuration of {b} in {s}. S: 6xn space-frame screw axes, one per
    column. link_frames, link_inertias: MR's M_{i-1,i} and G_i, or None. gravity: the
    gravity vector in {s}, (0, 0, -g) with g a positive symbol unless given. Every method
    takes the joint values as optional (theta1 ... thetan when left out) and simplify=.

    Build one from a numerical robot with Robot.symbolic() (its floats read as decimals), or by
    hand with from_screw_axes. MR 4.1, 8.3; notes chapter 4.
    """

    name: str
    M: sp.ImmutableMatrix
    S: sp.ImmutableMatrix
    joint_types: tuple[str, ...]
    joint_names: tuple[str, ...]
    link_frames: tuple[sp.ImmutableMatrix, ...] | None = None
    link_inertias: tuple[sp.ImmutableMatrix, ...] | None = None
    gravity: sp.ImmutableMatrix = field(
        default_factory=lambda: sp.ImmutableMatrix([0, 0, -sp.Symbol("g", positive=True)])
    )

    def __post_init__(self):
        S = sp.ImmutableMatrix(_screws(self.S))
        n = S.shape[1]
        if len(self.joint_types) != n or len(self.joint_names) != n:
            raise ValueError("joint_types and joint_names must have one entry per joint")
        object.__setattr__(self, "M", _frozen(self.M, (4, 4)))
        object.__setattr__(self, "S", S)
        object.__setattr__(self, "joint_types", tuple(self.joint_types))
        object.__setattr__(self, "joint_names", tuple(self.joint_names))
        object.__setattr__(self, "gravity", sp.ImmutableMatrix(_vector(self.gravity, 3)))
        if self.link_frames is not None:
            object.__setattr__(self, "link_frames", tuple(_frozen(m, (4, 4)) for m in self.link_frames))
        if self.link_inertias is not None:
            object.__setattr__(
                self, "link_inertias", tuple(_frozen(G, (6, 6)) for G in self.link_inertias)
            )

    @classmethod
    def from_robot(cls, robot, *, decimals: int | None = None) -> SymbolicRobot:
        """The symbolic twin of a screws.Robot, its floats read as decimals (see exact for
        decimals); Robot.symbolic calls this.

        Warns when, without decimals, the kinematics hold a number below 1e-5 that is not
        zero: almost always the round-off of a URDF's pi/2, which the formulas then carry
        and which keeps simplify from cancelling anything.
        """

        def ex(a):
            return exact(a, decimals=decimals)

        if decimals is None:
            geometry = [robot.M, robot.S, *(robot.link_frames or ())]
            tiny = [x for a in geometry for x in np.ravel(a) if 0 < abs(x) < 1e-5]
            if tiny:
                warnings.warn(
                    f"{robot.name}: {tiny[0]:.1e} in its kinematics is probably round-off (a "
                    "URDF's pi/2, say); its formulas will carry it and simplify will crawl. "
                    "Pass decimals= (decimals=6 rounds to the micrometre).",
                    stacklevel=3,
                )

        return cls(
            name=robot.name,
            M=ex(robot.M),
            S=ex(robot.S),
            joint_types=robot.joint_types,
            joint_names=robot.joint_names,
            link_frames=None if robot.link_frames is None else tuple(map(ex, robot.link_frames)),
            link_inertias=None if robot.link_inertias is None else tuple(map(ex, robot.link_inertias)),
            gravity=ex(robot.gravity),
        )

    @classmethod
    def from_screw_axes(
        cls, M, axes, *, name="robot", joint_types=None, joint_names=None, gravity=None
    ) -> SymbolicRobot:
        """Build a robot from M and a sequence of space-frame screw axes, one 6-vector per joint.

        As Robot.from_screw_axes: joint types default to reading each axis (omega = 0 means
        prismatic). Notes 4.1.
        """
        if isinstance(axes, (sp.MatrixBase, np.ndarray)) and len(axes.shape) == 2 and min(axes.shape) > 1:
            raise TypeError(
                "axes must be a sequence of 6-vectors, one per joint; "
                "a 6xn matrix is ambiguous when n = 6 (pass list of its columns)"
            )
        vectors = [_vector(a, 6) for a in axes]
        n = len(vectors)
        if joint_types is None:
            joint_types = tuple(
                "prismatic" if all(sp.simplify(x) == 0 for x in v[:3, 0]) else "revolute"
                for v in vectors
            )
        if joint_names is None:
            joint_names = tuple(f"joint{i + 1}" for i in range(n))
        extra = {} if gravity is None else {"gravity": gravity}
        return cls(
            name=name,
            M=M,
            S=sp.Matrix.hstack(*vectors),
            joint_types=tuple(joint_types),
            joint_names=tuple(joint_names),
            **extra,
        )

    @property
    def n(self) -> int:
        """The number of joints."""
        return self.S.shape[1]

    @property
    def theta(self) -> tuple[sp.Symbol, ...]:
        """theta1 ... thetan, the joint variables the methods use when none are passed."""
        return joint_symbols(self.n)

    @cached_property
    def B(self) -> sp.Matrix:
        """The body-frame screw axes at home, B = [Ad_{M^{-1}}] S. MR 4.1.3; notes 4.2."""
        return _done(adjoint(transform_inv(self.M)) * self.S, True)

    def fk(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """T_sb(theta), by the space form. Notes 4.1."""
        return fk_space(self.M, self.S, theta, simplify=simplify)

    def jacobian_space(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """J_s(theta). MR 5.1.1; notes 5.1."""
        return jacobian_space(self.S, theta, simplify=simplify)

    def jacobian_body(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """J_b(theta). MR 5.1.2; notes 5.1."""
        return jacobian_body(self.B, theta, simplify=simplify)

    def _dyn(self):
        if self.link_inertias is None or self.link_frames is None:
            from .robot import MissingInertias

            raise MissingInertias(
                f"{self.name} has no link inertias; build it from a robot that has them, "
                "or call with_inertias(link_frames, link_inertias)"
            )
        return self.link_frames, self.link_inertias, self.S

    def inverse_dynamics(
        self, theta=None, dtheta=None, ddtheta=None, F_tip=None, *, simplify: Simplify = True
    ) -> sp.Matrix:
        """tau = M(theta) ddtheta + c(theta, dtheta) + g(theta) + J^T F_tip. MR 8.3."""
        return inverse_dynamics(
            theta, dtheta, ddtheta, self.gravity, F_tip, *self._dyn(), simplify=simplify
        )

    def mass_matrix(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """M(theta). MR 8.3."""
        return mass_matrix(theta, *self._dyn(), simplify=simplify)

    def velocity_quadratic_forces(self, theta=None, dtheta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """c(theta, dtheta). MR 8.3."""
        return velocity_quadratic_forces(theta, dtheta, *self._dyn(), simplify=simplify)

    def gravity_forces(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
        """g(theta), with this robot's gravity. MR 8.3."""
        return gravity_forces(theta, self.gravity, *self._dyn(), simplify=simplify)

    def end_effector_forces(self, theta=None, F_tip=None, *, simplify: Simplify = True) -> sp.Matrix:
        """J^T(theta) F_tip; F_tip defaults to the symbols m_x ... f_z. MR 8.3."""
        return end_effector_forces(theta, F_tip, *self._dyn(), simplify=simplify)

    def forward_dynamics(
        self, theta=None, dtheta=None, tau=None, F_tip=None, *, simplify: Simplify = True
    ) -> sp.Matrix:
        """ddtheta from tau (tau1 ... taun when left out), with this robot's gravity. MR 8.5."""
        return forward_dynamics(
            theta, dtheta, tau, self.gravity, F_tip, *self._dyn(), simplify=simplify
        )

    def with_inertias(self, link_frames, link_inertias) -> SymbolicRobot:
        """A copy carrying MR's M_{i-1,i} list and G_i list."""
        return replace(self, link_frames=tuple(link_frames), link_inertias=tuple(link_inertias))

    def with_gravity(self, g) -> SymbolicRobot:
        """A copy with a different gravity vector."""
        return replace(self, gravity=g)

n property 

n

The number of joints.

theta property 

theta

theta1 ... thetan, the joint variables the methods use when none are passed.

B cached property 

B

The body-frame screw axes at home, B = [Ad_{M^{-1}}] S. MR 4.1.3; notes 4.2.

from_robot classmethod 

from_robot(robot, *, decimals=None)

The symbolic twin of a screws.Robot, its floats read as decimals (see exact for decimals); Robot.symbolic calls this.

Warns when, without decimals, the kinematics hold a number below 1e-5 that is not zero: almost always the round-off of a URDF's pi/2, which the formulas then carry and which keeps simplify from cancelling anything.

Source code in screws/symbolic.py 
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
@classmethod
def from_robot(cls, robot, *, decimals: int | None = None) -> SymbolicRobot:
    """The symbolic twin of a screws.Robot, its floats read as decimals (see exact for
    decimals); Robot.symbolic calls this.

    Warns when, without decimals, the kinematics hold a number below 1e-5 that is not
    zero: almost always the round-off of a URDF's pi/2, which the formulas then carry
    and which keeps simplify from cancelling anything.
    """

    def ex(a):
        return exact(a, decimals=decimals)

    if decimals is None:
        geometry = [robot.M, robot.S, *(robot.link_frames or ())]
        tiny = [x for a in geometry for x in np.ravel(a) if 0 < abs(x) < 1e-5]
        if tiny:
            warnings.warn(
                f"{robot.name}: {tiny[0]:.1e} in its kinematics is probably round-off (a "
                "URDF's pi/2, say); its formulas will carry it and simplify will crawl. "
                "Pass decimals= (decimals=6 rounds to the micrometre).",
                stacklevel=3,
            )

    return cls(
        name=robot.name,
        M=ex(robot.M),
        S=ex(robot.S),
        joint_types=robot.joint_types,
        joint_names=robot.joint_names,
        link_frames=None if robot.link_frames is None else tuple(map(ex, robot.link_frames)),
        link_inertias=None if robot.link_inertias is None else tuple(map(ex, robot.link_inertias)),
        gravity=ex(robot.gravity),
    )

from_screw_axes classmethod 

from_screw_axes(
    M,
    axes,
    *,
    name="robot",
    joint_types=None,
    joint_names=None,
    gravity=None,
)

Build a robot from M and a sequence of space-frame screw axes, one 6-vector per joint.

As Robot.from_screw_axes: joint types default to reading each axis (omega = 0 means prismatic). Notes 4.1.

Source code in screws/symbolic.py 
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
@classmethod
def from_screw_axes(
    cls, M, axes, *, name="robot", joint_types=None, joint_names=None, gravity=None
) -> SymbolicRobot:
    """Build a robot from M and a sequence of space-frame screw axes, one 6-vector per joint.

    As Robot.from_screw_axes: joint types default to reading each axis (omega = 0 means
    prismatic). Notes 4.1.
    """
    if isinstance(axes, (sp.MatrixBase, np.ndarray)) and len(axes.shape) == 2 and min(axes.shape) > 1:
        raise TypeError(
            "axes must be a sequence of 6-vectors, one per joint; "
            "a 6xn matrix is ambiguous when n = 6 (pass list of its columns)"
        )
    vectors = [_vector(a, 6) for a in axes]
    n = len(vectors)
    if joint_types is None:
        joint_types = tuple(
            "prismatic" if all(sp.simplify(x) == 0 for x in v[:3, 0]) else "revolute"
            for v in vectors
        )
    if joint_names is None:
        joint_names = tuple(f"joint{i + 1}" for i in range(n))
    extra = {} if gravity is None else {"gravity": gravity}
    return cls(
        name=name,
        M=M,
        S=sp.Matrix.hstack(*vectors),
        joint_types=tuple(joint_types),
        joint_names=tuple(joint_names),
        **extra,
    )

fk 

fk(theta=None, *, simplify=True)

T_sb(theta), by the space form. Notes 4.1.

Source code in screws/symbolic.py 
701
702
703
def fk(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """T_sb(theta), by the space form. Notes 4.1."""
    return fk_space(self.M, self.S, theta, simplify=simplify)

jacobian_space 

jacobian_space(theta=None, *, simplify=True)

J_s(theta). MR 5.1.1; notes 5.1.

Source code in screws/symbolic.py 
705
706
707
def jacobian_space(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """J_s(theta). MR 5.1.1; notes 5.1."""
    return jacobian_space(self.S, theta, simplify=simplify)

jacobian_body 

jacobian_body(theta=None, *, simplify=True)

J_b(theta). MR 5.1.2; notes 5.1.

Source code in screws/symbolic.py 
709
710
711
def jacobian_body(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """J_b(theta). MR 5.1.2; notes 5.1."""
    return jacobian_body(self.B, theta, simplify=simplify)

inverse_dynamics 

inverse_dynamics(
    theta=None,
    dtheta=None,
    ddtheta=None,
    F_tip=None,
    *,
    simplify=True,
)

tau = M(theta) ddtheta + c(theta, dtheta) + g(theta) + J^T F_tip. MR 8.3.

Source code in screws/symbolic.py 
723
724
725
726
727
728
729
def inverse_dynamics(
    self, theta=None, dtheta=None, ddtheta=None, F_tip=None, *, simplify: Simplify = True
) -> sp.Matrix:
    """tau = M(theta) ddtheta + c(theta, dtheta) + g(theta) + J^T F_tip. MR 8.3."""
    return inverse_dynamics(
        theta, dtheta, ddtheta, self.gravity, F_tip, *self._dyn(), simplify=simplify
    )

mass_matrix 

mass_matrix(theta=None, *, simplify=True)

M(theta). MR 8.3.

Source code in screws/symbolic.py 
731
732
733
def mass_matrix(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """M(theta). MR 8.3."""
    return mass_matrix(theta, *self._dyn(), simplify=simplify)

velocity_quadratic_forces 

velocity_quadratic_forces(
    theta=None, dtheta=None, *, simplify=True
)

c(theta, dtheta). MR 8.3.

Source code in screws/symbolic.py 
735
736
737
def velocity_quadratic_forces(self, theta=None, dtheta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """c(theta, dtheta). MR 8.3."""
    return velocity_quadratic_forces(theta, dtheta, *self._dyn(), simplify=simplify)

gravity_forces 

gravity_forces(theta=None, *, simplify=True)

g(theta), with this robot's gravity. MR 8.3.

Source code in screws/symbolic.py 
739
740
741
def gravity_forces(self, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """g(theta), with this robot's gravity. MR 8.3."""
    return gravity_forces(theta, self.gravity, *self._dyn(), simplify=simplify)

end_effector_forces 

end_effector_forces(
    theta=None, F_tip=None, *, simplify=True
)

J^T(theta) F_tip; F_tip defaults to the symbols m_x ... f_z. MR 8.3.

Source code in screws/symbolic.py 
743
744
745
def end_effector_forces(self, theta=None, F_tip=None, *, simplify: Simplify = True) -> sp.Matrix:
    """J^T(theta) F_tip; F_tip defaults to the symbols m_x ... f_z. MR 8.3."""
    return end_effector_forces(theta, F_tip, *self._dyn(), simplify=simplify)

forward_dynamics 

forward_dynamics(
    theta=None,
    dtheta=None,
    tau=None,
    F_tip=None,
    *,
    simplify=True,
)

ddtheta from tau (tau1 ... taun when left out), with this robot's gravity. MR 8.5.

Source code in screws/symbolic.py 
747
748
749
750
751
752
753
def forward_dynamics(
    self, theta=None, dtheta=None, tau=None, F_tip=None, *, simplify: Simplify = True
) -> sp.Matrix:
    """ddtheta from tau (tau1 ... taun when left out), with this robot's gravity. MR 8.5."""
    return forward_dynamics(
        theta, dtheta, tau, self.gravity, F_tip, *self._dyn(), simplify=simplify
    )

with_inertias 

with_inertias(link_frames, link_inertias)

A copy carrying MR's M_{i-1,i} list and G_i list.

Source code in screws/symbolic.py 
755
756
757
def with_inertias(self, link_frames, link_inertias) -> SymbolicRobot:
    """A copy carrying MR's M_{i-1,i} list and G_i list."""
    return replace(self, link_frames=tuple(link_frames), link_inertias=tuple(link_inertias))

with_gravity 

with_gravity(g)

A copy with a different gravity vector.

Source code in screws/symbolic.py 
759
760
761
def with_gravity(self, g) -> SymbolicRobot:
    """A copy with a different gravity vector."""
    return replace(self, gravity=g)

exact 

exact(a, *, decimals=None)

a as an exact SymPy object: a scalar, or a Matrix for anything array-like.

A float is read by sympy.nsimplify(x, rational=True), as the decimal it prints as (0.1 is 1/10, 1/3 is 1/3, np.pi / 2 is 15707963267949/10000000000000). decimals=k rounds every float to k places first: a URDF that writes pi/2 as 1.5708 leaves cos 1.5708 = -3.7e-6 where 0 was meant, and decimals=4 drops it. SymPy objects are left as they are, so sympy.pi / 2 stays pi/2. A 1-D sequence becomes a column. Addition, not in the MR library.

Source code in screws/symbolic.py 
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
def exact(a, *, decimals: int | None = None):
    """a as an exact SymPy object: a scalar, or a Matrix for anything array-like.

    A float is read by sympy.nsimplify(x, rational=True), as the decimal it prints as (0.1
    is 1/10, 1/3 is 1/3, np.pi / 2 is 15707963267949/10000000000000). decimals=k rounds
    every float to k places first: a URDF that writes pi/2 as 1.5708 leaves cos 1.5708 =
    -3.7e-6 where 0 was meant, and decimals=4 drops it. SymPy objects are left as they
    are, so sympy.pi / 2 stays pi/2. A 1-D sequence becomes a column. Addition, not in
    the MR library.
    """
    if isinstance(a, sp.MatrixBase):
        return sp.Matrix(a).applyfunc(lambda x: _scalar(x, decimals))
    if isinstance(a, (list, tuple, np.ndarray)):
        arr = np.asarray(a, dtype=object)
        if arr.ndim == 1:
            return sp.Matrix([_scalar(x, decimals) for x in arr])
        if arr.ndim == 2:
            return sp.Matrix([[_scalar(x, decimals) for x in row] for row in arr])
        raise ValueError(f"expected a scalar, vector or matrix; got shape {arr.shape}")
    return _scalar(a, decimals)

joint_symbols 

joint_symbols(n, name='theta')

The real symbols name1 ... namen, a tuple: theta1 ... thetan by default.

SymPy prints a trailing "dot" or "ddot" as an accent, so joint_symbols(2, "thetadot") displays as theta-dot-1, theta-dot-2. Addition, not in the MR library.

Source code in screws/symbolic.py 
146
147
148
149
150
151
152
def joint_symbols(n: int, name: str = "theta") -> tuple[sp.Symbol, ...]:
    """The real symbols name1 ... namen, a tuple: theta1 ... thetan by default.

    SymPy prints a trailing "dot" or "ddot" as an accent, so joint_symbols(2, "thetadot")
    displays as theta-dot-1, theta-dot-2. Addition, not in the MR library.
    """
    return tuple(sp.Symbol(f"{name}{i}", real=True) for i in range(1, n + 1))

rot_inv 

rot_inv(R)

The inverse of a rotation matrix, its transpose. MR 3.2.1; notes 3.3.

Source code in screws/symbolic.py 
158
159
160
def rot_inv(R) -> sp.Matrix:
    """The inverse of a rotation matrix, its transpose. MR 3.2.1; notes 3.3."""
    return _matrix(R, (3, 3)).T

vec_to_so3 

vec_to_so3(omega)

[omega], the 3x3 skew-symmetric matrix with [omega] y = omega x y. MR eq. 3.30; notes 3.5.

Source code in screws/symbolic.py 
163
164
165
166
def vec_to_so3(omega) -> sp.Matrix:
    """[omega], the 3x3 skew-symmetric matrix with [omega] y = omega x y. MR eq. 3.30; notes 3.5."""
    w = _vector(omega, 3)
    return sp.Matrix([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])

so3_to_vec 

so3_to_vec(so3mat)

The 3-vector omega from its skew-symmetric matrix [omega]. MR 3.2.3.1; notes 3.5.

Source code in screws/symbolic.py 
169
170
171
172
def so3_to_vec(so3mat) -> sp.Matrix:
    """The 3-vector omega from its skew-symmetric matrix [omega]. MR 3.2.3.1; notes 3.5."""
    m = _matrix(so3mat, (3, 3))
    return sp.Matrix([m[2, 1], m[0, 2], m[1, 0]])

exp3 

exp3(so3mat, *, simplify=True)

e^{[omega] theta}: the rotation matrix from the so(3) matrix [omega] theta.

Rodrigues' formula, I + sin(theta) [omega] + (1 - cos theta) [omega]^2, written in [omega] theta itself so the angle need not be passed separately. MR eq. 3.51; notes 3.6.

Source code in screws/symbolic.py 
207
208
209
210
211
212
213
214
215
216
217
218
def exp3(so3mat, *, simplify: Simplify = True) -> sp.Matrix:
    """e^{[omega] theta}: the rotation matrix from the so(3) matrix [omega] theta.

    Rodrigues' formula, I + sin(theta) [omega] + (1 - cos theta) [omega]^2, written in
    [omega] theta itself so the angle need not be passed separately. MR eq. 3.51; notes 3.6.
    """
    W = _matrix(so3mat, (3, 3))
    r = _angle(so3_to_vec(W))
    if r == 0:
        return sp.eye(3)
    R = sp.eye(3) + sp.sin(r) / r * W + (1 - sp.cos(r)) / r**2 * (W * W)
    return _done(R, simplify)

rot 

rot(omega_hat, theta, *, simplify=True)

Rot(omega_hat, theta): the rotation about the unit axis omega_hat by theta.

Addition, not in the MR library. MR 3.2.3.3; notes 3.6.

Source code in screws/symbolic.py 
221
222
223
224
225
226
def rot(omega_hat, theta, *, simplify: Simplify = True) -> sp.Matrix:
    """Rot(omega_hat, theta): the rotation about the unit axis omega_hat by theta.

    Addition, not in the MR library. MR 3.2.3.3; notes 3.6.
    """
    return exp3(vec_to_so3(omega_hat) * _scalar(theta), simplify=simplify)

rotation_from_rpy 

rotation_from_rpy(roll, pitch, yaw, *, simplify=True)

URDF fixed-axis roll-pitch-yaw: Rot(z, yaw) Rot(y, pitch) Rot(x, roll).

Addition, not in the MR library. MR Appendix B.1; notes 4.3.

Source code in screws/symbolic.py 
229
230
231
232
233
234
235
def rotation_from_rpy(roll, pitch, yaw, *, simplify: Simplify = True) -> sp.Matrix:
    """URDF fixed-axis roll-pitch-yaw: Rot(z, yaw) Rot(y, pitch) Rot(x, roll).

    Addition, not in the MR library. MR Appendix B.1; notes 4.3.
    """
    R = rot([0, 0, 1], yaw, simplify=False) * rot([0, 1, 0], pitch, simplify=False)
    return _done(R * rot([1, 0, 0], roll, simplify=False), simplify)

rp_to_transform 

rp_to_transform(R, p)

The homogeneous transformation T = (R, p). MR eq. 3.62; notes 3.4.

Source code in screws/symbolic.py 
241
242
243
244
245
246
def rp_to_transform(R, p) -> sp.Matrix:
    """The homogeneous transformation T = (R, p). MR eq. 3.62; notes 3.4."""
    T = sp.eye(4)
    T[:3, :3] = _matrix(R, (3, 3))
    T[:3, 3] = _vector(p, 3)
    return T

transform_to_rp 

transform_to_rp(T)

(R, p) from the homogeneous transformation T. MR 3.3.1; notes 3.4.

Source code in screws/symbolic.py 
249
250
251
252
def transform_to_rp(T) -> tuple[sp.Matrix, sp.Matrix]:
    """(R, p) from the homogeneous transformation T. MR 3.3.1; notes 3.4."""
    T = _matrix(T, (4, 4))
    return T[:3, :3], T[:3, 3]

transform_inv 

transform_inv(T)

T^{-1} = (R^T, -R^T p). MR eq. 3.64; notes 3.4.

Source code in screws/symbolic.py 
255
256
257
258
def transform_inv(T) -> sp.Matrix:
    """T^{-1} = (R^T, -R^T p). MR eq. 3.64; notes 3.4."""
    R, p = transform_to_rp(T)
    return rp_to_transform(R.T, -R.T * p)

trans 

trans(p)

Trans(p): the pure translation by p. Addition, not in the MR library. MR 3.3.1.

Source code in screws/symbolic.py 
261
262
263
def trans(p) -> sp.Matrix:
    """Trans(p): the pure translation by p. Addition, not in the MR library. MR 3.3.1."""
    return rp_to_transform(sp.eye(3), p)

vec_to_se3 

vec_to_se3(V)

[V], the 4x4 se(3) matrix of the twist V = (omega, v). MR eq. 3.72; notes 3.5.

Source code in screws/symbolic.py 
266
267
268
269
270
271
272
def vec_to_se3(V) -> sp.Matrix:
    """[V], the 4x4 se(3) matrix of the twist V = (omega, v). MR eq. 3.72; notes 3.5."""
    V = _vector(V, 6)
    m = sp.zeros(4, 4)
    m[:3, :3] = vec_to_so3(V[:3, 0])
    m[:3, 3] = V[3:, 0]
    return m

se3_to_vec 

se3_to_vec(se3mat)

The twist V = (omega, v) from its se(3) matrix [V]. MR 3.3.2.1; notes 3.5.

Source code in screws/symbolic.py 
275
276
277
278
def se3_to_vec(se3mat) -> sp.Matrix:
    """The twist V = (omega, v) from its se(3) matrix [V]. MR 3.3.2.1; notes 3.5."""
    m = _matrix(se3mat, (4, 4))
    return sp.Matrix.vstack(so3_to_vec(m[:3, :3]), m[:3, 3])

adjoint 

adjoint(T)

[Ad_T], the 6x6 matrix that changes the frame of a twist. MR definition 3.20; notes 3.5.

Source code in screws/symbolic.py 
281
282
283
284
285
286
287
288
def adjoint(T) -> sp.Matrix:
    """[Ad_T], the 6x6 matrix that changes the frame of a twist. MR definition 3.20; notes 3.5."""
    R, p = transform_to_rp(T)
    A = sp.zeros(6, 6)
    A[:3, :3] = R
    A[3:, :3] = vec_to_so3(p) * R
    A[3:, 3:] = R
    return A

screw_axis 

screw_axis(q, s_hat, h)

The screw axis S = (s_hat, -s_hat x q + h s_hat) through q, direction s_hat, pitch h.

Finite pitch only; use prismatic_axis for h = infinity. MR eq. 3.77; notes 3.5.

Source code in screws/symbolic.py 
291
292
293
294
295
296
297
def screw_axis(q, s_hat, h) -> sp.Matrix:
    """The screw axis S = (s_hat, -s_hat x q + h s_hat) through q, direction s_hat, pitch h.

    Finite pitch only; use prismatic_axis for h = infinity. MR eq. 3.77; notes 3.5.
    """
    q, s = _vector(q, 3), _vector(s_hat, 3)
    return sp.Matrix.vstack(s, q.cross(s) + _scalar(h) * s)

revolute_axis 

revolute_axis(q, s_hat)

Zero-pitch screw axis (s_hat, -s_hat x q) through the point q.

Addition, not in the MR library; the joint-frame derivation of notes 4.1.

Source code in screws/symbolic.py 
300
301
302
303
304
305
306
def revolute_axis(q, s_hat) -> sp.Matrix:
    """Zero-pitch screw axis (s_hat, -s_hat x q) through the point q.

    Addition, not in the MR library; the joint-frame derivation of notes 4.1.
    """
    q, s = _vector(q, 3), _vector(s_hat, 3)
    return sp.Matrix.vstack(s, -s.cross(q))

prismatic_axis 

prismatic_axis(s_hat)

Infinite-pitch screw axis (0, s_hat). Addition, not in the MR library. MR 3.3.2.2.

Source code in screws/symbolic.py 
309
310
311
def prismatic_axis(s_hat) -> sp.Matrix:
    """Infinite-pitch screw axis (0, s_hat). Addition, not in the MR library. MR 3.3.2.2."""
    return sp.Matrix.vstack(sp.zeros(3, 1), _vector(s_hat, 3))

exp6 

exp6(se3mat, *, simplify=True)

e^{[S] theta}: the homogeneous transformation from the se(3) matrix [S] theta.

MR proposition 3.25, (e^{[omega] theta}, G(theta) v), with G(theta) v written in [omega] theta and v theta; a pure translation when omega = 0. MR 3.3.3; notes 3.7.

Source code in screws/symbolic.py 
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
def exp6(se3mat, *, simplify: Simplify = True) -> sp.Matrix:
    """e^{[S] theta}: the homogeneous transformation from the se(3) matrix [S] theta.

    MR proposition 3.25, (e^{[omega] theta}, G(theta) v), with G(theta) v written in
    [omega] theta and v theta; a pure translation when omega = 0. MR 3.3.3; notes 3.7.
    """
    m = _matrix(se3mat, (4, 4))
    W, w = m[:3, :3], m[:3, 3]
    r = _angle(so3_to_vec(W))
    if r == 0:
        return rp_to_transform(sp.eye(3), w)
    W2 = W * W
    R = sp.eye(3) + sp.sin(r) / r * W + (1 - sp.cos(r)) / r**2 * W2
    p = w + (1 - sp.cos(r)) / r**2 * (W * w) + (r - sp.sin(r)) / r**3 * (W2 * w)
    return _done(rp_to_transform(R, p), simplify)

fk_space 

fk_space(M, S, theta=None, *, simplify=True)

Space form of the product of exponentials: e^{[S1]t1} ... e^{[Sn]tn} M.

S is 6xn, one screw axis per column, in {s} at the home position; theta defaults to theta1 ... thetan. MR eq. 4.14; notes 4.1.

Source code in screws/symbolic.py 
369
370
371
372
373
374
375
376
377
378
379
380
def fk_space(M, S, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """Space form of the product of exponentials: e^{[S1]t1} ... e^{[Sn]tn} M.

    S is 6xn, one screw axis per column, in {s} at the home position; theta defaults to
    theta1 ... thetan. MR eq. 4.14; notes 4.1.
    """
    S = _screws(S)
    theta = _joints(theta, S.shape[1])
    T = _matrix(M, (4, 4))
    for i in range(len(theta) - 1, -1, -1):
        T = _exp(S[:, i], theta[i]) * T
    return _done(T, simplify)

fk_body 

fk_body(M, B, theta=None, *, simplify=True)

Body form of the product of exponentials: M e^{[B1]t1} ... e^{[Bn]tn}.

B is 6xn, one screw axis per column, in {b} at the home position. MR eq. 4.16; notes 4.2.

Source code in screws/symbolic.py 
383
384
385
386
387
388
389
390
391
392
393
def fk_body(M, B, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """Body form of the product of exponentials: M e^{[B1]t1} ... e^{[Bn]tn}.

    B is 6xn, one screw axis per column, in {b} at the home position. MR eq. 4.16; notes 4.2.
    """
    B = _screws(B)
    theta = _joints(theta, B.shape[1])
    T = _matrix(M, (4, 4))
    for i in range(len(theta)):
        T = T * _exp(B[:, i], theta[i])
    return _done(T, simplify)

joint_frames 

joint_frames(M_joints, S, theta=None, *, simplify=True)

Every joint frame T_{0i}(theta) = e^{[S1]t1} ... e^{[Si]ti} M_joints[i-1], i = 1..n.

Addition, not in the MR library; the space form truncated after each joint. Notes 4.1.

Source code in screws/symbolic.py 
396
397
398
399
400
401
402
403
404
405
406
407
408
def joint_frames(M_joints, S, theta=None, *, simplify: Simplify = True) -> list[sp.Matrix]:
    """Every joint frame T_{0i}(theta) = e^{[S1]t1} ... e^{[Si]ti} M_joints[i-1], i = 1..n.

    Addition, not in the MR library; the space form truncated after each joint. Notes 4.1.
    """
    S = _screws(S)
    theta = _joints(theta, S.shape[1])
    T = sp.eye(4)
    out = []
    for i, Mi in enumerate(M_joints):
        T = T * _exp(S[:, i], theta[i])
        out.append(_done(T * _matrix(Mi, (4, 4)), simplify))
    return out

jacobian_space 

jacobian_space(S, theta=None, *, simplify=True)

The space Jacobian J_s(theta), 6xn, column i = [Ad_{e^{[S1]t1}...e^{[S_{i-1}]t_{i-1}}}] S_i.

MR eq. 5.11; notes 5.1.

Source code in screws/symbolic.py 
411
412
413
414
415
416
417
418
419
420
421
422
423
def jacobian_space(S, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """The space Jacobian J_s(theta), 6xn, column i = [Ad_{e^{[S1]t1}...e^{[S_{i-1}]t_{i-1}}}] S_i.

    MR eq. 5.11; notes 5.1.
    """
    S = _screws(S)
    theta = _joints(theta, S.shape[1])
    Js = S.copy()
    T = sp.eye(4)
    for i in range(1, len(theta)):
        T = T * _exp(S[:, i - 1], theta[i - 1])
        Js[:, i] = adjoint(T) * S[:, i]
    return _done(Js, simplify)

jacobian_body 

jacobian_body(B, theta=None, *, simplify=True)

The body Jacobian J_b(theta), 6xn, column i = [Ad_{e^{-[Bn]tn}...e^{-[B_{i+1}]t_{i+1}}}] B_i.

MR eq. 5.18; notes 5.1.

Source code in screws/symbolic.py 
426
427
428
429
430
431
432
433
434
435
436
437
438
def jacobian_body(B, theta=None, *, simplify: Simplify = True) -> sp.Matrix:
    """The body Jacobian J_b(theta), 6xn, column i = [Ad_{e^{-[Bn]tn}...e^{-[B_{i+1}]t_{i+1}}}] B_i.

    MR eq. 5.18; notes 5.1.
    """
    B = _screws(B)
    theta = _joints(theta, B.shape[1])
    Jb = B.copy()
    T = sp.eye(4)
    for i in range(len(theta) - 2, -1, -1):
        T = T * _exp(B[:, i + 1], -theta[i + 1])
        Jb[:, i] = adjoint(T) * B[:, i]
    return _done(Jb, simplify)

ad 

ad(V)

[ad_V], the 6x6 matrix of the Lie bracket: [V1, V2] = [ad_V1] V2. MR eq. 8.39; notes 8.2.

Source code in screws/symbolic.py 
444
445
446
447
448
449
450
451
452
def ad(V) -> sp.Matrix:
    """[ad_V], the 6x6 matrix of the Lie bracket: [V1, V2] = [ad_V1] V2. MR eq. 8.39; notes 8.2."""
    V = _vector(V, 6)
    W = vec_to_so3(V[:3, 0])
    A = sp.zeros(6, 6)
    A[:3, :3] = W
    A[3:, :3] = vec_to_so3(V[3:, 0])
    A[3:, 3:] = W
    return A

inverse_dynamics 

inverse_dynamics(
    theta,
    dtheta,
    ddtheta,
    g,
    F_tip,
    link_frames,
    link_inertias,
    S,
    *,
    simplify=True,
)

Joint forces/torques tau = M(theta) ddtheta + c(theta, dtheta) + g(theta) + J^T F_tip.

Newton-Euler forward and backward iterations, as screws.inverse_dynamics. theta, dtheta and ddtheta may be None: theta_i, thetadot_i, thetaddot_i. F_tip None means zero. MR 8.3.2, algorithm on p. 296; notes 8.3.

Source code in screws/symbolic.py 
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
def inverse_dynamics(
    theta, dtheta, ddtheta, g, F_tip, link_frames, link_inertias, S, *, simplify: Simplify = True
) -> sp.Matrix:
    """Joint forces/torques tau = M(theta) ddtheta + c(theta, dtheta) + g(theta) + J^T F_tip.

    Newton-Euler forward and backward iterations, as screws.inverse_dynamics. theta, dtheta
    and ddtheta may be None: theta_i, thetadot_i, thetaddot_i. F_tip None means zero.
    MR 8.3.2, algorithm on p. 296; notes 8.3.
    """
    S = _screws(S)
    n = S.shape[1]
    theta = _joints(theta, n)
    dtheta = _joints(dtheta, n, "thetadot")
    ddtheta = _joints(ddtheta, n, "thetaddot")
    Ms, Gs = _model(link_frames, link_inertias, S, n)
    F = sp.zeros(6, 1) if F_tip is None else _vector(F_tip, 6)
    Mi = sp.eye(4)
    A, AdT = [], [None] * (n + 1)
    V = [sp.zeros(6, 1)]
    Vd = [sp.Matrix.vstack(sp.zeros(3, 1), -_vector(g, 3))]
    AdT[n] = adjoint(transform_inv(Ms[n]))
    for i in range(n):
        Mi = Mi * Ms[i]
        A.append(adjoint(transform_inv(Mi)) * S[:, i])
        AdT[i] = adjoint(_exp(A[i], -theta[i]) * transform_inv(Ms[i]))
        V.append(AdT[i] * V[i] + A[i] * dtheta[i])
        Vd.append(AdT[i] * Vd[i] + A[i] * ddtheta[i] + ad(V[i + 1]) * A[i] * dtheta[i])
    tau = sp.zeros(n, 1)
    for i in range(n - 1, -1, -1):
        F = AdT[i + 1].T * F + Gs[i] * Vd[i + 1] - ad(V[i + 1]).T * (Gs[i] * V[i + 1])
        tau[i] = F.dot(A[i])
    return _done(tau, simplify)

mass_matrix 

mass_matrix(
    theta, link_frames, link_inertias, S, *, simplify=True
)

M(theta), one inverse-dynamics call per unit joint acceleration. MR 8.3.3; notes 8.3.

Source code in screws/symbolic.py 
500
501
502
503
504
505
506
507
508
509
510
511
512
def mass_matrix(theta, link_frames, link_inertias, S, *, simplify: Simplify = True) -> sp.Matrix:
    """M(theta), one inverse-dynamics call per unit joint acceleration. MR 8.3.3; notes 8.3."""
    S = _screws(S)
    n = S.shape[1]
    theta = _joints(theta, n)
    cols = [
        inverse_dynamics(
            theta, [0] * n, [int(j == i) for j in range(n)], [0, 0, 0], None,
            link_frames, link_inertias, S, simplify=False,
        )
        for i in range(n)
    ]
    return _done(sp.Matrix.hstack(*cols), simplify)

velocity_quadratic_forces 

velocity_quadratic_forces(
    theta,
    dtheta,
    link_frames,
    link_inertias,
    S,
    *,
    simplify=True,
)

c(theta, dtheta), the Coriolis and centripetal joint forces. MR 8.3.3; notes 8.3.

Source code in screws/symbolic.py 
515
516
517
518
519
520
521
522
def velocity_quadratic_forces(
    theta, dtheta, link_frames, link_inertias, S, *, simplify: Simplify = True
) -> sp.Matrix:
    """c(theta, dtheta), the Coriolis and centripetal joint forces. MR 8.3.3; notes 8.3."""
    n = _screws(S).shape[1]
    return inverse_dynamics(
        theta, dtheta, [0] * n, [0, 0, 0], None, link_frames, link_inertias, S, simplify=simplify
    )

gravity_forces 

gravity_forces(
    theta,
    g,
    link_frames,
    link_inertias,
    S,
    *,
    simplify=True,
)

g(theta), the joint forces that hold the chain against gravity g. MR 8.3.3; notes 8.3.

Source code in screws/symbolic.py 
525
526
527
528
529
530
def gravity_forces(theta, g, link_frames, link_inertias, S, *, simplify: Simplify = True) -> sp.Matrix:
    """g(theta), the joint forces that hold the chain against gravity g. MR 8.3.3; notes 8.3."""
    n = _screws(S).shape[1]
    return inverse_dynamics(
        theta, [0] * n, [0] * n, g, None, link_frames, link_inertias, S, simplify=simplify
    )

end_effector_forces 

end_effector_forces(
    theta,
    F_tip,
    link_frames,
    link_inertias,
    S,
    *,
    simplify=True,
)

J^T(theta) F_tip, the joint forces that create the end-effector wrench alone. MR 8.3.3.

F_tip may be None: the symbols m_x, m_y, m_z, f_x, f_y, f_z.

Source code in screws/symbolic.py 
533
534
535
536
537
538
539
540
541
542
543
544
545
def end_effector_forces(
    theta, F_tip, link_frames, link_inertias, S, *, simplify: Simplify = True
) -> sp.Matrix:
    """J^T(theta) F_tip, the joint forces that create the end-effector wrench alone. MR 8.3.3.

    F_tip may be None: the symbols m_x, m_y, m_z, f_x, f_y, f_z.
    """
    n = _screws(S).shape[1]
    if F_tip is None:
        F_tip = sp.symbols("m_x m_y m_z f_x f_y f_z", real=True)
    return inverse_dynamics(
        theta, [0] * n, [0] * n, [0, 0, 0], F_tip, link_frames, link_inertias, S, simplify=simplify
    )

forward_dynamics 

forward_dynamics(
    theta,
    dtheta,
    tau,
    g,
    F_tip,
    link_frames,
    link_inertias,
    S,
    *,
    simplify=True,
)

ddtheta = M(theta)^{-1} (tau - c - g - J^T F_tip). tau may be None: tau1 ... taun.

The symbolic inverse grows quickly; past two or three joints, expect to wait. MR 8.5.

Source code in screws/symbolic.py 
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
def forward_dynamics(
    theta, dtheta, tau, g, F_tip, link_frames, link_inertias, S, *, simplify: Simplify = True
) -> sp.Matrix:
    """ddtheta = M(theta)^{-1} (tau - c - g - J^T F_tip). tau may be None: tau1 ... taun.

    The symbolic inverse grows quickly; past two or three joints, expect to wait. MR 8.5.
    """
    S = _screws(S)
    n = S.shape[1]
    theta = _joints(theta, n)
    dtheta = _joints(dtheta, n, "thetadot")
    tau = sp.Matrix(_joints(tau, n, "tau"))
    rest = inverse_dynamics(
        theta, dtheta, [0] * n, g, F_tip, link_frames, link_inertias, S, simplify=False
    )
    M = mass_matrix(theta, link_frames, link_inertias, S, simplify=simplify)
    # M is positive definite, so elimination never meets a true zero pivot and a structural
    # zero test will do; SymPy's default test tries to prove each pivot nonzero, and on an
    # unsimplified M that takes minutes.
    return _done(M.LUsolve(tau - rest, iszerofunc=lambda e: e == 0), simplify)