Torque Control of a Puma 560 robot

The Programmable Universal Machine for Assembly (PUMA) 560 is a six degree-of-freedom robotic manipulator, where all the six joints are revolute. We will denote the $i^\mathrm{th}$ joint-angle with $q_i$, and the collective vector of joint angles with $\mathbf{q} = [q_0, q_1, q_2, q_3, q_4, q_5]$.

A robot is described by a set of links (generally rigid members) that are interconnected by joints. Thus, a robot can be seen as a chain of links, where the relative orientation of successive links is determined by the joint between the links. The joint's degree-of-freedom (for example, the angle for a revolute joint) geometrically determines the relative position and orientation (collectively, the pose) of a successive link. The kinematics of the robot -- the relation of the end-effector, typically the last link in the chain, to the robot configuration, i.e., the joints' degrees-of-freedom -- is, thus, a series of spatial transformations from the typically-fixed base of the robot to the end-effector through successive links. The spatial transformations achieved by each link may be described by a sequence of elementary transforms or through a specific convention, such as the Denavit–Hartenberg convention.

We will use the roboticstoolbox-python library to create and simulate the Puma 560 robot, and design a controller in Collimator. A combination of the Python ecosystem, offering a variety of open-source libraries, and its easy integration into Collimator for the design of controllers and block-diagram based programming offers a powerful combination. Collimator allows you to seamlessly integrate and work with any open-source library or even your own proprietary libraries within the platform.

Robot Kinematics

Forward kinematics

Forward kinematics is the problem of determining the end-effector pose given all the joint angles. The end-effector pose is typically represented as a homogenous transformation matrix, which contains both the orientation and the position of the end-effector. If the elementary transformation achieved by the $i^\mathrm{th}$ joint through it's degree-of-freedom $q_i$ is represented as $\mathbf{E_i}$, then for a robot with $n$ joints, the end-effector post $\mathbf{T}_\mathrm{e}$ can be written as:

$$ \mathbf{T}_\mathrm{e}  = \prod_{i=0}^{n} \mathbf{E_i}(q_i) = \mathcal{K}(\mathbf{q}),$$

where $\mathcal{K}$ represents the combined effect of all the link transformations, and can be seen as the forward-kinematics operator.

For the Puma 560 instance created within the `roboticstoolbox-python` toolbox, we can compute the forward kinematics through the `fkine` method as follows:

 from math import pi
q = [0., pi/2, -pi/2, 0. , 0., 0.]          # the six joint angles q0, q1, ..., q5
Te = puma.fkine(q)                          # Compute forward kinematics for a given q
print('Position of the end-effector')
print (Te.t)    # the position (x,y,z) of the end-effector
print('Orientation of the end-effector (expressed as a rotation matrix)')
print(Te.R)     # the orientation of the end-effector represented as a rotation matrix

You can visualize the robot as a noodle-plot available within the toolbox as follows:

 puma.plot(q)

Inverse kinematics

Inverse kinematics refers to the problem of finding the the joint-angles of a robot given the end-effector pose, i.e.

$$ q = \mathcal{I_K} (\mathbf{T}_\mathrm{e}),$$

where $\mathcal{I_K}$ can be seen as the inverse-kinematics mapping (if a solution exists). Note that this mapping is generally not unique. If analytical solution is not possible, then preferred configurations can be included as constraints and/or a numerical solution can be obtained where starting from a given configuration $\mathbf{q_0}$, the robot's configuration can be iteratively improved so that the error in the above equation converges to zero.

Within the `roboticstoolbox-python` toolbox, we can compute the inverse kinematics through various available methods. We will use the `ikine_LM` method, which uses Levenberg-Marquadt optimization.

 sol = puma.ikine_LM(Te)                 # Inverse kinematics with Levenberg-Marquadt optimization 
if sol.success:                         # Check that optimizer succeeded in finding a solution
    print("Inverse Kinematics solution:")
    print(sol.q)
else:
    print("Inverse Kinematics solution failed.")

Velocities and accelerations

The joint velocities and accelerations in the joint space are are denoted as

$$\mathbf{\dot{q}} = [{\dot{q}}_0, {\dot{q}}_1, \cdots, {\dot{q}}_n], \text{ and,}$$  

$$\mathbf{\ddot{q}} = [{\ddot{q}}_0, {\ddot{q}}_1, \cdots, {\ddot{q}}_n],$$

where $\dot{q} = \dfrac{dq}{dt}$ and $\ddot{q} = \dfrac{d^2q}{dt^2} = \dfrac{d \dot{q} } {dt}$.

Consequently: $\dot{q}= \displaystyle \int \ddot{q} \; dt$ and $q= \displaystyle \int \dot{q} \; dt$

The relation of the joint-space velocities to the task-space (end-effector) velocities can be computed with the jacobian ($\mathbf{J}$) of the forward kinematics operator. Additionally, the relation of the joint-space accelerations to the task-space accelerations can be computed with the jacobian and hessian  ($\mathbf{H}$) of the forward kinematics operator. While these are not strictly needed for this tutorial, the toolbox provides methods to compute the jacobian and hessian.

Robot Dynamics

So far, we have only looked at the kinematic relations relating the joint-space configuration (i.e. the joint angles) to the end-effector configuration (i.e. the task-space configuration). Robot dynamics is concerned with the relationship between the forces/torques on the joints to the robot's motion (i.e. velocities and accelerations, either in the joint-space or in the task space). This is achieved by specifying the dynamics parameters for each joint and link combination within the robot.

For each joint and link combination (i.e. a joint followed by a single rigid link), the following parameters describing the dynamics of the link must be prescribed

For the Puma 560 instance, these parameters can be viewed through the `puma.dynamics_list()` command as follows:

 puma.dynamics_list() 

Note: these parameters can be changed (or for a new robot assigned as desired). For example, the mass for link 1 can be changed by `puma.links[1].m = 17.5`.

Generating trajectories in the Toolbox

The toolbox provides us with methods to generate smooth trajectories between any two joint position vectors through the `jtraj` method. Given starting and end joint positions, this method allows for generation of smooth trajectories, i.e. the velocities and accelerations at the start and the end are zero and vary smoothly. For example:

qstart = [0., pi/2, -pi/2, 0. , 0., 0.]    # Starting joint position
qend = [pi/4, 0, 0, pi/4, pi/4, pi/4]      # Final joint position
tstart = 0.0                               # Start time
tend = 5.0                                 # End time
nsteps = 100                               # Number of steps we need the output of the trajectory at

tvec = np.linspace(tstart, tend, nsteps)   # Generate a vector of time instances for the trajectory output
tj = rtb.jtraj(qstart, qend, nsteps)       # tj.t contains the time vector, tj.q contains positions, tj,qd contains velocities, and tj.qd contains the accelerations

fig, axs = plt.subplots(nrows=puma.n, ncols=1, figsize=[7.5,8])        # (puma.n = 6, the dof of robot)
for index, ax in enumerate(axs):
#     ax.set_title(f'Joint {index}')
    ax.plot(tvec, tj.q[:,index], '-r', label=f'$q_{index}$')     # plot joint position
    ax.plot(tvec, tj.qd[:,index], '-g', label=f'$\dot{{q}}_{index}$')     # plot joint velocity
    ax.plot(tvec, tj.qdd[:,index], '-b', label=f'$\ddot{{q}}_{index}$')     # plot joint acceleration
    ax.legend(loc='best')
fig.tight_layout()
plt.show()

Inverse dynamics (mapping motion to torques)

The general equation of motion (essentially the rigid-body equations) for robots can be expressed in the form below

$$ \boldsymbol\tau = \mathbf{M(q)} \; \mathbf{\ddot{q}} + \mathbf{C (q, \dot{q})} \; \mathbf{\dot{q}} + \mathbf{g}(\mathbf{q}) + \boldsymbol\tau_{ext} ,$$

where:

- $\boldsymbol\tau = [\tau_0, \tau_1, \cdots, \tau_n]$ is the vector of  generalised torques at the joints ($\tau_i$ represents the generalised torque at the $i^\mathrm{th}$ joint),

- $\mathbf{M(q)}$ is the intertia matrix,

- $\mathbf{C (q, \dot{q})}$ is the Coriolis and Centripetal coupling matrix,

- $\mathbf{g}(\mathbf{q})$ is the load due to gravity, and

- $\boldsymbol\tau_{ext}$ are torques due to external forces, including friction. The torque due to an external wrench $\mathcal{F}_{ext}$ can be written as $\mathbf{J}(\mathbf{q})^T\mathcal{F}_{ext}$, where $\mathbf{J}$ is an appropriate Jacobian, and the torques due to friction (typically viscous friction and Coulomb friction) can be written as $\mathbf{F}(\mathbf{\dot{q}})$, depending on the joint velocities $\mathbf{\dot{q}}$.

The above equation gives the generalized joint torques given the joint positions $\mathbf{q}$, joint velocities $\mathbf{\dot{q}}$, and joint accelerations $\mathbf{\ddot{q}}$. This is the inverse dynamics problem in robotics. The general way of solving the inverse dynamics problem is the Recursive Newton-Euler algorithm (RNE), and available within the toolbox through the method `rne`.

The following code snippet demonstrates computation of inverse dynamics within the toolbox.

# Inverse dynamics
q = np.array([0., pi/2, -pi/2, 0. , 0., 0.]) # Joint positions
qd = 0.1*np.ones((puma.n,))                  # Joint velocities 
qdd = 0.1*np.ones((puma.n,))                 # Joint accelerations

tau = puma.rne(q, qd, qdd)         # Invoke RNE to solve inverse dynamics and compute generalised torques
print(tau)

Forward dynamics (mapping torques to motion)

Forward dynamics is the problem of determining the motion (specifically, the joint accelerations) given the applied generalized torques and the current robot configuration (joint positions and velocities). The forward dynamics can be, at least conceptually, solved by rearranging the above equation as follows:

$$  \mathbf{\ddot{q}} = \mathbf{M^{-1}(q)} \;\left[ \boldsymbol\tau -  \mathbf{C (q, \dot{q})} \; \mathbf{\dot{q}} - \mathbf{g}(\mathbf{q})  - \boldsymbol\tau_{ext} \right],$$

where $\mathbf{M^{-1}(q)}$ is the inverse of the inertia matrix $\mathbf{M(q)}$.

For solving the forward dynamics problem, typically the RNE method is used in the background. For example $\boldsymbol\tau' = \mathbf{M(q)} \; \mathbf{\ddot{q}} + \mathbf{C (q, \dot{q})} \; \mathbf{\dot{q}} + \mathbf{g}(\mathbf{q})$ with $\mathbf{\ddot{q}}=0$ can be first solved through RNE to compute $\boldsymbol\tau'$. Subsequently, $\mathbf{\ddot{q}} = \mathbf{M^{-1}(q)} \;\left[ \boldsymbol\tau - \boldsymbol\tau'\right]$ can be computed through standard linear algebra.

Similarly, RNE can also be used to compute the inertial, coriolois, and gravitational load matrices by setting appropriate combinations of the elements of $\mathbf{q}$, $\mathbf{\dot{q}}$, and $\mathbf{\ddot{q}}$ to zero and/or one.

The following code snippet demonstrates forward dynamics solution in the toolbox.

qdd = puma.accel(q,qd, tau)       # Compute acceleration through forward dynamics
print(qdd)

Some control algorithms may require computation of the Inertia, Coriolis, Gravitational loads, Friction torques, and torques due to payloads (external load on the end effector). This can be achieved through the toolbox as follows:

M = puma.inertia(q)             # Compute Inertia matrix
C = puma.coriolis(q,qd)         # Compute Coriolis matrix
g = puma.gravload(q)            # Compute Gravitational load vector
tau_f = puma.friction(qd)       # Torque due to Friction (to be added to the motor output torque; use negative sign if using in equations described above)
W = [1., 1. , 1., 0., 0., 0.]   # Payload on the end-effector:  W = [Fx Fy Fz Mx My Mz]
tau_p = puma.pay(W)             # Generalised torque due to a payload wrench at the end-effector current configuration, i.e. q = puma.qon

print("Mass matrix is:")
print(M)
print("Coriolis matrix is:")
print(C)
print("Garvitational load is:")
print(g)
print("Friction torque")
print(tau_f)
print(f"Torque due to an external payload wrench {W} applied to the end-efector")
print(tau_p)

Robot Control

Equipped with the above forward and inverse dynamics of the robot, we can now think about controlling the robot. There are multiple ways to think about robot control, but generally we are concerned with making the robot's end-effector reach desired points or track desired trajectories over time (designed after consideration of a planned task: a related but separate problem of path planning). The desired points or the trajectories can be specified in the joint-space, i.e. specification of the joint positions $\mathbf{q}$, the joint velocities $\mathbf{\dot{q}}$, and the joint accelerations $\mathbf{\ddot{q}}$ as functions of time. This is called joint-space control. Alternatively, the  dersired points or the trajetories can be specified in terms of the positions, velocities, and accelerations of the end-effector post ($\mathbf{T}_e$). This is called task-space control. Recall that the joint-space variables and task-space variables are related through the jacobian ($\mathbf{J}$) and the hessian ($\mathbf{H}$), which can be easily computed. Conceptually, the control mechanism is identical for both cases. In either the joint-space or task-space control framework, the control for industrial robots can be either:

1. Position-resolved: the joint-positions are directly specified as commands (with the assumption that the internal robot controllers are able to follow these position commands).

2. Velocity-resolved: the joint-velocities are directly specified as commands (with the assumption that the internal robot controllers are able to follow these velocity commands).

3. Torque control: Here, the torque applied to the joints is specified as commands. This is the most general and fundamental case, as the currents applied to the motor are the commands (control variables), that directly influence the joint torques.

In what follows, we will look at one form of computed torque control, which is one of the forms of torque control.

Joint-space Computed Torque Control (CTC)

For computed torque control, our goal is to specify the joint-torques directly such that the joints follow a desired trajectory (combination of positions, velocities, and accelerations) over time. The specification of a trajectory in the joint-space directly influences the trajectory in the task-space. Denoting the desired trajectory positions, velocities, and accelerations as $\mathbf{q}_s$, $\mathbf{\dot{q}}_s$, and $\mathbf{\ddot{q}}_s$, respectively, the error vector between the desired and current position is:

$$\mathbf{e} = \mathbf{q}_s - \mathbf{q}.$$

Correspondingly, the error derivative (alternatively, the error between the desired and current velocities) is:

$$\mathbf{\dot{e}} = \mathbf{\dot{q}}_s - \mathbf{\dot{q}}.$$

With proportional and derivate gains denoted as $\mathbf{K}_p$ and $\mathbf{K}_d$, respectively, the terms $\mathbf{K}_p \mathbf{e}$ and $\mathbf{K}_d \mathbf{\dot{e}}$ can be fed back to compute the torque needed for controlling the robot. The computed-torques $\boldsymbol\tau_c$ that should be applied to the joints are given by:

Note:

- The above CTC equation requires the knowledge of the Robot model: $\mathbf{M(q)}$, $\mathbf{C (q, \dot{q})}$, and $\mathbf{g(q)}$. The friction forces may be included in $\boldsymbol\tau_{ext}$, or even be ignored. Similarly other disturbances may also be ignored, leaving the feedback terms in the controller equation to still obtain good robot control.

- $\mathbf{K}_p$ and $\mathbf{K}_d$ are the position and velocity gain/damping matrices. Easy choice for these is a diagonal matrix, where the diagonal terms correspond to the gain values for each joint.

- An integral error term of the form $\displaystyle \int \mathbf{K}_i \mathbf{e}\; dt$ can be added to the first term of the RHS, where $\mathbf{K}_i$ is the integral gain matrix, but is often not necessary.  If this term is added the CTC equation becomes:

- Often simpler versions of the CTC law, where even the Coriolis terms $\mathbf{C (q, \dot{q})}$, along with external forces and friction forces are ommited in the control law can be used. These can perform quite well, even if the robot model is not quite accurate.

When the $\boldsymbol\tau_{ext}$ term is neglected, a CTC block diagram is shown below.

Building the Controller and Robot simulator in Collimator

Armed with the above background, we can now simulate the Puma 560 robot (i.e. given torques calculate its motion) and control it (i.e. apply the right torques for the robot to follow a desired trajectory)

Initialization

Let's create a new model and create the Robot instance and import some modules that we will later use. The script content is shown below

import numpy as np
import roboticstoolbox as rtb
from bisect import bisect

puma560 = rtb.models.DH.Puma560()
puma = puma560.nofriction()         # We will work with the version of the robot that excludes Coulomb friction but includes viscous friction
njoints = puma.n

Generate a desired trajectory

As a first step, we will create a sub-model which, given a set of joint positions and the time at which the robot needs to be attain these positions, generate the desired trajectories in the joint-space. Subsequently, we will use the CTC control method to control the robot so that it follows this desired trajectory. If you wish to specify the poses in the task-space as opposed to joint positions, you can use the `ikine_LM` method (described above) to first perform inverse kinematics and obtain the corresponding joint positions.

Our block-diagram for this sub-model is shown in the Figure below. It takes `time` and `tq` (lists of time and joint positions) as input and, through a Python script block, outputs $\mathbf{q}_s$, $\mathbf{\dot{q}}_s$, and $\mathbf{\ddot{q}}_s$.

The `trajectory_generator` python script block takes `time` and `tq` as an input and outputs 3 vectors (6x joint positions $\mathbf{q}_s$, 6x joint velocities $\mathbf{\dot{q}}_s$, and 6x joint accelerations $\mathbf{\ddot{q}}_s$), which are the outputs of the sub-model. The code inside the Python script block is as follows

if time==0: # Construct to compute some quantities only once (i.e at the first time step) 
    # Import needed libraries
    import numpy as np
    import roboticstoolbox as rtb
    from bisect import bisect

    # Initialise Robot instance
    puma560 = rtb.models.DH.Puma560()
    puma = puma560.nofriction()
    njoints = puma.n

    # Helper function to interpolate trajectories
    def get_interp_state(time, traj):
        '''
        Given a trajectory object (traj), returns q, qd, and qdd,
        which are interpolated from within the trajectory object
        '''
        import numpy as np
        q = np.zeros(njoints)
        qd = np.zeros(njoints)
        qdd = np.zeros(njoints)

        for joint in range(njoints):
            q[joint] = np.interp(time, traj.t, traj.q[:,joint])
            qd[joint] = np.interp(time, traj.t, traj.qd[:,joint])
            qdd[joint] = np.interp(time, traj.t, traj.qdd[:,joint])
        
        return (q,qd,qdd)

    seg_times = []          # Time segments
    seg_traj = []           # Trajectory segments

    nsteps = int(nsteps)
   
    for tq_start, tq_end in zip(tq_list, tq_list[1:]):
        tstart = tq_start[0]
        tend = tq_end[0]
        qstart = tq_start[1:]
        qend  = tq_end[1:]
        tvec = np.linspace(tstart, tend, nsteps)
        tj = rtb.jtraj(qstart, qend, nsteps)
        tj.t = tvec
        seg_traj.append(tj)
        seg_times.append(tend)

# Find which segment the time corresponds to
tj_index = bisect(seg_times, time)
if tj_index >= len(seg_times):
    print(f"Simulation time {time} is beyond the maximum trajectory definition time {seg_times[-1]}")
    print("Setting all state values to last specified value")
    q,qd,qdd = get_interp_state(time, seg_traj[-1])

else:
    q,qd,qdd = get_interp_state(time, seg_traj[tj_index])

The above code takes segments, pairs of successive time and joint positions as input, and then uses the toolbox trajectory generator method `jtraj` to create smooth segment trajectories. Finally, for a given `time`, the code finds the correct trajectory to use and interpolates the trajectory data to output $\mathbf{q}_s$, $\mathbf{\dot{q}}_s$, and $\mathbf{\ddot{q}}_s$ at that `time`.

Implement robot dynamics

To simulate the robot, we need a model for the Puma 560 dynamics which, given joint torques, can simulate its motion. This block is conceptually very simple: we use the `puma.accel(q, qd, joint_torque)` method to compute the acceleration, then use the integrator block once to get the velocities, and then once again to compute the position. The 'Puma 560 Dynamics' submodel is shown in the figure below, which takes the joint torque (six-dimensional vector for the six joint torques) as an input.

The `robot_acceleration' Python script contains the following short snippet

if time == 0:
    import roboticstoolbox as rtb
    puma560 = rtb.models.DH.Puma560()
    puma = puma560.nofriction()

acceleration = puma.nofriction().accel(q, qd, joint_torque)  # Get acceleration

Design the controller

With the robot dynamics model and the desired trajectory, all that remains is to design the controller so that the robot follows the desired trajectory. A conceptual diagram is shown below. Taking the clock as an input, the `generate_trajectory` sub-model outputs the desired trajectory. The controller then, taking this desired trajectory, along with the output of the robot dynamics model (the position and velocities) as feedback needs to generate the torques such that the robot follows the desired trajectory. The torque generated by the Controller is fed to the robot. We will use a PD CTC controller and assume identical $K_p$ and $K_d$ gains for all the six joints. These two gains are also the inputs to the controller.

With the CTC method described above, we can now build the controller subsystem. The subsystem is shown below, which takes the aforementioned inputs. The main control logic for CTC is inside the `Controller_logic` python script.

The content of the Python script is below and implements the CTC equations described previously. We will ignore the friction force components in the feedback and treat them as disturbances. The CTC equation becomes:

if time == 0:
    import numpy as np
    import roboticstoolbox as rtb
    p560 = rtb.models.DH.Puma560()
    puma = p560.nofriction()
    njoints = puma.n

# Calculate mass and Coriolis/centripetal matrices at current (measured) location
M = puma.inertia(q)               # Mass/inertia matrix
C = puma.coriolis(q, qd)          # Coriolis/centripetal matrix

# Compute gravitational load at current (measured) location
g = puma.gravload(q)              # Gravload

Kpmat = np.diag([Kp]*njoints).    # Construct diagonal Kp mattrix from scalar Kp
Kdmat = np.diag([Kd]*njoints)     # Construct diagonal Kd mattrix from scalar Kd

# Compute error
e = q_s - q                       # The error vector
ed = qd_s - qd                    # The error vector derivative

a = Kpmat @ e + Kdmat @ ed        # The feedback acceleration

computed_torque = M @ (qdd_s + a) + C @ qd + G

Analyze controller stability

The CTC controller is asymptotically stable for symmetric positive-definite gain matrices $\mathbf{K}_p$ and $\mathbf{K}_d$.

Now that our robot and controller system is ready, we can select blocks for visualization of the robot-controller combined system. In particular, we want to visualize the desired trajectory $\mathbf{q}_s$, the actual trajectory $\mathbf{q}$, the error $\mathbf{e} = \mathbf{q}_s - \mathbf{q}$, and the torques generated by our controller. The latter is important to ensure that the generated torques are not high (i.e. larger than what the joint motors can produce) and that they vary smoothly. The complete diagram with visualization enabled is shown below:

Run the simulation

Let's first run the model with low values of $K_p$ and $K_d$ gains. Let's set these block values to reflect $K_p=10$ and $K_d=1$.

The resulting model output is shown below:

Clearly, the desired trajectory (first plot) is very different from the actual robot trajectory (second plot). The third plot shows the error between the corresponding components of the first and second plots. The last plot shows the CTC generated torque.

From the third plot (showing error), we notice that the maximum joint angle error is approximately 2.5 radians . This is equivalent to an error of approximately 150 degrees. It may be sensible for us to get an error of less than 2 degrees, which is equivalent to approximately 0.035 radians. This is our target. Depending on the joint, the Puma 560 has maximum motor torques of 80–180 Nm. This is a constraint that we need to keep in mind: the motor torques should not exceed the maximum motor torque.

Let's increase the gains to $K_p=3000$ and $K_d=10$. The model output is shown below:

The maximum error is now approximately 0.023 radians, which is less than 2 degrees, and fulfils the error requirement during the entire trajectory. The maximum torques are approximately 40 Nm, which is also in the safe region.

Further things for you to try:

- Vary the $K_p$ and $K_d$ gain values to see how the robot performs.

- Try creating different trajectories (both fast and slow) and assess the controller performance.

- Omit the Coriolis feedback term for the controller and see what effect it has and whether the robot can still be controlled reliably with the CTC contoller.

- Include a $K_i$ gain and assess the performance.