3D rotation for arm

Question

I am trying to model the behavior of a 3d arm. It is characterized by four parts, having lengths h2, h3, h5, h6. The figure shows cylinders but I ask you to consider them as segments. The first and third cylinders from the bottom can rotate. I managed to write the right equations, but I can't find how to describe the position of the tip of the last cylinder (the top one) as a function of the angles formed between the cylinders (a2, a3, a5) and of the two rotations ( rotation, rotation2). How should I model the rotation considering the axis of the rotation depends on the rotation of the previous cylinderr and on the previous angles?

3D Drawing example

x2 = 0

y2 = 0

z2 = h2

x3 = (h3*sin((a2))) * cos((rotation))

y3 = (h3*sin((a2))) * sin((rotation))

z3 = h2 + h3*cos((a2))

x5 = ((h3sin((a2))) + h5sin(((a2+a3)))) * cos((rotation))

y5 = ((h3sin((a2))) + h5sin(((a2+a3)))) * sin((rotation))

z5 = h2 + h3cos((a2)) + h5cos((a2+a3))

x6 = ((h3sin((a2)) + h5sin(((a2+a3)))+ h6sin((a5+a3+a2)))) cos((rotation))

y6 = ((h3sin((a2)) + h5sin(((a2+a3))) + h6*sin((a5+a3+a2))))*sin((rotation))

z6 = h2 + h3cos((a2)) + h5cos((a2+a3)) + h6*(cos((a2+a3+a5)))

This is working but I need to add something to x6, y6 and z6, to make it rotatin according to the rotation of the 3rd cylinder from the bottom.

Thanks.

Answer 1

The simplest method is to use two unit vectors per joint to describe the orientation of the joint.

To do that, we need to look at the basic math first.

To rotate an unit vector $\hat{v}$ by angle $\theta$ around unit vector $\hat{a}$, you can use Rodrigues' rotation formula: $$\hat{v}^\prime = \hat{v} \cos \theta + (\hat{a} \times \hat{v}) \sin \theta + \hat{a} ( \hat{a} \cdot \hat{v} )(1 - \cos \theta) \tag{a}\label{BtVa}$$ where $\times$ denotes vector cross product, and $\cdot$ denotes vector dot product (and $\hat{v}^\prime$ is the vector $\hat{v}$ after the rotation). For simplicity, I'll use notation $$\hat{v}^\prime = R(\hat{v}, \theta, \hat{a}) \tag{b}\label{BtVb}$$ for $\eqref{BtVa}$, as in "rotate $\hat{v}$ by $\theta$ around unit axis $\hat{a}$".

Let's say the initial segment starts at origin ($\vec{0} = (0, 0, 0)$), in direction $\hat{v}_0 = (0, 0, 1)$, with $\hat{u}_0 = (1, 0, 0)$ perpendicular to $\hat{v}_0$. Let's say the initial segment is rotated by $\theta_0$ around its axis ($\hat{v}_0$), and its length is $L_0$. The end point of the initial segment is $\vec{p}_1$, $$\vec{p}_1 = L_1 \vec{v}_1 \tag{1a}\label{BtV1a}$$ and the basis vectors at that point are $\hat{u}_1$ and $\hat{v}_1$, $$\left\lbrace \, \begin{aligned} \hat{u}_1 &= R(\hat{u}_0, \theta_1, \hat{v}_0) \\ \hat{v}_1 &= \hat{v}_0 \\ \end{aligned} \right . \tag{1b}\label{BtV1b}$$

Let's say the second segment starts with an elbow joint, and has length $L_2$. Given elbow angle $\theta_2$, the end point and basis vectors there are $$\left\lbrace \, \begin{aligned} \hat{u}_2 &= \hat{u}_1 \\ \hat{v}_2 &= R(\hat{v}_1, \theta_2, \hat{u}_1) \\ \vec{p}_2 &= \vec{p}_1 + L_2 \hat{v}_2 \\ \end{aligned} \right . \tag{2}\label{BtV2}$$

Let's say the third segment starts with a rotation, and has length $L_3$. Given rotation angle $\theta_3$, the end point as basis vectors there are $$\left\lbrace \, \begin{aligned} \hat{u}_3 &= R(\hat{u}_2, \theta_3, \hat{v}_2) \\ \hat{v}_3 &= \hat{v}_2 \\ \vec{p}_3 &= \vec{p}_2 + L_3 \hat{v}_3 \\ \end{aligned} \right . \tag{3}\label{BtV3}$$ and so on.

Essentially, if the segment $k$ starts with a rotation around its axis, you apply $$\left\lbrace \, \begin{aligned} \hat{u}_k &= R(\hat{u}_{k-1}, \theta_k, \hat{v}_{k-1}) \\ \hat{v}_k &= \hat{v}_{k-1} \\ \vec{p}_k &= \vec{p}_{k-1} + L_k \hat{v}_k \\ \end{aligned} \right . \tag{rotation}\label{BtVrotation}$$ but if it starts with an elbow joint, you apply $$\left\lbrace \, \begin{aligned} \hat{u}_k &= \hat{u}_{k-1} \\ \hat{v}_k &= R(\hat{v}_{k-1}, \theta_k, \hat{u}_{k-1}) \\ \vec{p}_k &= \vec{p}_{k-1} + L_k \hat{v}_k \\ \end{aligned} \right . \tag{elbow}\label{BtVelbow}$$

We can combine both rotation and elbow (assuming rotation comes first). If we use $\theta_k$ for the rotation and $\varphi_k$ for the elbow angle, then the end point and basis vectors there are $$\left\lbrace \, \begin{aligned} \hat{u}_k &= R(\hat{u}_{k-1}, \theta_k, \hat{v}_{k-1}) \\ \hat{v}_k &= R(\hat{v}_{k-1}, \varphi_k, \hat{u}_k) \\ \vec{p}_k &= \vec{p}_{k-1} + L_k \hat{v}_k \\ \end{aligned} \right . \tag{both}\label{BtVboth}$$

When we use floating-point numbers, rounding errors will creep in. We can basically eliminate those by normalizing the $\hat{v}$ vector to unit length after $\eqref{BtVboth}$, $$\hat{v} = \frac{\hat{v}_k}{\left\lVert\hat{v}_k\right\rVert}$$ then making sure $\hat{u}$ is perpendicular to it, $$\vec{u} = \hat{u}_k - \hat{v} ( \hat{v} \cdot \hat{u}_k )$$ and normalizing it too to unit length, $$\hat{u} = \frac{\vec{u}}{\left\lVert\vec{u}\right\rVert}$$

---

Here is an example implementation in Python 3:

# SPDX-License-Identifier: CC0-1.0
# -*- encoding: utf-8 -*-
from math import sin, cos, sqrt

def segment(u, v, p, L, rotation, elbow):
    v = normalize(v)
    u = rotate(u, rotation, v)
    v = normalize(rotate(v, elbow, u))
    d = u[0]*v[0] + u[1]*v[1] + u[2]*v[2]
    u = normalize((u[0] - d*v[0], u[1] - d*v[1], u[2] - d*v[2]))
    p = (p[0] + L*v[0], p[1] + L*v[1], p[2] + L*v[2])
    return u, v, p

def normalize(vec):
    """Return vector 'vec' normalized to unit length"""
    n = sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2])
    return (vec[0]/n, vec[1]/n, vec[2]/n)

def rotate(vec, angle, axis):
    """Return vector 'vec' rotated by 'angle' radians around unit axis vector 'axis'."""
    ax = axis[0]
    ay = axis[1]
    az = axis[2]

    vx = vec[0]
    vy = vec[1]
    vz = vec[2]

    s = sin(angle)
    c = cos(angle)
    g = (ax*vx + ay*vy + az*vz)*(1 - c)

    rx = vx*c + (ay*vz - az*vy)*s + ax*g
    ry = vy*c + (az*vx - ax*vz)*s + ay*g
    rz = vz*c + (ax*vy - ay*vx)*s + az*g

    return (rx, ry, rz)

if __name__ == '__main__':
    from sys import stderr, argv, exit
    from math import pi

    if len(argv) < 2 or argv[1] == '-h' or argv[1] == '--help':
        if len(argv) > 0 and len(argv[0]) > 0:
            this = argv[0]
        else:
            this = "(this)"

        stderr.write("\n")
        stderr.write("Usage: %s [ -h | --help ]\n" % this)
        stderr.write("       %s LEN/ROT/TUR [ LEN/ROT/TUR ... ]\n" % this)
        stderr.write("Where:\n")
        stderr.write("       LEN   is the length of the segment\n")
        stderr.write("       ROT   is the rotation (degrees) around the axis\n")
        stderr.write("       TUR   is the elbow angle (degrees)\n")
        stderr.write("This will print the location and basis vector at the end of each segment.\n")
        stderr.write("The initial direction is towards positive Z axis, with elbows facing\n")
        stderr.write("the X axis: u=(0,1,0), v=(0,0,1).\n")
        stderr.write("\n")
        exit(0)

    u = (0, 1, 0)
    v = (0, 0, 1)
    p = (0, 0, 0)

    print("X = %.6f, Y = %.6f, Z = %.6f, U = (%.6f, %.6f, %.6f), V = (%.6f, %.6f, %.6f)" % (*p, *u, *v))

    for arg in argv[1:]:
        part = arg.split("/")
        if len(part) != 3:
            stderr.write("%s: Invalid parameter.\n" % arg)
            exit(1)

        slen = float(part[0])
        srot = float(part[1]) * pi / 180.0
        stur = float(part[2]) * pi / 180.0

        u, v, p = segment(u, v, p, slen, srot, stur)

        print("LEN = %.6f, ROT = %.2f, TUR = %.2f" % (slen, srot * 180.0 / pi, stur * 180.0 / pi))
        print("X = %.6f, Y = %.6f, Z = %.6f, U = (%.6f, %.6f, %.6f), V = (%.6f, %.6f, %.6f)" % (*p, *u, *v))

Save it as say example.py, and run python3 example.py to see the usage.

To see what happens if the first segment is rotated by 45° and 1 unit long, and the second segment is an elbow with 30° deflection (elbow angle 180°-30° = 150°) and two units long, run

python3 example.py 1/45/0 2/0/30

The last line of the output tells you that the tip ends up at $x = 0.707107$, $y = 0.707107$, and $z = 2.732051$, and that the orientation there is such that $\hat{u} = (-0.707107, 0.707107, 0.0)$ and $\hat{v} = (0.353553, 0.353553, 0.866025)$.