Alexander J. Elias, Rensselaer Polytechnic Institute, Troy, NY.

# Redundancy parameterization and inverse kinematics of 7-DOF revolute
manipulators

Alexander J. Elias and John T. Wen  Department of Electrical, Computer, and
Systems Engineering, Rensselaer Polytechnic Institute, Troy, NY
<EMAIL_ADDRESS>
###### Abstract

Seven degree-of-freedom (DOF) robot arms have one redundant DOF which does not
change the translational or rotational motion of the end effector. The
redundant DOF offers greater manipulability of the arm configuration to avoid
obstacles and steer away from singularities, but it must be parameterized to
fully specify the joint angles for a given end effector pose. For 7-DOF
revolute (7R) manipulators, we introduce a new concept of generalized
shoulder-elbow-wrist (SEW) angle, a generalization of the conventional SEW
angle but with an arbitrary choice of the reference direction function. The
SEW angle is easy for human operators to visualize as a rotation of the elbow
about the line from the shoulder to the wrist and has been used in the
teleoperation of space robot arms. Since the conventional SEW angle
formulation is prone to singularities, we introduce a special choice of the
reference direction function called the stereographic SEW angle which has a
singularity in only one direction in the workspace. We prove that such a
singularity is unavoidable for any parameterization. We also include
expressions for the SEW angle Jacobian along with singularity analysis.
Finally, we provide inverse kinematics solutions for most known 7R
manipulators using the general SEW angle and the subproblem decomposition
method. These solutions are often closed-form but may sometimes involve a 1D
or 2D search. Inverse kinematics solutions, examples, and evaluations are
available in a publicly accessible repository.

###### keywords:

Kinematics, Redundant Robots, Industrial Robots, Space Robotics and
Automation, Telerobotics and Teleoperation, Humanoid Robot Systems

##  1 Introduction

Most industrial robot arms have six revolute joints to control the six degrees
of freedom (DOF) of the robot end effector pose, but a human arm has seven
DOF: three for the shoulder, one for the elbow, and three for the wrist.
Similarly, there are 7-DOF revolute (7R) robot arms such as the Robotics
Research Corporation (RRC) arm (Robotics Research Corporation, 2005), Baxter
(Rethink Robotics, 2015), Sawyer (Rethink Robotics, 2022), Yumi (ABB, 2022),
and the Space Station Robot Manipulator System (SSRMS) (Crane III, Duffy and
Carnahan, 1991). The extra redundant degree of freedom means that there is a
continuum of arm configurations for a given hand or robot end effector pose.
Holding the end effector pose constant while moving through the continuum of
arm configurations, called self-motion, commonly looks like the elbow rotating
around the line passing from the shoulder to the wrist (Figure 1). Benefits of
7R arms over 6R arms include using redundancy to avoid singularities and
obstacles (Hollerbach, 1985), optimize motion time (Chen, Zhang, Huang, Wu and
Ota, 2023), avoid joint motion limits (Flacco, De Luca and Khatib, 2012), and
avoid joint torque limits (Hollerbach and Suh, 1987).

To fully specify the pose of a 7R robot up to a finite number of solutions,
the end effector pose must be augmented by a secondary task. During pure self-
motion, the only difference between parameterizations would be the rate of
movement; differences between parameterizations are made clearer during motion
of the end effector. Redundancy in human and many 7R robot arms may be
conveniently parameterized by the shoulder-elbow-wrist (SEW) angle, sometimes
called the elbow angle, which characterizes the rotation of the plane
containing the shoulder, elbow, and wrist about the shoulder-wrist line with
respect to a reference plane. This reference plane is conventionally chosen as
the plane containing the shoulder-wrist line and a reference vector. This
redundancy parameterization is easy to visualize. Furthermore, for certain 7R
arms, the inverse kinematics has an analytical solution, i.e., for a given
robot end effector pose and SEW angle, the finite set of the seven robot joint
angles may be solved directly instead of iteratively. The augmented Jacobian,
the $7\times 7$ matrix that maps the joint velocity vector to the end effector
spatial velocity and the SEW angular velocity, is easily characterized.

Figure 1: Self-motion for a Motoman SIA50D (R-R-3RE-2R) robot arm. For a given
end effector pose, the spherical elbow may be on a curve which is the
intersection of a torus formed by joints 1 and 2 a sphere centered at joints 6
and 7.

An issue with the conventional SEW angle becomes apparent when the shoulder-
wrist line is collinear with the reference vector because the reference plane
becomes undefined. This is referred to as an algorithmic singularity, a
singularity due to the choice of redundancy parameterization.

In this paper, we introduce a new generalized concept of SEW angle which
includes the conventional SEW angle as a special case. We show that an
algorithmic singularity is unavoidable for any redundancy parameterization,
but a special choice of the generalized SEW angle based on the stereographic
projection changes the bidirectional singularity to a unidirectional
singularity. The singularity direction may be located towards the base of the
arm so it will not be encountered in the robot workspace. The advantages of
SEW angle redundancy parameterization are still retained for the generalized
SEW angle, including analytical inverse kinematics for many 7R arms, intuitive
teleoperation, and an analytical Jacobian.

We also provide inverse kinematics (IK) solutions using the general SEW angle
and the subproblem decomposition method (Elias and Wen, 2022) for most 7R
robots used in practice or mentioned in the literature. The solutions are
often closed-form, but for some robots IK is solved using a 1D or 2D search.

The remainder of the paper is organized as follows. In Section 2 we discuss
previous related works, and we describe forward kinematics for 7R arms using
coordinate-free notation and the product of exponentials approach. We
introduce the general SEW angle in Section 3 including forward and
differential kinematics, we discuss singularity conditions, and we relate it
to the conventional SEW angle. In Section 4 we prove the existence of
algorithmic singularities for any redundancy parameterization. We define the
stereographic SEW angle in Section 5, discuss its relationship to
stereographic projection and its singularity behavior, and we provide an
example comparing using the conventional and stereographic SEW angle
formulations. We provide IK solutions in Section 6, and we conclude in Section
7.

Inverse kinematics solutions, examples, and evaluations are available in a
publicly accessible repository111https://github.com/rpiRobotics/stereo-sew.

##  2 Background

###  2.1 Robot Notation

To notate different families of robot kinematic parameters, we follow and
slightly extend the notation introduced by Pieper (1969). A single revolute
joint is notated by R, and when multiple joints intersect, they may be notated
as 2R or 3R for two or three intersecting joints, respectively. The order of
joints is given in order from the base to the end effector. For example, a
robot with a spherical shoulder, a revolute elbow, and a spherical wrist may
be notated as 3R-R-3R. We also introduce the notation of 2R|| and 3R|| to
indicate two or three consecutive parallel revolute joints. Note that since a
given joint may intersect or be parallel with both the joint before it and the
joint after it, a single robot may fall into multiple robot kinematic
families.

We use superscript S, E, or W to indicate when the shoulder, elbow, or wrist
is placed at a joint or joint intersection. For example, 2RE would indicate a
2R joint with the elbow placed at the joint intersection. Unless otherwise
indicated, a robot has its shoulder on the first joint or joint intersection
and has its wrist on its last joint or joint intersection.

###  2.2 Related Literature

The analysis and parameterization of the redundant degree of freedom in 7R
manipulators has been primarily driven by space robotics applications and more
recently motivated by industrial robots in manufacturing and humanoid robots.
Early papers focused on analyzing the geometry, topology, and differential
kinematics of 7-DOF manipulators. Baillieul (1985) introduced the concept of
the augmented Jacobian (termed the extended Jacobian) and discussed the
algorithmic singularity. Hollerbach (1985) discussed options for 7-DOF
manipulator designs, including how adding an extra degree of freedom removes
internal singularities. The paper discusses the self-motion manifold and was
one of the earliest papers to recommend using the conventional SEW angle (with
the reference vector pointing up). Burdick and Seraji (1989) gave an overview
of the topology of the self-motion manifold. Kreutz-Delgado, Long and Seraji
(1990, 1992) provided a detailed analysis of the conventional SEW angle with
application to 3R-R-3R arms and the RRC arm, which has no intersecting joint
axes. They also provided the expression for the SEW Jacobian and singularity
analysis.

There have been several other proposed redundancy parameterizations besides
the conventional SEW angle. Yan, Mu and Xu (2014) attempted to address the
singularity issue of the conventional SEW angle by proposing a method which
picks two different reference vectors and switches between the corresponding
SEW angles when one of them encounters a singularity. The resulting
parameterization is non-smooth, and singularities would still be present.
Shimizu, Yoon and Kitagaki (2007) used the SEW angle where the reference plane
is formed by the elbow when joint three is zero. They explained the reference
plane is undefined in a shoulder or elbow singularity, so there is no benefit
in terms of singularity existence as compared to the conventional SEW angle.

One can also parameterize the redundant DOF by choosing one joint angle and
reducing the problem to the IK of a non-redundant 6R manipulator, but this
introduces singularities resulting from the equivalent 6R robot. Xu, She and
Xu (2014) found the inverse kinematics for a 2R-3R||-2R arm by parameterizing
the redundant degree of freedom with any joint angle. Jiang, Huo, Liu and Liu
(2013) found the R-R-3R-2R arm IK solution by parameterizing joint 1. An,
Clement and Reed (2014) found the IK solution for a 2R-2R-3R arm by
parameterizing joint 1. Tondu (2006) found IK for a 3R-R-3R arm by
parameterizing joint 1, and Pfurner (2016) found the IK for a 3R-R-3R arm by
parameterizing joint 2 or 3. Nammoto and Kosuge (2012) found the IK solutions
for an R-R-3R-2R arm parameterized by joint 1. Sinha and Chakraborty (2019)
found a 1D search-based IK for R-2R-2R-2R arms parameterized by joint 1.

Many papers provide inverse kinematics solutions using the conventional SEW
angle. Gong, Li and Zhang (2019); Tian, Xu and Zhan (2021); Faria, Ferreira,
Erlhagen, Monteiro and Bicho (2018) used the conventional SEW angle to provide
closed-form IK for 3R-R-3R arms. An, Clement and Reed (2014) found a closed-
form IK solution for 2R-2R-3R arms. Many papers used an iterative IK method
with an approximate closed-form solution as a starting point: Wang, Zhao,
Wang, Zhang, Li and Liu (2021) for an R-2R-R-3R arm, and Ma, Xie, Jiang and
Liu (2021) for a 2R-3R||-2R arm. (Zhao, Yang, Zhao, Yang and Zhao, 2023) also
found an iterative solution for an 2R-3R||-2R. The paper states closed-form IK
is not possible when using SEW angle. However, (Jin, Liu, Wang and Liu, 2020)
solved IK for a 2R-3R||-2R manipulator in closed form by defining the SEW
plane to be perpendicular to joint axes 3, 4, and 5.

There are also applications of the conventional SEW angle not just to typical
7-DOF robots, but also to human arms and wearable robots (Kim, Miller, Al-
Refai, Brand and Rosen, 2011, 2012; Li, Roldan, Milutinović and Rosen, 2013;
Wang, 2013; Liu, Chen and Steil, 2017; Su, Enayati, Vantadori, Spinoglio,
Ferrigno and De Momi, 2018; Wang, Peng, Hou, Li, Luo, Chen and Wang, 2019; Li,
Han, He, Li, Liu and Xiong, 2022). The conventional SEW angle can also be used
to help with redundancy resolution. Lamperti, Zanchettin and Rocco (2015) used
conventional SEW angle to propose a human-like redundancy resolution method.
Xiong, Zhou and Yao (2020) used the conventional SEW angle to parameterize the
redundant degree of freedom for null-space impedance control. Algorithmic
singularities remain an issue in these methods and would need to be avoided.
They are not explicitly addressed in these papers.

###  2.3 Robot Kinematics

Table 1: Nomenclature. Coordinate-Free Notation
---
${\mathcal{O}}$ | Point in Euclidean space.
$\vec{p}$ | Vector in Euclidean space.
${\mathcal{E}}$ |  Orthonormal frame, $=[\vec{e}_{x}\ \vec{e}_{y}\ \vec{e}_{z}]$.
${\cal R}(\vec{h},\theta)$ |  Rotation operator about $\vec{h}$ over angle $\theta$.
Robot Kinematics
${\mathcal{O}}_{S}$ | Shoulder.
${\mathcal{O}}_{E}$ | Elbow.
${\mathcal{O}}_{W}$ | Wrist.
${\mathcal{O}}_{C}$ | Projection of elbow on shoulder-wrist line.
$J$ | End effector Jacobian.
$J_{E}$ | Elbow Jacobian.
$J_{W}$ | Wrist Jacobian.
General SEW Angle
$k_{SEW}$ |  $=p_{SW}^{\times}p_{SE}$.
$n_{SEW}$ |  $=k_{SEW}/\left\|{k_{SEW}}\right\|$. SEW plane normal.
$\psi$ | SEW angle.
$e_{x}{=}f_{x}(p_{SW})$ | Reference direction function.
$(e_{x},e_{y},e_{SW})$ | SEW angle coordinate frame.
$J_{f_{x}}$ |  Reference direction Jacobian w.r.t. $p_{SW}$.
$J_{\psi}$ | SEW angle Jacobian.
$J_{A}$ | Augmented Jacobian.
Conventional SEW Angle
$e_{r}$ |  Arbitrary unit reference vector.
$k_{y}$ |  $=p_{SW}^{\times}e_{r}$, meaning $e_{y}=k_{y}/\left\|{k_{y}}\right\|$.
Stereographic SEW Angle
$e_{t}$ |  Unit translation vector.
$k_{rt}$ |  $=(e_{SW}-e_{t})^{\times}e_{r}$.
$k_{x}$ |  $=k_{rt}^{\times}p_{SW}$, meaning $e_{x}=k_{x}/\left\|{k_{x}}\right\|$.

We use the coordinate-free notation shown in Table 1. Vector $\vec{p}$
represented in frame ${\mathcal{E}}_{a}$ is the $\mathbb{R}^{3}$ vector
$p_{a}={\mathcal{E}}_{a}^{*}\vec{p}$, where

${\mathcal{E}}_{a}^{*}=\begin{bmatrix}\vec{e}_{x}\cdot\\\ \vec{e}_{y}\cdot\\\
\vec{e}_{z}\cdot\end{bmatrix}$ (1)

is the adjoint of ${\mathcal{E}}_{a}$. Frame ${\mathcal{E}}_{b}$ represented
in frame ${\mathcal{E}}_{a}$ is the $SO(3)$ matrix
$R_{ab}={\mathcal{E}}_{a}^{*}{\mathcal{E}}_{b}$. Rotation ${\cal
R}(\vec{h},\theta)$ in frame ${\mathcal{E}}_{a}$ is the $SO(3)$ matrix ${\tt
R}(h_{a},\theta)=e^{h_{a}^{\times}\theta}$, where $(\cdot)^{\times}$ is the
$3\times 3$ skew-symmetric matrix representation of the cross product:

$h^{\times}:=\begin{bmatrix}0&-h_{z}&h_{y}\\\ h_{z}&0&-h_{x}\\\
-h_{y}&h_{x}&0\end{bmatrix}.$ (2)

For a unit vector $h$, $-h^{\times^{2}}=-h^{\times}h^{\times}=I-hh^{T}$ is the
projector onto the orthogonal complement of $h$.

Consider a 7R robot as shown in Figure 2 with joint angles $q=[q_{1}\ q_{2}\
\cdots\ q_{7}]^{T}$. We will use the product of exponentials approach to
describe the arm kinematics. Denote the base frame and base frame origin as
$({\mathcal{E}}_{0},{\mathcal{O}}_{0})$ and the end effector task frame and
task frame origin as $({\mathcal{E}}_{T},{\mathcal{O}}_{T})$. Choose the link
frame origins ${\mathcal{O}}_{i}$ along each unit joint axis $\vec{h}_{i}$.
Let $\vec{p}_{ij}$ be the vector from ${\mathcal{O}}_{i}$ to
${\mathcal{O}}_{j}$. Define the $i$th frame ${\mathcal{E}}_{i}={\cal
R}(\vec{h}_{i},q_{i}){\mathcal{E}}_{i-1}$. Then the forward kinematics of the
task frame represented in the base frame is

$\displaystyle R_{0T}={}$ $\displaystyle
R_{01}R_{12}R_{23}R_{34}R_{45}R_{56}R_{67}R_{7T}$ (3a)
$\displaystyle\begin{split}p_{0T}={}&p_{01}+R_{01}p_{12}+R_{02}p_{23}+R_{03}p_{34}\\\
{}&+R_{04}p_{45}+R_{05}p_{56}+R_{06}p_{67}+R_{07}p_{7T}\end{split}$ (3b)

where $R_{ij}=R_{i,i+1}\dotsm R_{j-1,j}$, $h_{i}$ and $p_{i-1,i}$ are constant
$\mathbb{R}^{3}$ vectors representing $\vec{h}_{i}$ and $\vec{p}_{i-1,i}$ in
${\mathcal{E}}_{i-1}$, $R_{i-1,i}=e^{h_{i}^{\times}q_{i}}$ is ${\cal
R}(\vec{h}_{i},q_{i})$ represented in ${\mathcal{E}}_{i-1}$, $R_{7T}$ is a
constant $SO(3)$ matrix for the fixed wrist-tool transform, and $p_{7T}$ is
the constant tool offset in the 7 frame. The constant vectors $h_{i}$,
$p_{i-1,i}$, $p_{7T}$ and constant transform $R_{7T}$ are obtained by putting
the arm in the zero configuration $q=0$.

Figure 2: General 7R robot arm with example shoulder, elbow, and wrist points
${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$, and ${\mathcal{O}}_{W}$ fixed in
${\mathcal{E}}_{0}$, ${\mathcal{E}}_{3}$, and ${\mathcal{E}}_{7}$,
respectively. In general, these points may be placed anywhere in the kinematic
chain.

Define three points $\mathcal{O}_{S}$, $\mathcal{O}_{E}$, and
$\mathcal{O}_{W}$ to represent the robot shoulder, elbow, and wrist,
respectively. The location for these points are arbitrary, but different
choices will lead to different inverse kinematics methods and different
singularity structures. For easier inverse kinematics, $\mathcal{O}_{S}$
should be constant in the base frame and ${\mathcal{O}}_{W}$ should be
constant in the 7 frame (which also means it is constant in the tool frame).
${\mathcal{O}}_{E}$ must be placed somewhere in the kinematic chain such that
it rotates around the line passing through ${\mathcal{O}}_{S}$ and
${\mathcal{O}}_{W}$ as the robot moves through its redundant degree of freedom
while keeping the end effector pose constant. For many robots, a good choice
is to make ${\mathcal{O}}_{E}$ constant in frame ${\mathcal{E}}_{3}$ or
${\mathcal{E}}_{4}$. In this paper, we will use ${\mathcal{E}}_{3}$ for
illustration. Representing these points in their respective constant frames,
we have constant vectors $p_{0S}$, $p_{3E}$, and $p_{7W}$. For a given pose,
we can find $p_{SE}$, $p_{SW}$, and $p_{SW}$, which are $\vec{p}_{SE}$,
$\vec{p}_{SW}$, and $\vec{p}_{SW}$ represented in ${\mathcal{E}}_{0}$. The
shoulder-wrist vector is

$p_{SW}=p_{0T}-R_{07}p_{7T}+R_{07}p_{7W}-p_{0S},$ (4)

and the shoulder-elbow vector is

$p_{SE}=p_{01}+R_{01}p_{12}+R_{02}p_{23}+R_{03}p_{3E}-p_{0S}.$ (5)

It is often helpful for the inverse kinematics solution to set the shoulder,
elbow, or wrist offset vector to $0$ so that ${\mathcal{O}}_{S}$,
${\mathcal{O}}_{E}$, or ${\mathcal{O}}_{W}$ is coincident with the link frame
origin.

On some robots, such as those with three consecutive parallel axes, it may be
helpful to define the shoulder-elbow vector $p_{SE}$ to be a unit vector
$e_{SE}$ which is equal to one of the joint axes represented in the base
frame. For example, we may pick $p_{SE}=e_{SE}=R_{02}h_{3}$. Since three
parallel joint axes are the limit of three intersecting axes where the point
of intersection moves infinitely far away, this can be interpreted as choosing
$p_{SE}$ to be the normalized vector pointing at this intersection (Figure 3).
For some robots this leads to closed-form inverse kinematics using the
subproblem decomposition approach.

(a) 3R elbow.

(b) 3R|| elbow.

Figure 3: A 3R|| joint is the limit of a 3R joint as the intersection point
moves to infinity. If this joint is the elbow, then although in the limit
$p_{SE}$ has infinite length, the normalized vector $e_{SE}$ is defined and is
equal to the three parallel joint axes.

##  3 General SEW Angle

###  3.1 Kinematics Description

Consider a 7-DOF arm with shoulder, elbow, and wrist defined. The SEW
(shoulder-elbow-wrist) angle $\psi$, also commonly referred to as the arm
angle or swivel angle, is the angle of the shoulder-elbow vector $p_{SE}$
about the shoulder-wrist vector $p_{SW}$ with respect to some reference
vector. Consider an arbitrary mapping, called the reference direction
function, from the shoulder-wrist vector $p_{SW}$ to a unit vector $e_{x}$
that is orthogonal to $p_{SW}$. Denote this mapping by $e_{x}=f_{x}(p_{SW})$.
From this we can form an orthonormal basis $(e_{x},\ e_{y},\ e_{SW})$, where
$e_{SW}$ is the normalized $p_{SW}$ and $e_{y}=e_{SW}^{\times}e_{x}$. This
frame may be used to measure the SEW angle.

Figure 4: The SEW angle $\psi$ is the angle of the elbow measured from
$e_{x}=f_{x}(p_{SW})$ about $e_{SW}$, which is also the angle of the SEW plane
normal vector $n_{SEW}$ measured from $e_{y}$ about $e_{SW}$. In the general
SEW angle, the reference direction function $f_{x}(p_{SW})$ is arbitrary but
with the constraints that the output is unit length and orthogonal to
$p_{SW}$.

There are two equivalent definitions of the SEW angle, as shown in Figure 4.
The elbow definition is that $\psi$ is the angle between shoulder-elbow vector
$p_{SE}$ and reference vector $e_{x}$ measured along the shoulder-wrist
rotational axis $e_{SW}$. The plane definition is that $\psi$ is the angle
between $n_{SEW}$, which is the normal vector of the SEW plane containing the
shoulder, elbow and, wrist, and $e_{y}$, which is the normal vector of the
reference plane containing the shoulder, wrist, and reference vector $e_{x}$.
While the elbow definition is helpful for forward kinematics, the plane
definition is more useful for inverse kinematics when using the subproblem
decomposition approach.

Using the elbow definition, the SEW angle is

$\psi=\operatorname*{arg\,min}_{\theta}\left\|{{\tt
R}(e_{SW},\theta)e_{x}-p_{SE}}\right\|.$ (6)

We can also rewrite this more compactly by removing the component of $p_{SE}$
along $e_{SW}$. Let ${\mathcal{O}}_{C}$ be the point on $p_{SW}$ such that
$p_{CE}$ is orthogonal to $e_{SW}$, i.e.,

$p_{CE}=-e_{SW}^{\times^{2}}p_{SE}.$ (7)

Then,

$e_{CE}={\tt R}(e_{SW},\psi)e_{x},$ (8)

where $e_{CE}=p_{CE}/\left\|{p_{CE}}\right\|$.

Using the plane definition, the SEW angle is

$n_{SEW}={\tt R}(e_{SW},\psi)e_{y},$ (9)

where the normal of the plane containing the shoulder, elbow, and wrist is

$\displaystyle n_{SEW}$
$\displaystyle=\frac{k_{SEW}}{\left\|{k_{SEW}}\right\|},$ (10a) $\displaystyle
k_{SEW}$ $\displaystyle=p_{SW}^{\times}p_{SE}.$ (10b)

Subproblem 1 (Elias and Wen, 2022) may be used to solve the forward kinematics
for the SEW angle, which is

$\displaystyle\psi$
$\displaystyle=\mbox{atan2}\left(e_{y}^{T}p_{SE},e_{x}^{T}p_{SE}\right)$ (11a)
$\displaystyle=\mbox{atan2}\left(e_{y}^{T}p_{CE},e_{x}^{T}p_{CE}\right)$ (11b)
$\displaystyle=\mbox{atan2}\left(-e_{x}^{T}n_{SEW},e_{y}^{T}n_{SEW}\right)$
(11c)
$\displaystyle=\mbox{atan2}\left(-e_{x}^{T}k_{SEW},e_{y}^{T}k_{SEW}\right).$
(11d)

To perform inverse kinematics, we can use a given $(R_{0T},p_{0T})$ to find
$p_{SW}$ from (4). Next, use the reference direction function $f_{x}(p_{SW})$
to find $e_{x}$ or $e_{y}$. Then, for a given $\psi$, we can calculate
$e_{CE}$ or $n_{SEW}$ using (8) or (9), respectively, which may then be used
to determine the joint angle vector $q$. For the subproblem decomposition
approach in Section 6, we always calculate $n_{SEW}$.

Some robots are not easily parameterized using the SEW angle. For example, the
FANUC R-1000iA/120F-7B is an R-3R||-3R robot (FANUC, 2022). The redundant
degree of freedom in this family of robots is the movement of joints 2, 3, and
4, along with the corresponding movement in the spherical wrist, which results
in the shortening or lengthening of the distance between joints 2 and 4 (Shi,
Guo, Chen, Chen and Yang, 2021). However, with ${\mathcal{O}}_{S}$ placed at
joint 1 and ${\mathcal{O}}_{W}$ placed at the spherical wrist, there is no
movement of joints 2, 3, or 4 around $e_{SW}$ during self-motion; they stay in
the same half plane and the SEW angle would fail to parameterize the redundant
degree of freedom. Robots such as this are better parameterized by specifying
a joint angle. (A good choice for this robot is $q_{3}$.)

To find the Jacobian of the SEW angle with respect to the joint angles,
$J_{\psi}$, we will write it in terms of the partial Jacobians mapping joint
angular velocities to the linear velocity of ${\mathcal{O}}_{E}$ and
${\mathcal{O}}_{W}$:

$\dot{p}_{SE}=\dot{p}_{0E}=J_{E}\dot{q},\quad\dot{p}_{SW}=\dot{p}_{0W}=J_{W}\dot{q}$
(12)

If ${\mathcal{O}}_{S}$ is not constant in the base frame, then $J_{S}$ is
nonzero and we must subtract $J_{S}\dot{q}$.

Taking the total derivative of (11b), we obtain

$\dot{\psi}=\frac{1}{\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}\dot{p}_{CE}-e_{y}^{T}\dot{e}_{x}.$
(13)

Geometrically, these two terms are the angular velocities of $p_{CE}$ and
$e_{x}$ around the axis of rotation $e_{SW}$. Define $J_{f_{x}}$ such that
$\dot{e}_{x}=J_{f_{x}}\dot{p}_{SW}$, which depends on the choice for the
function $f_{x}(p_{SW})$. Then by using (7), we can expand (13) as

$\dot{\psi}=\frac{1}{\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}\dot{p}_{0E}\\\
-\left(e_{y}^{T}J_{f_{x}}+\frac{e_{SW}^{T}p_{SE}}{\left\|{p_{SW}}\right\|\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}\right)\dot{p}_{0W}$
(14)

Since we have

$\dot{\psi}=J_{\psi}\dot{q}$ (15)

we can write the SEW Jacobian as

$J_{\psi}=J_{\psi,E}J_{E}+J_{\psi,W}J_{W},$ (16)

where

$\displaystyle J_{\psi,E}$
$\displaystyle=\frac{(e_{SW}^{\times}e_{CE})^{T}}{\left\|{p_{CE}}\right\|},$
(17) $\displaystyle J_{\psi,W}$
$\displaystyle=-e_{y}^{T}J_{f_{x}}-\frac{e_{SW}^{T}p_{SE}}{\left\|{p_{SW}}\right\|\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}.$
(18)

We can form the $7\times 7$ augmented Jacobian $J_{A}$ by stacking $J$ and
$J_{\psi}$:

$J_{A}=\begin{bmatrix}J\\\ J_{\psi}\end{bmatrix},\ \begin{bmatrix}\omega\\\
v\\\ \dot{\psi}\end{bmatrix}=J_{A}\dot{q}.$ (19)

The augmented task space, which has the end effector orientation as well as
the SEW angle, has 7 degrees of freedom.

###  3.2 Singularity Conditions

Table 2: Singularity conditions. Cases 2 and 3 are both algorithmic singularities. Condition | Singularity Name | Description
---|---|---
1\. $J$ loses rank | Kinematic | End effector cannot move in one direction
A. Null space of $J$ tangent to self-motion manifold | Internal | Extra continuous self-motion possible
B. Null space of $J$ not tangent to self-motion manifold | Boundary | Self-motion is instantaneous
2\. $J_{A}$ singular (Full rank $J$ and $J_{\psi}$) | Augmentation | Self-motion doesn’t change SEW angle
3\. $J_{\psi}$ undefined | SEW Angle | SEW angle undefined
A. ${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$, ${\mathcal{O}}_{W}$ collinear | Collinear |  Depends on choice of ${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$, ${\mathcal{O}}_{W}$
B. $J_{f_{x}}$ undefined | Coordinate |  Depends on choice of $f_{x}(p_{SW})$

When controlling a robot using end effector pose augmented with SEW angle,
singularities occur when the matrix $J_{A}^{-1}$ does not exist. When a robot
is close to a singularity, there may be large internal joint motion which is
undesirable and possibly dangerous. There are a number of conditions which
result in a singularity, as shown in Table 2. Multiple singularity types can
occur simultaneously.

The self-motion manifold for 7-DOF spatial manipulators is a curve in joint
space for which all points map to the same end effector pose (Burdick and
Seraji, 1989). A single end effector pose may have multiple self-motion
manifolds, where each manifold belongs to a different inverse kinematics
branch. This manifold exists independently of any parameterization of the
redundant degree of freedom, and the goal of parameterizations such as SEW
angle is to assign a value to each point on the self-motion manifold. When $J$
is full rank, the self-motion manifold is one-dimensional and the null space
of $J$ is the tangent to the self-motion manifold.

A kinematic singularity occurs when $J$ loses rank, meaning some spatial
velocity of the end effector cannot be achieved. This is a condition that
depends on the kinematics and joint angles of the robot and does not depend on
the parameterization of the redundant degree of freedom. There are two types
of kinematic singularities: Internal singularities and boundary singularities.
At internal singularities, e.g., when two joint axes are collinear, the null
space of $J$ is tangent to the self-motion manifold. A new degree of freedom
for self-motion is introduced, but parameterizations such as the SEW angle may
not be able to parameterize this degree of freedom. At boundary singularities,
e.g., when two links are collinear, the null space of $J$ is not tangent to
the self-motion manifold, and self-motion is only instantaneous. In some
cases, the entire null space of $J$ causes only instantaneous rather than
continuous self-motion, and the self-motion manifold degenerates into a point:
The self-motion manifold is zero-dimensional and any parameterization for the
redundant degree of freedom is unnecessary because for each inverse kinematics
branch there is only one choice for $\psi$.

For all conditions other than the kinematic singularity, we call them
algorithmic singularities as they occur not because of the robot itself but
because of the algorithms we use to parameterize the redundancy. Many authors
(Kreutz-Delgado, Long and Seraji, 1990; Chiaverini, 1997; Marani, Kim, Yuh and
Chung, 2003) have defined algorithmic singularities to occur when $J_{A}^{-1}$
does not exist and $J$ and $J_{\psi}$ are separately full rank. However, as in
Wang and Kazerounian (1995); Faria, Ferreira, Erlhagen, Monteiro and Bicho
(2018), we generalize the definition of algorithmic singularity to include
cases where $J_{\psi}$ may not be full rank (i.e., may be a zero vector) or
may not exist.

One type of algorithmic singularity is when $J_{A}$ loses rank, which is
possible even when $J$ and $J_{\psi}$ are full rank. We call this the
augmentation singularity, as it occurs only when end effector and SEW angle
rates are considered simultaneously. Algebraically, this singularity occurs
when $J_{\psi}$ is linearly dependant with the rows of $J$. Geometrically,
this means self-motion does not cause a change in SEW angle.

The second type of algorithmic singularity is when $J_{A}^{-1}$ does not exist
because $J_{\psi}$ does not exist, which we call the SEW angle singularity.
There are two cases for the SEW angle singularity. The first case, called the
collinear singularity, happens when ${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$,
and ${\mathcal{O}}_{W}$ are collinear, which causes $p_{SW}=0$ or $p_{CE}=0$.
The second case, called the coordinate singularity, is when $J_{\psi}$ does
not exist because $J_{f_{x}}$ does not exist. In fact, in section 4, we show
that for any choice of $f_{x}(p_{SW})$ there will always be choices of
$p_{SW}$ that causes singularities in $J_{f_{x}}$.

Given that the collinear singularity is unavoidable for some robots, it can be
beneficial to place ${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$, and
${\mathcal{O}}_{W}$ such that the SEW angle is undefined exactly when there is
only one choice for elbow position per IK branch due to boundary
singularities. This reduces the total number of singularities in the
workspace. In the case of a 2R-2R-3R robot, placing the shoulder, elbow, and
wrist at the joint intersections means that SEW angle is undefined when the
robot has no self-motion of the elbow to parameterize anyway.

The condition of $J_{\psi}$ losing rank, meaning $J_{\psi}=0$, may occur for a
general parameterization of the redundant degree of freedom, but for most
reasonable choices of placement of ${\mathcal{O}}_{S}$, ${\mathcal{O}}_{E}$,
and ${\mathcal{O}}_{W}$, this condition will not occur for the SEW angle.
Choi, Oh, Oh, Park and Chung (2004) called this case the secondary task
singularity.

###  3.3 Conventional SEW Angle

The conventional SEW angle, shown in Figure LABEL:sub@fig:f_x_conv, is a
widely used parameterization. Kreutz-Delgado, Long and Seraji (1990) were the
first to provide detailed analysis of the conventional SEW angle, but the idea
was described earlier by Hollerbach (1985).

Let $e_{r}$ be an arbitrary unit reference vector. For the conventional SEW
angle, we define the reference direction function $f_{x}$ as

$\displaystyle e_{x}$ $\displaystyle=f_{x}(p_{SW})=e_{y}^{\times}e_{SW},$
(20a) $\displaystyle e_{y}$
$\displaystyle=\frac{k_{y}}{\left\|{k_{y}}\right\|},$ (20b) $\displaystyle
k_{y}$ $\displaystyle=p_{SW}^{\times}e_{r}.$ (20c)

Equivalently, $e_{x}=f_{x}(p_{SW})$ is the normalized
$-e_{SW}^{\times^{2}}e_{r}$, so $e_{x}$ is the unit vector orthogonal to
$e_{SW}$ closest to $e_{r}$.

(a) Conventional SEW angle.

(b) Stereographic SEW angle.

Figure 5:  Geometric interpretations for $e_{x}=f_{x}(p_{SW})$. (a) In the
conventional SEW angle, $e_{x}$ is the normalized version of the component of
$e_{r}$ orthogonal to $e_{SW}$, and $e_{y}$ is normal to the plane containing
$e_{SW}$ and $e_{r}$. (b) In the stereographic SEW angle, $k_{rt}$ is normal
to the plane containing $e_{SW}-e_{t}$ and $e_{r}$, $e_{y}$ is the normalized
version of the component of $k_{rt}$ orthogonal to $e_{SW}$, and $e_{x}$ is
normal to $k_{rt}$ and $e_{SW}$.

The conventional SEW angle is intuitive to understand. $\psi=0$ corresponds to
$e_{CE}$ pointing as much towards $e_{r}$ as possible. This is similar to
navigating on a globe using cardinal directions where $e_{r}$ provides the
north direction and the SEW angle is measured from north.

The conventional SEW angle becomes undefined and a coordinate singularity
occurs when $k_{y}=p_{SW}^{\times}e_{r}=0$, which occurs when $p_{SW}$ is
collinear with $e_{r}$. In this case. all possible choices of $e_{CE}$ are
equally close to $e_{r}$. This is akin to being on the north or south pole of
a globe where there is no north direction and cardinal directions are
undefined. If the shoulder is constant in the base frame, then this
singularity occurs when the wrist is on the line spanned by $e_{r}$ passing
through the wrist, as shown in Figure
LABEL:sub@fig:singularity_locations_conv.

---
(a) Conventional SEW angle.

---
(b) Stereographic SEW angle.

Figure 6:  Grid representing $(e_{x},e_{y})$ for any choice of $e_{SW}$ (top),
and cylinder representing locations for ${\mathcal{O}}_{W}$ where the robot is
at or near a coordinate singularity. (a) The conventional SEW angle has two
singularities of order one, and the coordinate axes are lines of latitude and
longitude. This results in a bidirectional singularity region in task space.
(b) The stereographic SEW angle has a single singularity of order 2, which
results in a unidirectional singularity region.

Using the elbow definition of the SEW angle, the conventional SEW angle can be
succinctly expressed as

$\psi=\operatorname*{arg\,min}_{\theta}\left\|{{\tt
R}(e_{SW},\theta)e_{r}-p_{SE}}\right\|,$ (21)

(notice we may use $e_{r}$ in place of $e_{x}$), which may be directly solved
using Subproblem 1 as

$\displaystyle\psi$
$\displaystyle=\mbox{atan2}\left((e_{SW}^{\times}e_{r})^{T}p_{SE},-(e_{SW}^{\times^{2}}e_{r})^{T}p_{SE}\right)$
(22a)
$\displaystyle=\mbox{atan2}(e_{SW}^{T}e_{r}^{\times}p_{CE},e_{r}^{T}p_{CE}).$
(22b)

Alternatively, using the plane definition we have

$\displaystyle\psi$
$\displaystyle=\operatorname*{arg\,min}_{\theta}\left\|{{\tt
R}(e_{SW},\theta)k_{y}-k_{SEW}}\right\|$ (23a)
$\displaystyle=\mbox{atan2}((e_{SW}^{\times}k_{y})^{T}k_{SEW},-(e_{SW}^{\times^{2}}k_{y})^{T}k_{SEW})$
(23b)
$\displaystyle=\mbox{atan2}(e_{SW}^{T}k_{y}^{\times}k_{SEW},k_{y}^{T}k_{SEW}).$
(23c)

To calculate the Jacobian, we can express $-e_{y}^{T}\dot{e}_{x}$ as

$\frac{e_{x}^{T}}{\left\|{k_{y}}\right\|}\dot{k}_{y}=\frac{e_{x}^{T}}{\left\|{k_{y}}\right\|}e_{r}^{\times}\dot{p}_{SW}=\frac{e_{SW}^{T}e_{r}}{\left\|{k_{y}}\right\|}e_{y}^{T}\dot{p}_{SW}.$
(24)

Therefore, the Jacobian with respect to the wrist is

$J_{\psi,W}=\frac{e_{SW}^{T}e_{r}}{\left\|{k_{y}}\right\|}e_{y}^{T}-\frac{e_{SW}^{T}p_{SE}}{\left\|{p_{SW}}\right\|\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}.$
(25)

The Jacobian becomes undefined when $k_{y}=p_{SW}^{\times}e_{r}=0$, again
exhibiting the algorithmic singularity condition when $e_{r}$ and $p_{SW}$
become collinear.

##  4 Existence of Singularity

Hollerbach (1985) showed any revolute manipulator will have internal
singularities associated with end effector orientation because it is always
possible to find a pose with all joints lying in the same plane. Gottlieb
(1986) used topology to arrive at the same conclusion: Every smooth map
$T^{n}\rightarrow SO(3)$ must have singularities. Furthermore, he showed it is
impossible to construct a smooth left-inverse of a mapping $T^{4}\rightarrow
SO(3)$, so there is no singularity-free function to map from orientation to
joint angles in a 4R joint. (However, it is possible to always find joint
angles to follow a given trajectory while avoiding singularities, but this
does not follow a function and so will not be cyclical.)

In this section, we will show for a 7-DOF revolute arm with a sufficiently
large workspace, e.g., a 3R-R-3R robot with orthogonal consecutive joints, and
a task space augmented by any parameterization of the redundant degree of
freedom, e.g., the SEW angle, there will always be an algorithmic singularity.

The so-called Hairy Ball Theorem (McGrath, 2016) states that a continuous
vector field on a sphere must have at least one point where it vanishes. We
will use this theorem to show that a singularity must exist for any redundancy
parameterization, $\phi:T^{7}\to\mathbb{R}$. Given any end effector pose
$(p_{0T},R_{0T})$ and $\phi$, there exists a finite number of inverse
kinematics solutions $q$. Consider the sphere generated by a constant
shoulder-wrist distance $\left\|{p_{SW}}\right\|$ such that the elbow
${\mathcal{O}}_{E}$ does not lie on the shoulder-wrist line, i.e., $p_{SE}$
and $p_{SW}$ are not collinear. For any point on the sphere and a constant
orientation and $\phi$, find the inverse kinematics solution $q$ that is on
the same branch of the finite number of solutions. Define the vector field
$e_{CE}(p_{SE})$ as the unit vector perpendicular to $p_{SE}$ and pointing
towards the elbow, which is the normalized version of
$-e_{SW}^{\times^{2}}p_{SE}$, as shown in Figure 7. By the Hairy Ball Theorem,
$e_{CE}$ cannot be continuous everywhere on the sphere. This implies there is
discontinuity in the joint angle while the wrist travels along the sphere,
corresponding to the algorithmic singularity.

Figure 7: For each possible position of the wrist on a sphere around the
shoulder while fixing the value of the arbitrary redundancy parameterization
$\phi$, define a unit vector tangent to the sphere and pointing towards the
elbow. By Hairy Ball Theorem, this vector field cannot be continuous.

It is also straightforward to demonstrate that for the general SEW angle
specifically, any choice of $f_{x}(p_{SW})$ must have a singularity. If we
restrict the input to have some constant nonzero $\left\|{p_{SW}}\right\|$,
then $f_{x}$ defines a unit-magnitude vector field on a sphere. By the Hairy
Ball Theorem, this vector field must be discontinuous. For the conventional
SEW angle, the singularity occurs when $e_{SW}=\pm e_{r}$. If we choose $\phi$
to be the arm angle where $e_{x}$ corresponds to $e_{CE}$ when $q_{3}=0$ and
the robot has a spherical wrist, then the singularity occurs when $e_{SW}=\pm
h_{1}$.

Another common parameterization choice is $\phi(q)=q_{1}$. In this case if the
robot has a spherical wrist then the singularity occurs when $e_{SW}=\pm
R_{01}h_{2}$.

The Poincaré–Hopf theorem (Poincaré, 1885; Hopf, 1927) implies that not only
must there be a singularity on the sphere, but the total order of
singularities must be two. For this vector field, the order of the singularity
is how many times the elbow rotates about the shoulder-wrist line as the wrist
travels around the singularity once. In all the examples of $\phi$ shown
above, there are two antipodal singularities of order one.

The goal of Section 5 is to find a redundancy parameterization that has a
single singularity on this sphere of order two. This is in some sense the
best-case singularity structure, as this results in a singularity along a
half-line in the robot workspace. For most robots, this line can be chosen so
that it goes into the structure holding the robot in place, meaning the
singularity is out of the reachable workspace.

##  5 Stereographic SEW angle

###  5.1 Definition

The conventional definition of the SEW angle in Section 3.3 encounters a
singularity when the shoulder-wrist vector $p_{SW}$ becomes collinear with the
reference vector $e_{r}$. As discussed in Section 4, an algorithmic
singularity is unavoidable, but we can reduce its impact by slightly changing
the SEW angle definition so that the bidirectional line condition becomes a
unidirectional half-line condition, as shown in Figure 6b.

For the stereographic SEW angle (Figure 5b), we define the reference direction
function $f_{x}$ as

$\displaystyle e_{x}$
$\displaystyle=f_{x}(p_{SW})=\frac{k_{x}}{\left\|{k_{x}}\right\|},$ (26a)
$\displaystyle k_{x}$ $\displaystyle=k_{rt}^{\times}p_{SW},$ (26b)
$\displaystyle k_{rt}$ $\displaystyle=(e_{SW}-e_{t})^{\times}e_{r}.$ (26c)

This also means $e_{y}$ is the normalized version of
$-e_{SW}^{\times^{2}}k_{rt}$. $e_{t}$ is an arbitrary vector chosen to place
the singularity structure. We will show we need the conditions
$\left\|{e_{r}}\right\|=1$, $\left\|{e_{t}}\right\|=1$, and $e_{r}^{T}e_{t}=1$
to achieve the half-line singularity condition. If we instead set $e_{t}=0$
then (26) becomes the conventional SEW angle definition.

Using the plane definition, the stereographic SEW angle can be written as

$\psi=\operatorname*{arg\,min}_{\theta}\left\|{{\tt
R}(e_{SW},\theta)k_{rt}-k_{SEW}}\right\|,$ (27)

which may be solved using

$\psi=\mbox{atan2}(e_{SW}^{T}k_{rt}^{\times}k_{SEW},k_{SEW}^{T}k_{rt}).$ (28)

Using the elbow definition, we may equivalently write

$\psi=\mbox{atan2}(k_{rt}^{T}p_{CE},-e_{SW}^{T}k_{rt}^{\times}p_{CE}).$ (29)

For any $(e_{r},e_{t})$ with nonzero $e_{r}$ we can always pick a new
$(\tilde{e}_{r},\tilde{e}_{t})$ such that $e_{r}^{T}e_{t}=1$ and
$\left\|{e_{r}}\right\|=1$ where the direction of $k_{rt}$ is identical
according to

$\tilde{e}_{r}=\frac{e_{r}}{\left\|{e_{r}}\right\|},\quad\tilde{e}_{t}=-\tilde{e}_{r}^{\times^{2}}e_{t}.$
(30)

We can show that once we require $\left\|{e_{r}}\right\|=1$ and
$e_{r}^{T}e_{t}=0$ (without loss of generality), the half-line singularity
condition occurs if and only if $\left\|{e_{t}}\right\|=1$. The algorithmic
singularity corresponds to $k_{x}=0$, which occurs when $k_{rt}$ is a zero
vector or collinear with $e_{SW}$. We consider two cases below:

  1. 1.

$\left\|{e_{t}}\right\|\leq 1$: For two choices of $e_{SW}$, $k_{rt}=0$:

$e_{SW}=e_{t}\pm\sqrt{1-\left\|{e_{t}}\right\|^{2}}e_{r}$ (31)

This corresponds to the points on the unit sphere which intersect with the
line spanned by $e_{r}$ and translated by $e_{t}$.

  2. 2.

$\left\|{e_{t}}\right\|\geq 1$: For two choices of $e_{SW}$, $k_{rt}$ is
collinear with $e_{SW}$:

$e_{SW}=\frac{e_{t}}{\left\|{e_{t}}\right\|^{2}}\pm\sqrt{{\left\|{e_{t}}\right\|^{2}}-1}\frac{e_{t}^{\times}e_{r}}{\left\|{e_{t}}\right\|^{2}}.$
(32)

This corresponds to the points on the unit sphere which is tangent to a plane
passing through the line spanned by $e_{r}$ and translated by $e_{t}$.

There are two unique singularities if $\left\|{e_{t}}\right\|<1$ and 2 unique
singularities if $\left\|{e_{t}}\right\|>1$. By choosing
$\left\|{e_{t}}\right\|=1$, both (31) and (32) simplify to the half-line
condition $e_{SW}=e_{t}$. Geometrically, this is because the line spanned by
$e_{r}$ and translated by $e_{t}$ only passes through one point on the unit
sphere (at $e_{t}$), and the only plane tangent to the unit sphere and passing
through the line spanned by $e_{r}$ and translated by $e_{t}$ is tangent at
$e_{t}$. If we instead set $e_{t}=0$, then we recover the conventional SEW
angle.

A good choice of $e_{t}$ is to point in the opposite direction of the robot
workspace (e.g., into the ground) so that the singularity will not affect the
robot operation.

For the Jacobian, we must calculate

$-e_{y}^{T}\dot{e}_{x}=-\frac{1}{\left\|{k_{x}}\right\|}e_{y}^{T}\dot{k}_{x}.$
(33)

Recall that $k_{x}=k_{rt}^{\times}p_{SW}$, which means the derivative is

$\displaystyle\dot{k}_{x}=k_{rt}^{\times}\dot{p}_{SW}-p_{SW}^{\times}\dot{k}_{rt},$
(34)

and so

$-e_{y}^{T}\dot{e}_{x}=-\frac{(e_{y}^{\times}k_{rt})^{T}}{\left\|{k_{x}}\right\|}\dot{p}_{SW}+\frac{(e_{y}^{\times}p_{SW})^{T}}{\left\|{k_{x}}\right\|}\dot{k}_{rt}.$
(35)

We can rewrite the first term as

$\begin{split}-\frac{(e_{y}^{\times}k_{rt})^{T}}{\left\|{k_{x}}\right\|}\dot{p}_{SW}&=-\frac{e_{SW}^{T}k_{rt}}{\left\|{k_{x}}\right\|}e_{x}^{T}J^{W}\dot{q}\\\
&=\frac{e_{SW}^{T}e_{t}^{\times}e_{r}}{\left\|{k_{x}}\right\|}e_{x}^{T}J^{W}\dot{q}.\end{split}$
(36)

Notice that

$\dot{e}_{SW}=-\frac{e_{SW}^{\times^{2}}}{\left\|{p_{SW}}\right\|}\dot{p}_{SW}.$
(37)

The second term can be written as

$\begin{split}\frac{(e_{y}^{\times}p_{SW})^{T}}{\left\|{k_{x}}\right\|}\dot{k}_{rt}&=\frac{\left\|{p_{SW}}\right\|}{\left\|{k_{x}}\right\|}e_{x}^{T}\left(e_{r}^{\times}\frac{{e_{SW}^{\times}}^{2}}{\left\|{p_{SW}}\right\|}J_{W}\dot{q}\right)\\\
&=\frac{e_{SW}^{T}e_{r}}{\left\|{k_{x}}\right\|}e_{y}^{T}J_{W}\dot{q}.\end{split}$
(38)

This means the Jacobian with respect to the wrist is

$\begin{split}J_{\psi,W}={}&\frac{e_{SW}^{T}e_{r}}{\left\|{k_{x}}\right\|}e_{y}^{T}+\frac{e_{SW}^{T}e_{t}^{\times}e_{r}}{\left\|{k_{x}}\right\|}e_{x}^{T}\\\
&{}-\frac{e_{SW}^{T}p_{SE}}{\left\|{p_{SW}}\right\|\left\|{p_{CE}}\right\|}(e_{SW}^{\times}e_{CE})^{T}\end{split}$
(39)

As expected based on the previous singularity analysis, other than the
$p_{SW}=0$ and $p_{CE}=0$ conditions from the general SEW angle, $J_{\psi}$ is
only undefined when $k_{x}=0$.

In deriving the Jacobian, we made no assumptions on the norms or orthogonality
of $e_{r}$ or $e_{t}$. Setting $e_{t}=0$ lets us recover the Jacobian for the
conventional SEW angle.

###  5.2 Relationship to Stereographic Projection

The stereographic SEW angle gets its name from stereographic projection, which
is a type of one-to-one mapping between points on a sphere and a plane
(Needham, 1997). Stereographic projection has the following geometric
definition: Pick a point on the sphere as the pole of the projection and place
the plane tangent to the sphere at the antipodal point. Corresponding points
on the sphere and plane are collinear with the pole.

We can show the reference direction $e_{x}$ for the stereographic SEW angle is
generated by performing a stereographic projection of a constant vector field
onto a unit sphere (Figure 8). For any choice of $e_{t}$ and $e_{r}$, we can
generate a constant vector field in the $e_{r}$ direction on the projection
plane and place the projection pole at $e_{t}$. Then, for any choice of
$e_{SW}$, we can form a projection line passing through $e_{t}$ and $e_{SW}$
and intersecting with the projection plane. The projection of $e_{r}$ onto the
sphere is then the vector which is tangent to the circle formed by the
intersection of the unit sphere and the plane which contains the tips of
$e_{t}$ and $e_{SW}$ and is parallel to $e_{r}$. This plane is normal to
$k_{rt}$, and so the projection of $e_{r}$ from the plane to the sphere is
$e_{x}$.

Figure 8: The stereographic projection of $e_{r}$ onto $e_{SW}$ is $e_{x}$,
where the pole of the projection is at $e_{t}$.

###  5.3 Preservation of Angles

Although it appears that we have three DOF in picking the parameters for the
stereographic SEW angle (two DOF for $e_{t}$ and one remaining DOF for
$e_{r}$), we can show that the one DOF for $e_{r}$ does not change the
behavior of the SEW angle; it only changes the angle by a constant. Changing
the angle of $e_{r}$ around $e_{t}$ is equivalent to changing the SEW angle
$\psi$ by the same angle.

Since stereographic projection is a conformal mapping, changes of angles in
the projection plane get preserved after projecting onto the unit sphere. This
means changing the angle of $e_{r}$ on the projection plane corresponds to an
equal change of angle to $e_{x}$ and $e_{y}$.

We can also prove this preservation of angles property directly (Figure 9).
First, set $e_{r}=e_{r,1}$, which results in $e_{x,1}$. This vector is tangent
to the circle which is tangent to $e_{r,1}$ and is passing through $e_{SW}$
and $e_{t}$. Now, pick a new $e_{r}=e_{r,2}$ which is $e_{r,1}$ rotated by an
angle $\alpha$ about $-e_{t}$. This generates $e_{x,2}$ which is tangent to
the circle passing through $e_{SW}$ and $e_{t}$ but tangent to $e_{r,2}$. The
angle from $e_{x,1}$ to $e_{x,2}$ about $e_{SW}$ is an angle $\beta$. The
angles of intersection of any two circles on a sphere are equal, so
$\alpha=\beta$.

The preservation of angles property can aid in analyzing the behavior of SEW
angle when only $q_{1}$ changes in the typical case of $e_{t}=-h_{1}$, which
occurs when the first joint of the robot is pointing up and the singularity
direction is chosen to point down. In this case, the when $q_{1}$ changes but
all other joint angles are held constant, $\psi$ changes at the same rate.
This means the first element of $J_{\psi}$ is always 1.

We can compare this to the conventional SEW angle where $e_{r}=h_{1}$. In this
case, the SEW angle does not change when only $q_{1}$ changes. (The
conventional SEW angle has rotational invariance about $e_{r}$.) This means
the first element of $J_{\psi}$ is always 0.

Figure 9: $e_{x,i}$ is tangent to the circle generated from the plane through
$e_{t}$, $e_{t}+e_{r,i}$ and $e_{SW}$. The angles of intersections of any two
circles on a sphere are equal. Therefore, $\alpha=\beta$.

###  5.4 Singularity Behavior

The singularity for the stereographic SEW angle is of order two. This means
that if $e_{SW}$ travels in a small circle around $e_{t}$, $e_{x}$ rotates
twice for each rotation of $e_{SW}$. This is unlike the conventional SEW
angle, where each singularity is of order one.

Although a singularity of order two would result in larger elbow motion than a
singularity of order one for a fixed SEW angle as $e_{SW}$ passes close to
$e_{t}$, large motion disappears if $e_{SW}$ passes directly through $e_{t}$
in a smooth path. As a path gets closer to the singularity, $e_{x}$ gets
closer to making a full revolution of $2\pi$ radians. At the limit, the elbow
rotates by exactly $2\pi$, which is equivalent to not rotating at all. We can
compare this to the conventional SEW angle, where passing through the
singularity in a smooth path causes $e_{x}$ to rotate by $\pi$ radians.

###  5.5 Example

Two joint trajectories were generated for a KUKA LBR iiwa 14 R820 robot (KUKA,
2022) with constants $R_{07}=I$ and $\psi=\pi/4$: One using the conventional
SEW angle with $e_{r}=[0\ 0\ 1]^{T}$, and the other using the stereographic
SEW angle with $e_{t}=[0\ 0\ {-1}]^{T}$ and $e_{r}=[0\ 1\ 0]^{T}$. The task-
space trajectory for $p_{0T}$ took a straight-line path through 4 points, with
$x=\pm 0.40$ m, $y=0.01$ m, and $z=0.36\pm 0.40$ m. There was a small offset
in the $y$ direction as otherwise there would not be any large joint motion
for the stereographic SEW angle. For each trajectory, the initial pose was
picked such that $q_{2}>0$, $q_{4}>0$, and $q_{6}>0$. Results are shown in
Figure 10.

Figure 10: Joint-space trajectories using constant conventional or
stereographic SEW angles. Whereas the conventional SEW angle results in large
joint motion when the wrist passes near a line, the stereographic SEW angle
results in large joint motion when passing near a half-line.

For the conventional SEW angle, the singularities occur near $e_{SW}=\pm[0\ 0\
1]^{T}$. In following the trajectory with the conventional SEW angle, there
were large joint motions in two places, each corresponding to the positive and
negative direction.

For the stereographic SEW angle, the singularity only occurs near $e_{SW}=[0\
0\ {-1}]^{T}$. We see that the large joint motion only occurred at one time in
the trajectory. The joint motion was also larger than in the conventional SEW
case, as the singularity is of order two instead of one. The singularity
occurs when the wrist is below the base, which in many cases would be out of
the feasible workspace.

##  6 Inverse Kinematics

Table 3: Kinematic families of robots with IK solutions demonstrated in this paper. The Motoman SIA50D appears as two examples since one robot may be part of multiple kinematic families. IK Type | Kinematic Family | Robot Example
---|---|---
Closed-Form | 1. | 2R-2R-3R |  FREND (Debus and Dougherty, 2009)
|  |  |  KUKA LBR iiwa 14 R820 (KUKA, 2022)
| 2. | 3R-R-3R |  Motoman SIA5D (Yaskawa, 2022b)
| 3. |  R-R-3RE-2R |  Motoman SIA50D (Yaskawa, 2022a)
| 4. | 2R-3R-2R |
| 5. | 2R-3R||-2R |  SSRMS (Crane III, Duffy and Carnahan, 1991)
|  |  |  SPDM (Mukherji, Ray, Stieber and Lymer, 2001)
|  |  |  ERA (Boumans and Heemskerk, 1998)
| 6. |  R-R-3R||E-2R |
1D Search | 7. |  R-2R-2RE-2R |  Sawyer (Rethink Robotics, 2022)
|  |  |  Baxter (Rethink Robotics, 2015)
|  |  |  OSAM-2 (Motiv Space Systems, 2022)
|  |  |  OB7 (Productive Robots, 2023)
| 8. |  3R-RE-2R-R |  Franka Production 3 (Franka Emika, 2022)
|  |  |  xArm7 (Ufactory, 2022)
| 9. |  R-2RS-R-3R |  Motoman SIA50D (Yaskawa, 2022a)
2D Search | 10. | General 7-DOF |  ABB Yumi (ABB, 2022)
|  |  |  RRC (Robotics Research Corporation, 2005)

###  6.1 Problem Formulation

The inverse kinematics problem for a 7R robot arm is to find all possible
joint angles $q$ corresponding to an end effector pose and SEW angle
$(R_{0T},p_{0T},\psi)$ given the robot kinematic parameters
$\left(\\{p_{i-1,i}\\}_{i=1}^{7},\ p_{7T},\ \\{h_{i}\\}_{i=0}^{7},\
R_{7T}\right)$ and stereographic SEW parameters $\left(e_{r},\ e_{t},\
p_{iS},\ p_{jE},\ p_{kW}\right)$. These inverse kinematics solutions are
agnostic to the SEW formulation, and so the conventional SEW angle definition
may be used instead.

The inverse kinematics procedures can apply not just to 7-DOF manipulators,
but also to 6-DOF manipulators which have been provided an extra degree of
freedom, say, by being placed on an omnidirectional mobile base with the
location of the origin of the base specified. For example, if we use a UR5
robot (Universal Robots, 2022), tilt the robot so the first joint is not
vertical, and pick the origin of the mobile base to be directly under the
intersection of joints 1 and 2, then the system becomes a 3R-R-2R-R or
3R-2R||-2R robot.

To find the inverse kinematics of a 7-DOF robot where the redundant degree of
freedom is parameterized by some joint angle $q_{i}$, find the 6-DOF robot
generated by fixing $q_{i}$ and refer to the inverse kinematics procedures
provided in Elias and Wen (2022).

Without loss of generality, assume $p_{01}=0$. (Otherwise, subtract $p_{01}$
from $p_{0T}$ and $p_{0S}$.) Rewrite the kinematics equations in terms of
$R_{07}$ and $p_{07}$, which can be immediately calculated:

$\displaystyle R_{07}={}$ $\displaystyle
R_{0T}R_{7T}^{T}=R_{01}R_{12}R_{23}R_{34}R_{45}R_{56}R_{67},$ (40a)
$\displaystyle\begin{split}p_{07}={}&p_{0T}-R_{07}p_{7T}\\\
={}&R_{01}p_{12}+R_{02}p_{23}+R_{03}p_{34}\\\
&+R_{04}p_{45}+R_{05}p_{56}+R_{06}p_{67}.\end{split}$ (40b)

The inverse kinematics procedures can now be written in terms of $(R_{07},\
p_{07},\ \psi)$. If the shoulder is constant in the 0 frame and the wrist is
constant in the 7 frame, then we can use (9) to find $n_{SEW}$ since $\psi$ is
given $p_{SW}$ is known:

$p_{SW}=p_{07}+R_{07}p_{7W}-p_{0S}.$ (41)

A key step in the inverse kinematics procedure is to find the elbow location,
i.e., $p_{SE}$. This sometimes involves finding the shoulder angle
$\theta_{S}$ or the wrist angle $\theta_{W}$, defined as

$p_{SE}={\tt R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{SE}}\right\|,$ (42)
$p_{EW}={\tt R}(n_{SEW},\theta_{W})e_{SW}\left\|{p_{EW}}\right\|.$ (43)

Note that for a given shoulder position, wrist position, and SEW angle, the
elbow must be somewhere in a half plane: Given $n_{SEW}$ and $e_{CE}$, we
require $n_{SEW}^{T}p_{SE}=0$ and $e_{CE}^{T}p_{SE}>0$. To place the elbow in
the correct half plane, $\theta_{S}\in[0,\pi]$ and $\theta_{W}\in[-\pi,0]$. A
singularity occurs when $\theta_{S}=0$ or $\theta_{W}=0$, as this corresponds
to the shoulder, elbow, and wrist being collinear.

(a) 1\. 2R-2R-3R. (b) 2\. 3R-R-3R. (c) 3\. R-R-3RE-2R.

(d) 4\. 2R-3R-2R. (e) 5\. 2R-3R||-2R. (f) 6\. R-R-3R||E-2R.

(g) 7\. R-2R-2RE-2R. (h) 8\. 3R-RE-2R-R. (i) 9\. R-2RS-R-3R.

Figure 11:  Examples of robots in kinematic families with closed-form and 1D
search solutions.

A number of inverse kinematics solutions are provided below, and the different
kinematic families, along with examples of specific robots for some types, are
shown in Table 3 and Figure 11. There exist many other kinematic families, but
we include enough examples to demonstrate the technique of applying the
subproblem decomposition method to solve the inverse kinematics problem. The
reader is referred to the discussions in Elias and Wen (2022) for information
regarding handling multiple branches, extraneous solutions, least-squares
solutions, 1D and 2D searches, and internal and boundary singularities.

Closed-form solutions may exist when a robot has 3R or 3R|| joints, as a 3R
joint may result in decoupling between position and orientation, and a 3R||
joint is the limit of a 3R joint as the point of intersection moves infinitely
far away. (It only makes sense to place ${\mathcal{O}}_{E}$ infinitely far
away, as placing ${\mathcal{O}}_{S}$ or ${\mathcal{O}}_{W}$ infinitely far
away results in $e_{SW}$ being a constant vector in the base or end effector
frame.) In other cases, the solution requires a 1D or 2D search over $q_{i}$,
$\theta_{S}$, or $\theta_{W}$.

A common step in the IK solutions below is to solve the orientation of a 3R
joint. To solve such a spherical joint (shown here at the robot wrist)

$R_{45}R_{56}R_{67}=R_{47},$ (44)

first solve for $(q_{5},q_{6})$ using Subproblem 2

$R_{56}h_{7}=R_{54}R_{47}h_{7}.$ (45)

Then, solve for $q_{7}$ using Subproblem 1:

$R_{67}p=R_{65}R_{54}R_{47}p,$ (46)

where $p$ is any vector not collinear with $h_{7}$.

In the following IK solutions, always pick the origins of intersecting axes
for that kinematic family to be coincident. For example, in a 3R-R-3R robot,
pick $\mathcal{O}_{1}=\mathcal{O}_{2}=\mathcal{O}_{3}=\mathcal{O}_{S}$, which
means $p_{12}=p_{23}=p_{0S}=0$. When an elbow is made of parallel joints (say,
that include joint 3), pick $p_{SE}=e_{SE}=R_{02}h_{3}$.

###  6.2 IK Solutions

####  6.2.1 2R-2R-3R Arm

The position equation becomes

$p_{07}=p_{SW}=R_{02}(p_{23}+R_{24}p_{45}),$ (47)

and given $\theta_{S}$ we have

$R_{02}p_{23}={\tt R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{23}}\right\|.$ (48)

Use Subproblem 3 to find $\theta_{S}\in[0,\pi]$:

$\left\|{{\tt
R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{23}}\right\|-p_{07}}\right\|=\left\|{p_{45}}\right\|.$
(49)

Find $(q_{1},q_{2})$ using Subproblem 2:

$R_{12}p_{23}=R_{10}{\tt R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{23}}\right\|.$
(50)

Similarly, find $(q_{3},q_{4})$ using Subproblem 2:

$R_{32}(R_{21}R_{10}p_{07}-p_{24})=R_{34}p_{45}.$ (51)

Finally, find $(q_{5},q_{6},q_{7})$ by solving the spherical wrist:

$R_{45}R_{56}R_{67}=(R_{01}R_{12}R_{23}R_{34})^{T}R_{07}.$ (52)

####  6.2.2 3R-R-3R Arm

The position equation becomes

$p_{07}=p_{SW}=R_{03}(p_{34}+R_{34}p_{45}).$ (53)

Solve for up to two solutions of $q_{4}$ using Subproblem 3:

$\left\|{R_{34}p_{45}+p_{34}}\right\|=\left\|{p_{07}}\right\|.$ (54)

Represent $R_{03}$ as three consecutive orthogonal rotations:

$R_{03}={\tt R}(e_{SW},\theta_{a}){\tt R}(n_{SEW},\theta_{b}){\tt
R}(e_{SW},\theta_{c}).$ (55)

There are up to two solutions for $(\theta_{a},\theta_{b},\theta_{c})$, but
they represent the same $R_{03}$, so only keep one solution. Solve for
$(\theta_{b},\theta_{c})$ using Subproblem 2:

${\tt R}(n_{SEW},\theta_{b})^{T}p_{07}={\tt
R}(e_{SW},\theta_{c})(p_{34}+R_{34}p_{45}).$ (56)

Then, use Subproblem 4 to find $\theta_{a}$, keeping only the solutions that
place the elbow in the correct half plane:

$n_{SEW}^{T}{\tt R}(e_{SW},\theta_{a}){\tt R}(n_{SEW},\theta_{b}){\tt
R}(e_{SW},\theta_{c})p_{3E}=0.$ (57)

Find $(q_{1},q_{2},q_{3})$ by solving the spherical shoulder:

$R_{01}R_{12}R_{23}={\tt R}(e_{SW},\theta_{a}){\tt R}(n_{SEW},\theta_{b}){\tt
R}(e_{SW},\theta_{c}).$ (58)

Similarly, find $(q_{5},q_{6},q_{7})$ by solving (52).

####  6.2.3 R-R-3RE-2R Arm

Write $\theta_{W}$ as

$R_{05}p_{56}={\tt R}(n_{SEW},\theta_{W})e_{SW}\left\|{p_{56}}\right\|.$ (59)

Using Subproblem 5, we can find up to four solutions of
$(q_{1},q_{2},\theta_{W})$, where $\theta_{W}\in[-\pi,0]$:

$p_{07}-{\tt R}(n_{SEW},\theta_{W})e_{SW}\left\|{p_{56}}\right\|\\\
=R_{01}(p_{12}+R_{12}p_{23}).$ (60)

Next, find up to two solutions of $(q_{6},q_{7})$ using Subproblem 2:

$R_{67}R_{07}^{T}{\tt
R}(n_{SEW},\theta_{W})e_{SW}\left\|{p_{56}}\right\|=R_{65}p_{56}.$ (61)

To solve for $(q_{3},q_{4},q_{5})$, solve the spherical elbow:

$R_{23}R_{34}R_{45}=(R_{01}R_{12})^{T}R_{07}(R_{56}R_{67})^{T}.$ (62)

####  6.2.4 2R-3R-2R Arm

Find $\theta_{S}\in[0,\pi]$ using Subproblem 3:

$\left\|{{\tt
R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{23}}\right\|-p_{07}}\right\|=\left\|{p_{56}}\right\|.$
(63)

Find $(q_{1},q_{2})$ with Subproblem 2 to solve (50). Similarly, find
$(q_{6},q_{7})$:

$R_{67}R_{07}^{T}(p_{07}-R_{02}p_{23})=R_{65}p_{56}.$ (64)

Find $(q_{3},q_{4},q_{5})$ by solving (62).

####  6.2.5 2R-3R||-2R Arm

This robot is the limit of a 2R-3R-2R arm where the intersection point of the
elbow joint moves to infinity. We can write

$R_{01}R_{12}h_{3}={\tt R}(n_{SEW},\theta_{S})e_{SW}.$ (65)

Combining with the position equation gives

$h_{3}^{T}(p_{23}+p_{34}+p_{45}+p_{56})=h_{3}^{T}R_{02}^{T}p_{07}.$ (66)

Solve for $\theta_{S}\in[0,\pi]$ using Subproblem 4:

$e_{SW}^{T}{\tt
R}(n_{SEW},\theta_{S})^{T}p_{07}=h_{3}^{T}(p_{23}+p_{34}+p_{45}+p_{56}).$ (67)

Find $(q_{1},q_{2})$ using Subproblem 2:

$R_{12}h_{3}=R_{10}{\tt R}(n_{SEW},\theta_{S})e_{SW}.$ (68)

Find $(q_{3}+q_{4}+q_{5},q_{6},q_{7})$ by solving a spherical joint:

$R_{25}R_{56}R_{67}=R_{02}^{T}R_{07}.$ (69)

Use Subproblem 3 to find $q_{4}$:

$\left\|{p_{34}+R_{34}p_{45}}\right\|=\left\|{R_{02}^{T}p_{07}-p_{23}-R_{25}p_{56}}\right\|.$
(70)

Use Subproblem 1 to find $q_{3}$:

$R_{23}(p_{34}+R_{34}p_{45})=R_{02}^{T}p_{07}-p_{23}-R_{25}p_{56}.$ (71)

Find $q_{5}$ with subtraction, wrapping to $[-\pi,\pi]$ if desired.

####  6.2.6 R-R-3R||E-2R Arm

This is a more general version of a 2R-3R||-2R robot. Use Subproblem 6 to find
$(\theta_{S},q_{2})$, where $\theta_{S}\in[0,\pi]$:

$e_{SW}^{T}{\tt R}(n_{SEW},\theta_{S})^{T}p_{07}-h_{3}^{T}R_{21}p_{12}\\\
=h_{3}^{T}(p_{23}+p_{34}+p_{45}+p_{56}),$ (72a) $h_{1}^{T}{\tt
R}(n_{SEW},\theta_{S})\ e_{SW}-h_{1}^{T}R_{12}h_{3}=0.$ (72b)

Use Subproblem 1 to find $q_{1}$ solving (65). Find
$(q_{3}+q_{4}+q_{5},q_{6},q_{7})$ by solving a spherical joint (69). Use
Subproblem 3 to find $q_{4}$:

$\left\|{p_{34}+R_{34}p_{45}}\right\|\\\
=\left\|{R_{02}^{T}p_{07}-R_{21}p_{12}-p_{23}-R_{25}p_{56}}\right\|.$ (73)

Use Subproblem 1 to find $q_{3}$:

$R_{23}(p_{34}+R_{34}p_{45})\\\
=R_{02}^{T}p_{07}-R_{21}p_{12}-p_{23}-R_{25}p_{56}.$ (74)

Find $q_{5}$ with subtraction, wrapping to $[-\pi,\pi]$ if desired.

####  6.2.7 R-2R-2RE-2R Arm (1D Search)

Figure 12: Error function for a Sawyer (R-2R-2RE-2R) arm. This function has
eight branches, although only four are seen for this pose. A 1D search may be
used to find the zeros, which correspond to IK solutions.

(a) $\theta_{W}=-1.0371$. (b) $\theta_{W}=-1.0166$. (c) $\theta_{W}=-0.9797$.

(d) $\theta_{W}=-0.9304$. (e) $\theta_{W}=-0.7450$. (f) $\theta_{W}=-0.6936$.

Figure 13: Six IK solutions for a Sawyer (R-2R-2RE-2R) arm corresponding to
the zeros of the error shown in Figure 12.

Given $\theta_{W}$, we can write

$R_{05}p_{56}={\tt R}(n_{SEW},\theta_{W})e_{SW}\left\|{p_{56}}\right\|.$ (75)

Then, find $q_{1}$ using Subproblem 3:

$\left\|{R_{01}p_{12}-p_{07}+R_{05}p_{56}}\right\|=\left\|{p_{34}}\right\|.$
(76)

Find $(q_{2},q_{3})$ using Subproblem 2:

$R_{23}p_{34}=R_{21}(R_{10}p_{07}-R_{10}R_{05}p_{56}-p_{12}).$ (77)

Find $(q_{4},q_{5})$ using Subproblem 2:

$R_{45}p_{56}=R_{43}(R_{32}R_{21}(R_{10}p_{07}-p_{12})-p_{34}).$ (78)

The error is a metric of solvability of

$R_{56}R_{67}=R_{05}^{T}R_{07}.$ (79)

By projecting onto $h_{6}$ and $h_{7}$, we get the error

$e(\theta_{W})=h_{6}^{T}R_{05}^{T}R_{07}h_{7}-h_{6}^{T}h_{7}.$ (80)

Search over $\theta_{W}\in[-\pi,0]$ to find all solutions of
$e(\theta_{W})=0$. For each solution $\theta_{W}$, calculate $q_{6}$ and
$q_{7}$ using Subproblem 1:

$R_{56}h_{7}=R_{05}^{T}R_{07}h_{7},$ (81) $R_{76}h_{6}=R_{07}^{T}R_{05}h_{6}.$
(82)

A Sawyer arm example is shown in Figures 12 and 13.

####  6.2.8 3R-RE-2R-R Arm (1D Search)

Given $\theta_{S}\in[0,\pi]$, we have

$R_{03}p_{34}={\tt R}(n_{SEW},\theta_{S})e_{SW}\left\|{p_{34}}\right\|.$ (83)

Then use Subproblem 3 to find $q_{7}$ from

$\left\|{R_{07}^{T}(p_{07}-R_{03}p_{34})-R_{76}p_{67}}\right\|=\left\|{p_{45}}\right\|.$
(84)

Solve $(q_{5},q_{6})$ using Subproblem 2:

$R_{54}p_{45}=R_{56}(R_{67}R_{07}^{T}(p_{07}-{R_{03}p_{34}})-p_{67}).$ (85)

We have the identity

$R_{43}p_{34}=R_{47}R_{07}^{T}R_{03}p_{34},$ (86)

which may be expressed as an error function in terms of $\theta_{S}$:

$e(\theta_{S})=h_{4}^{T}(p_{34}-R_{47}R_{07}^{T}R_{03}p_{34}).$ (87)

Search over $\theta_{S}\in[0,\pi]$ to find the zeros of this error. For each
solution $\theta_{S}$, find $q_{4}$ with Subproblem 1, then find
$(q_{1},q_{2},q_{3})$ by solving the spherical shoulder:

$R_{01}R_{12}R_{23}=R_{07}(R_{34}R_{45}R_{56}R_{67})^{T}.$ (88)

####  6.2.9 R-2RS-R-3R Arm (1D Search)

Given $q_{1}\in[-\pi,\pi]$, solve for $q_{4}$ using Subproblem 3:

$\left\|{p_{34}+R_{34}p_{45}}\right\|=\left\|{R_{10}p_{07}-p_{12}}\right\|.$
(89)

Solve for $(q_{2},q_{3})$ using Subproblem 2:

$R_{21}(R_{10}p_{07}-p_{12})=R_{23}(p_{34}+R_{34}p_{45}).$ (90)

Find the shoulder-elbow vector $p_{SE}=R_{03}p_{34}$, from which we can
calculate the error for $\psi$ in terms of $q_{1}$. A plot of this error is a
good graphical tool to determine what range of SEW angle is feasible for a
given end effector pose. For each solution $q_{1}$, find $(q_{5},q_{6},q_{7})$
by solving (52).

####  6.2.10 General 7-DOF Arm (2D Search)

Pick $\mathcal{O}_{1}=\mathcal{O}_{S}$, $\mathcal{O}_{4}=\mathcal{O}_{E}$, and
$\mathcal{O}_{7}=\mathcal{O}_{W}$. Given $(q_{1},q_{2})$, find $q_{3}$ using
Subproblem 4, keeping only solutions which place the elbow in the correct half
plane:

$n_{SEW}^{T}R_{02}R_{23}p_{34}=-n_{SEW}^{T}(R_{01}p_{12}+R_{02}p_{23}).$ (91)

Then, find $(q_{5},q_{6},q_{7})$ with Subproblem 5 to solve

$-p_{67}+R_{67}R_{07}^{T}(p_{07}-p_{14})=R_{65}(p_{56}+R_{54}p_{45}),$ (92)

and find the error

$e(q_{1},q_{2})=\left\|{R_{03}^{T}R_{07}R_{47}^{T}h_{4}-h_{4}}\right\|.$ (93)

Search over $(q_{1},q_{2})$ to find zeros of this error. Then, find $q_{4}$
using Subproblem 1:

$R_{34}p=R_{03}^{T}R_{07}R_{47}^{T}p,$ (94)

where $p$ is any vector not collinear with $h_{4}$.

###  6.3 Least-Squares Inverse Kinematics

Any IK solution which only uses Subproblems 1–4 is robust to boundary
singularities if the least-squares solutions for these subproblems is used, as
discussed in Elias and Wen (2022). For some special cases, the IK solution is
also the solution to the global least-squares IK problem, which is posed as

$\displaystyle\min_{q}$ $\displaystyle\left\|{p_{0T}(q)-p_{0T}^{des}}\right\|$
(95) s.t. $\displaystyle R_{0T}(q)=R_{0T}^{des},\ \psi(q)=\psi^{des}$

where $(R_{0T}^{des},p_{0T}^{des},\psi^{des})$ is the desired end effector
pose and SEW angle. For some robots, the $\psi(q)=\psi^{des}$ constraint
cannot be achieved when at a boundary singularity because the self-motion
manifold degenerates to a point, and so this constraint must be dropped when
$p_{0T}(q)\neq p_{0T}^{des}$.

3R-R-3R, 2R-3R-2R, and 2R-2R-3R arms can all achieve least-squares inverse
kinematics if the task frame is at the wrist center ($p_{7T}=0$), all 2R
joints have a spherical workspace
($h_{i}^{T}h_{i+1}=h_{i+1}^{T}p_{i+1,i+2}=0$) and all 3R joints can achieve
any orientation ($h_{i}^{T}h_{i+1}=h_{i+1}^{T}h_{i+2}=0$).

For a 3R-R-3R arm, since rotation of the whole arm is always possible between
the spherical shoulder and wrist, the self-motion manifold does not degenerate
to a point at boundary singularities. Therefore, ${\mathcal{O}}_{E}$ should be
placed such that it is not collinear with ${\mathcal{O}}_{S}$ and
${\mathcal{O}}_{W}$ at the workspace boundary. (Using
$p_{SE}=e_{SE}=R_{03}h_{4}$ may also be a good option.)

For 2R-3R-2R and 2R-2R-3R arms, the robot’s self-motion manifold degenerates
into a point at boundary singularities, so although the SEW angle will be
undefined at the boundary singularity, the SEW angle is not needed. This also
means the SEW angle constraint in (95) must be dropped.

##  7 Conclusion

We have introduced the general SEW angle which allows us to analyze the
behavior of the conventional SEW angle but with arbitrary reference direction
function. A special choice of the reference direction function, the
stereographic SEW angle, reduces the effect of the coordinate singularity as
compared to the conventional SEW angle. The stereographic SEW angle allows the
use of more of the workspace without encountering singularities. Even at a
singularity, the arm can have continuous joint movement as long as a smooth
path is taken directly through the singularity. We have shown that since an
algorithmic singularity is unavoidable for any choice of parameterization of
the redundant degree of freedom, the stereographic SEW angle is ideal in that
it only encounters a singularity when the wrist is at a half-line from the
shoulder.

We have also used the subproblem decomposition method to provide IK solutions
for most known 7R robots. These solutions are often closed-form and may
sometimes require a 1D or 2D search. This method finds all IK solutions and
finds least-squares solutions for some robots as well. We provide IK solutions
for both common robots as well as robots which do not seem to be manufactured
yet, such as a R-R-3R||-2R arm.

There exist other 7R robots which may have similar solutions to the ones
provided above. It may be worthwhile to find a closed-form or polynomial root-
finding IK solution to the robots which are solved in this paper using a 1D or
2D search.

The authors declare that there is no conflict of interest. The author(s)
received no financial support for the research, authorship, and/or publication
of this article.

## References

  * ABB (2022) ABB (2022) IRB 14000 YuMi - collaborative robot.  https://new.abb.com/products/robotics/collaborative-robots/yumi/irb-14000-yumi.  (accessed Oct. 31, 2022).
  * An et al. (2014) An HH, Clement WI and Reed B (2014) Analytical inverse kinematic solution with self-motion constraint for the 7-DOF restore robot arm.  In: _2014 IEEE/ASME International Conference on Advanced Intelligent Mechatronics_. IEEE, pp. 1325–1330.
  * Baillieul (1985) Baillieul J (1985) Kinematic programming alternatives for redundant manipulators.  In: _Proceedings. 1985 IEEE International Conference on Robotics and Automation_ , volume 2. IEEE, pp. 722–728.
  * Boumans and Heemskerk (1998) Boumans R and Heemskerk C (1998) The European robotic arm for the international space station.  _Robotics and Autonomous systems_ 23(1-2): 17–27.
  * Burdick and Seraji (1989) Burdick J and Seraji H (1989) Characterization and control of self-motions in redundant manipulators.  In: _Proceedings of the NASA Conference on Space Telerobotics, Volume 2_. pp. 3–14.
  * Chen et al. (2023) Chen Y, Zhang X, Huang Y, Wu Y and Ota J (2023) Kinematics optimization of a novel 7-DOF redundant manipulator.  _Robotics and Autonomous Systems_ 163: 104377.
  * Chiaverini (1997) Chiaverini S (1997) Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators.  _IEEE Transactions on Robotics and Automation_ 13(3): 398–410.
  * Choi et al. (2004) Choi Y, Oh Y, Oh SR, Park J and Chung WK (2004) Multiple tasks manipulation for a robotic manipulator.  _Advanced Robotics_ 18(6): 637–653.
  * Crane III et al. (1991) Crane III CD, Duffy J and Carnahan T (1991) A kinematic analysis of the space station remote manipulator system (SSRMS).  _Journal of Robotic Systems_ 8(5): 637–658.
  * Debus and Dougherty (2009) Debus T and Dougherty S (2009) Overview and performance of the front-end robotics enabling near-term demonstration (FREND) robotic arm.  In: _AIAA Infotech@Aerospace Conference and AIAA Unmanned…Unlimited Conference_. p. 1870.
  * Elias and Wen (2022) Elias AJ and Wen JT (2022) Canonical subproblems for robot inverse kinematics.  _arXiv preprint arXiv:2211.05737_ .
  * FANUC (2022) FANUC (2022) FANUC R-1000iA/120F-7B robot.  https://www.fanucamerica.com/products/robots/series/r-1000ia/r-1000ia-120f-7b-7-axis-robot.  (accessed Dec. 21, 2022).
  * Faria et al. (2018) Faria C, Ferreira F, Erlhagen W, Monteiro S and Bicho E (2018) Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance.  _Mechanism and Machine Theory_ 121: 317–334.
  * Flacco et al. (2012) Flacco F, De Luca A and Khatib O (2012) Motion control of redundant robots under joint constraints: Saturation in the null space.  In: _2012 IEEE International Conference on Robotics and Automation_. IEEE, pp. 285–292.
  * Franka Emika (2022) Franka Emika (2022) Franka production 3.  https://www.franka.de/production.  (accessed Dec. 21, 2022).
  * Gong et al. (2019) Gong M, Li X and Zhang L (2019) Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator.  _IEEE Access_ 7: 18662–18674.
  * Gottlieb (1986) Gottlieb DH (1986) Robots and fibre bundles.  _Bull. Soc. Math. Belg_ 38: 219–223.
  * Hollerbach and Suh (1987) Hollerbach J and Suh K (1987) Redundancy resolution of manipulators through torque optimization.  _IEEE Journal on Robotics and Automation_ 3(4): 308–316.
  * Hollerbach (1985) Hollerbach JM (1985) Optimum kinematic design for a seven degree of freedom manipulator.  In: _Robotics research: The second international symposium_. Citeseer, pp. 215–222.
  * Hopf (1927) Hopf H (1927) Vektorfelder inn-dimensionalen mannigfaltigkeiten.  _Mathematische Annalen_ 96(1): 225–249.
  * Jiang et al. (2013) Jiang L, Huo X, Liu Y and Liu H (2013) An integrated inverse kinematic approach for the 7-DOF humanoid arm with offset wrist.  In: _2013 IEEE International Conference on Robotics and Biomimetics (ROBIO)_. IEEE, pp. 2737–2742.
  * Jin et al. (2020) Jin M, Liu Q, Wang B and Liu H (2020) An efficient and accurate inverse kinematics for 7-DOF redundant manipulators based on a hybrid of analytical and numerical method.  _IEEE Access_ 8: 16316–16330.
  * Kim et al. (2011) Kim H, Miller LM, Al-Refai A, Brand M and Rosen J (2011) Redundancy resolution of a human arm for controlling a seven DOF wearable robotic system.  In: _2011 Annual International Conference of the IEEE Engineering in Medicine and Biology Society_. IEEE, pp. 3471–3474.
  * Kim et al. (2012) Kim H, Miller LM, Byl N, Abrams GM and Rosen J (2012) Redundancy resolution of the human arm and an upper limb exoskeleton.  _IEEE Transactions on Biomedical Engineering_ 59(6): 1770–1779.
  * Kreutz-Delgado et al. (1990) Kreutz-Delgado K, Long M and Seraji H (1990) Kinematic analysis of 7 DOF anthropomorphic arms.  In: _Proceedings., IEEE International Conference on Robotics and Automation_. IEEE, pp. 824–830.
  * Kreutz-Delgado et al. (1992) Kreutz-Delgado K, Long M and Seraji H (1992) Kinematic analysis of 7-DOF manipulators.  _The International Journal of Robotics Research_ 11(5): 469–481.
  * KUKA (2022) KUKA (2022) LBR iiwa.  https://www.kuka.com/en-us/products/robotics-systems/industrial-robots/lbr-iiwa.  (accessed Oct. 31, 2022).
  * Lamperti et al. (2015) Lamperti C, Zanchettin AM and Rocco P (2015) A redundancy resolution method for an anthropomorphic dual-arm manipulator based on a musculoskeletal criterion.  In: _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 1846–1851.
  * Li et al. (2022) Li S, Han K, He P, Li Z, Liu Y and Xiong Y (2022) Human-like redundancy resolution: An integrated inverse kinematics scheme for anthropomorphic manipulators with radial elbow offset.  _Advanced Engineering Informatics_ 54: 101812.
  * Li et al. (2013) Li Z, Roldan JR, Milutinović D and Rosen J (2013) The rotational axis approach for resolving the kinematic redundancy of the human arm in reaching movements.  In: _2013 35th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC)_. IEEE, pp. 2507–2510.
  * Liu et al. (2017) Liu W, Chen D and Steil J (2017) Analytical inverse kinematics solver for anthropomorphic 7-DOF redundant manipulators with human-like configuration constraints.  _Journal of Intelligent & Robotic Systems_ 86(1): 63–79.
  * Ma et al. (2021) Ma B, Xie Z, Jiang Z and Liu H (2021) Precise semi-analytical inverse kinematic solution for 7-DOF offset manipulator with arm angle optimization.  _Frontiers of Mechanical Engineering_ 16(3): 435–450.
  * Marani et al. (2003) Marani G, Kim J, Yuh J and Chung WK (2003) Algorithmic singularities avoidance in task-priority based controller for redundant manipulators.  In: _Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453)_ , volume 4. IEEE, pp. 3570–3574.
  * McGrath (2016) McGrath P (2016) An extremely short proof of the hairy ball theorem.  _The American Mathematical Monthly_ 123(5): 502–503.
  * Motiv Space Systems (2022) Motiv Space Systems (2022) xLink space-rated modular robotic arm system.  https://motivss.com/products-capabilities/robotics/xlink/.  (accessed Dec. 21, 2022).
  * Mukherji et al. (2001) Mukherji R, Ray DA, Stieber M and Lymer J (2001) Special purpose dexterous manipulator (SPDM) advanced control features and development test results.  _Proceedings of the 6th International Symposium on Artificial Intelligence, Robotics and Automation in Space (i-SAIRAS)_ .
  * Nammoto and Kosuge (2012) Nammoto T and Kosuge K (2012) An analytical solution for a redundant manipulator with seven degrees of freedom.  _International Journal of Automation and Smart Technology_ 2(4): 339–346.
  * Needham (1997) Needham T (1997) _Visual complex analysis_.  Oxford University Press.
  * Pfurner (2016) Pfurner M (2016) Closed form inverse kinematics solution for a redundant anthropomorphic robot arm.  _Computer Aided Geometric Design_ 47: 163–171.
  * Pieper (1969) Pieper DL (1969) _The kinematics of manipulators under computer control_.  Stanford University.
  * Poincaré (1885) Poincaré H (1885) Sur les courbes définies par les équations différentielles.  _J. Math. Pures Appl._ 4: 167–244.
  * Productive Robots (2023) Productive Robots (2023) OB7 collaborative robots.  https://www.productiverobotics.com/ob7-products/.  (accessed Jun. 21, 2023).
  * Rethink Robotics (2015) Rethink Robotics (2015) Baxter overview.  https://sdk.rethinkrobotics.com/wiki/Baxter_Overview.  (accessed Dec. 21, 2022).
  * Rethink Robotics (2022) Rethink Robotics (2022) Sawyer, the high performance collaborative robot.  https://www.rethinkrobotics.com/sawyer.  (accessed Dec. 21, 2022).
  * Robotics Research Corporation (2005) Robotics Research Corporation (2005) Dexterous manipulators and advanced control systems.  Technical report, Robotics Research Corporation.  URL http://www.robotics-research.com/RRCTechDoc.PDF.  Accessed Oct. 31, 2022.
  * Shi et al. (2021) Shi X, Guo Y, Chen X, Chen Z and Yang Z (2021) Kinematics and singularity analysis of a 7-DOF redundant manipulator.  _Sensors_ 21(21): 7257.
  * Shimizu et al. (2007) Shimizu M, Yoon WK and Kitagaki K (2007) A practical redundancy resolution for 7 DOF redundant manipulators with joint limits.  In: _Proceedings 2007 IEEE International Conference on Robotics and Automation_. IEEE, pp. 4510–4516.
  * Sinha and Chakraborty (2019) Sinha A and Chakraborty N (2019) Geometric search-based inverse kinematics of 7-DoF redundant manipulator with multiple joint offsets.  In: _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 5592–5598.
  * Su et al. (2018) Su H, Enayati N, Vantadori L, Spinoglio A, Ferrigno G and De Momi E (2018) Online human-like redundancy optimization for tele-operated anthropomorphic manipulators.  _International Journal of Advanced Robotic Systems_ 15(6): 1729881418814695\.
  * Tian et al. (2021) Tian X, Xu Q and Zhan Q (2021) An analytical inverse kinematics solution with joint limits avoidance of 7-DOF anthropomorphic manipulators without offset.  _Journal of the Franklin Institute_ 358(2): 1252–1272.
  * Tondu (2006) Tondu B (2006) A closed-form inverse kinematic modelling of a 7R anthropomorphic upper limb based on a joint parametrization.  In: _2006 6th IEEE-RAS International Conference on Humanoid Robots_. IEEE, pp. 390–397.
  * Ufactory (2022) Ufactory (2022) Ufactory xArm 7.  https://www.ufactory.cc/product-page/ufactory-xarm-7.  (accessed Dec. 21, 2022).
  * Universal Robots (2022) Universal Robots (2022) UR5 collaborative robot arm.  https://www.universal-robots.com/products/ur5-robot/.  (accessed Oct. 31, 2022).
  * Wang et al. (2019) Wang C, Peng L, Hou ZG, Li J, Luo L, Chen S and Wang W (2019) Kinematic redundancy analysis during goal-directed motion for trajectory planning of an upper-limb exoskeleton robot.  In: _2019 41st Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC)_. IEEE, pp. 5251–5255.
  * Wang (2013) Wang Y (2013) Closed-form inverse kinematic solution for anthropomorphic motion in redundant robot arms.  Technical report, Arizona State University.
  * Wang et al. (2021) Wang Y, Zhao C, Wang X, Zhang P, Li P and Liu H (2021) Inverse kinematics of a 7-DOF spraying robot with 4R 3-DOF non-spherical wrist.  _Journal of Intelligent & Robotic Systems_ 101(4): 1–17.
  * Wang and Kazerounian (1995) Wang Z and Kazerounian K (1995) Identification and resolution of structural and algorithmic singularity in redundancy control of serial manipulations.  _Journal of robotic systems_ 12(7): 465–478.
  * Xiong et al. (2020) Xiong G, Zhou Y and Yao J (2020) Null-space impedance control of 7-degree-of-freedom redundant manipulators based on the arm angles.  _International Journal of Advanced Robotic Systems_ 17(3): 1729881420925297\.
  * Xu et al. (2014) Xu W, She Y and Xu Y (2014) Analytical and semi-analytical inverse kinematics of SSRMS-type manipulators with single joint locked failure.  _Acta Astronautica_ 105(1): 201–217.
  * Yan et al. (2014) Yan L, Mu Z and Xu W (2014) Analytical inverse kinematics of a class of redundant manipulator based on dual arm-angle parameterization.  In: _2014 IEEE International Conference on Systems, Man, and Cybernetics (SMC)_. IEEE, pp. 3744–3749.
  * Yaskawa (2022a) Yaskawa (2022a) Motoman SIA50D 7-axis robot arm.  https://www.motoman.com/en-us/products/robots/industrial/assembly-handling/sia-series/sia50d.  (accessed Dec. 21, 2022).
  * Yaskawa (2022b) Yaskawa (2022b) Motoman SIA5D 7-axis robot arm.  https://www.motoman.com/en-us/products/robots/industrial/assembly-handling/sia-series/sia5d.  (accessed Dec. 21, 2022).
  * Zhao et al. (2023) Zhao J, Yang X, Zhao Z, Yang G and Zhao L (2023) Inverse kinematics and multi-objective configuration optimization of the SSRMS manipulator.  _Advances in Space Research_ .