Anthropomorphic RRR Manipulator

This example extends the basic kinematic workflow to a spatial serial manipulator.

We will use a three-degree-of-freedom anthropomorphic RRR robot and compute its forward kinematics, inspect intermediate frames, obtain the end-effector pose, compute the geometric Jacobian, evaluate the model numerically, and visualize the resulting configuration.

The objective is to show that the same workflow used for a planar robot can be applied directly to a spatial manipulator.

Problem

Consider a serial manipulator with three revolute joints.

The first joint rotates about the base vertical axis, while the second and third joints form an articulated arm.

Let

\[ q_1,\; q_2,\; q_3 \]

be the joint variables, and let

\[ d_1,\; l_2,\; l_3 \]

represent the geometric dimensions of the robot.

Using the classical Denavit-Hartenberg convention, the robot is described by:

Link	(a_i)	(\alpha_i)	(d_i)	(\theta_i)	Joint
1	(0)	(\pi/2)	(d_1)	(q_1)	revolute
2	(l_2)	(0)	(0)	(q_2)	revolute
3	(l_3)	(0)	(0)	(q_3)	revolute

Unlike the planar 2R manipulator, this mechanism moves in three-dimensional space.

We want to obtain its symbolic kinematic model and then inspect one numerical configuration.

Robot model

First import the required objects:

from sympy import pi

from moro import Robot
from moro.abc import q1, q2, q3, d1, l2, l3

Create the robot:

robot = Robot(
    (0, pi / 2, d1, q1, "r"),
    (l2, 0, 0, q2, "r"),
    (l3, 0, 0, q3, "r"),
)

Inspect the resulting object:

robot

which returns:

Robot RRR

The number of degrees of freedom is:

robot.dof

which returns:

The joint variables are:

robot.qs

and the DH table can be inspected with:

robot.dh_table

At this point, the robot model is fully symbolic.

Forward kinematics

The homogeneous transformation of the end-effector with respect to the base frame is available through:

T = robot.T
T

For this robot, the transformation depends on all three joint variables and contains both position and orientation information.

The end-effector position has the structure

\[\begin{split} p = \begin{bmatrix} \left(l_2\cos q_2+l_3\cos(q_2+q_3)\right)\cos q_1 \\ \left(l_2\cos q_2+l_3\cos(q_2+q_3)\right)\sin q_1 \\ d_1+l_2\sin q_2+l_3\sin(q_2+q_3) \end{bmatrix}. \end{split}\]

This expression already shows the characteristic structure of an anthropomorphic manipulator:

(q_1) rotates the arm around the base axis;
(q_2) and (q_3) determine the radial and vertical position of the arm.

The complete transformation also contains the orientation of the final frame.

Intermediate frames

For spatial manipulators, inspecting intermediate frames is often useful.

The transformations of the three link frames with respect to the base are:

T10 = robot.T_i0(1)
T20 = robot.T_i0(2)
T30 = robot.T_i0(3)

The last transformation is equivalent to the complete end-effector transformation:

T30 == robot.T

which evaluates to:

True

The rotation matrices can also be accessed directly:

R10 = robot.R_i0(1)
R20 = robot.R_i0(2)
R30 = robot.R_i0(3)

Similarly, the origin of each frame in base coordinates can be obtained with:

r1 = robot.r_o(1)
r2 = robot.r_o(2)
r3 = robot.r_o(3)

These intermediate quantities are particularly useful when studying the geometry of spatial robots or when building Jacobians.

End-effector pose

A homogeneous transformation contains both orientation and position.

The orientation of the end-effector is:

R = robot.T[:3, :3]
R

while its Cartesian position is:

p = robot.T[:3, 3]
p

These quantities can also be obtained from the corresponding frame transformation:

R = robot.R_i0(3)
p = robot.r_o(3)

The position depends on the three joint coordinates:

\[\begin{split} p(q_1,q_2,q_3)= \begin{bmatrix} x \\ y \\ z \end{bmatrix}. \end{split}\]

For this particular robot,

\[ x= \left(l_2\cos q_2+l_3\cos(q_2+q_3)\right)\cos q_1, \]

\[ y= \left(l_2\cos q_2+l_3\cos(q_2+q_3)\right)\sin q_1, \]

and

\[ z= d_1+l_2\sin q_2+l_3\sin(q_2+q_3). \]

Unlike the planar 2R example, the end-effector position can now vary in all three Cartesian coordinates.

Jacobian matrix

The geometric Jacobian is available through:

J = robot.J
J

For a three-degree-of-freedom manipulator, the result is a (6\times3) matrix.

The first three rows represent the linear velocity Jacobian:

Jv = J[:3, :]
Jv

and the final three rows represent the angular velocity Jacobian:

Jw = J[3:, :]
Jw

The complete relationship is:

\[\begin{split} \begin{bmatrix} v \\ \omega \end{bmatrix} = J(q)\dot q. \end{split}\]

In this case, the Jacobian describes motion in three-dimensional space, so both its linear and angular parts contain information that is not present in a purely planar mechanism.

The Jacobian columns correspond to the contribution of each revolute joint to the motion of the end-effector.

Numerical evaluation

Now assign numerical values to the robot geometry:

\[ d_1=1.0, \qquad l_2=1.2, \qquad l_3=0.9. \]

Consider the joint configuration:

\[ q_1=\frac{\pi}{6}, \qquad q_2=-\frac{\pi}{9}, \qquad q_3=\frac{7\pi}{36}. \]

These values correspond to approximately:

\[ q_1=30^\circ, \qquad q_2=-20^\circ, \qquad q_3=35^\circ. \]

Create the substitution dictionary:

values = {
    d1: 1.0,
    l2: 1.2,
    l3: 0.9,
    q1: pi / 6,
    q2: -pi / 9,
    q3: 7 * pi / 36,
}

The end-effector transformation can now be evaluated numerically:

T_num = robot.T.subs(values).evalf()
T_num

The Cartesian position is:

p_num = p.subs(values).evalf()
p_num

The orientation matrix is:

R_num = R.subs(values).evalf()
R_num

and the Jacobian at the same configuration is:

J_num = robot.J.subs(values).evalf()
J_num

The same symbolic model can be reused for any other configuration by modifying only the substitution dictionary.

Inspecting intermediate frames numerically

The intermediate transformations can also be evaluated using the same numerical values:

T10_num = robot.T_i0(1).subs(values).evalf()
T20_num = robot.T_i0(2).subs(values).evalf()
T30_num = robot.T_i0(3).subs(values).evalf()

Similarly, the frame origins are:

r1_num = robot.r_o(1).subs(values).evalf()
r2_num = robot.r_o(2).subs(values).evalf()
r3_num = robot.r_o(3).subs(values).evalf()

This makes it possible to inspect the complete spatial geometry of the manipulator, not only the end-effector pose.

Visualization

A spatial robot is especially useful for illustrating the interactive visualization tools.

Create a visualizer:

from moro.visualization import RobotVisualizer

viz = RobotVisualizer(robot)

Matplotlib

The configuration can be rendered with Matplotlib:

fig, ax = viz.plot(
    values,
    backend="matplotlib",
)

A different initial camera orientation can be selected with:

fig, ax = viz.plot(
    values,
    backend="matplotlib",
    view_init=(30, 45),
)

When running in a standard Python script:

import matplotlib.pyplot as plt

plt.show()

The resulting 3D plot shows the spatial arrangement of the links and coordinate frames.

Three.js

For an interactive representation:

viz.plot(
    values,
    backend="threejs",
)

The Three.js viewer is especially useful for this robot because the configuration is genuinely three-dimensional.

The robot can be inspected using:

free orbit navigation;
Front view;
Top view;
Isometric view;
orthographic projection;
perspective projection.

The coordinate frames help reveal how the orientation changes from one link to the next.

Reusing the model

The symbolic robot can be evaluated at another configuration without creating a new Robot instance:

another_configuration = {
    d1: 1.0,
    l2: 1.2,
    l3: 0.9,
    q1: pi / 3,
    q2: pi / 6,
    q3: -pi / 4,
}

The new end-effector pose is obtained with:

robot.T.subs(another_configuration).evalf()

and the same configuration can be visualized immediately:

viz.plot(
    another_configuration,
    backend="threejs",
)

This reuse of the symbolic model is one of the main advantages of the workflow.

Discussion

The anthropomorphic RRR manipulator extends the workflow introduced with the planar 2R robot to three-dimensional kinematics.

The same Robot object provides access to:

robot.T
robot.T_i0(i)
robot.R_i0(i)
robot.r_o(i)
robot.J

These quantities describe different aspects of the same symbolic kinematic model.

The complete workflow remains:

define the DH model
        ↓
compute symbolic frame transformations
        ↓
inspect end-effector pose
        ↓
compute the geometric Jacobian
        ↓
substitute numerical values
        ↓
visualize the configuration

The main difference with the planar example is geometric rather than procedural. Once the robot is defined, the same API can be applied to both planar and spatial serial manipulators.

This RRR robot is also a useful model for more advanced analyses.

For example, the end-effector position

\[ p(q_1,q_2,q_3) \]

can be used as the basis for numerical inverse kinematics, and the geometric Jacobian can be used by iterative inverse-kinematics algorithms.

The next examples build directly on this model to solve Cartesian position targets numerically.