RobotControl.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Section IV. DYNAMICS AND CONTROL\n",
    "    \n",
    "# Chapter 16. Control of Articulated Robots"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Articulated robot control is a well-understood field, and industrial\n",
    "robots can attain impressive levels of precision even along complex\n",
    "trajectories. However, for optimal performance it is important for the\n",
    "controller designer and the trajectory designer (or trajectory planner)\n",
    "to cooperate or at least be cognizant about the limitations of each\n",
    "others' components.\n",
    "\n",
    "As an example, consider an industrial robot performing a semi-repetitive\n",
    "task like welding, where improving productivity means reducing the\n",
    "duration of every cycle. The robot cannot instantaneously accelerate and\n",
    "decelerate due to inertia, so if the desired trajectory slows down\n",
    "faster than the robot's motors can brake, then the robot will overshoot\n",
    "the desired target. Also, if the robot is operating in tight quarters,\n",
    "the amount of clearance needed between the robot and obstacles is\n",
    "typically proportional to its speed. The applications engineer must\n",
    "either closely coordinate with the controls engineer to design the\n",
    "trajectory according to the controller's specifications --- or else face\n",
    "a tedious process of trial-and-error.\n",
    "\n",
    "In many respects this chapter is quite vague on the details about *how\n",
    "to actually implement* a beautifully-performing articulated robot\n",
    "controller. This is by necessity; almost every robot that you purchase\n",
    "or build is going to have a different set of control capabilities and\n",
    "interfaces. An inexpensive hobby robot will not, by and large, have\n",
    "pinpoint accuracy and convenient trajectory design tools. A hydraulic\n",
    "industrial robot will not have a light touch. In most situations, as a\n",
    "robot engineer you often must (if I may twist a quote by Donald\n",
    "Rumsfeld) go to war with the robot you have, not the robot you wish you\n",
    "have. And one of the first battles you will face is to \"wrap\" the\n",
    "out-of-the-box control functionality of the robot into a useful,\n",
    "familiar interface that will help the application designer work with\n",
    "higher-level primitive operations.\n",
    "\n",
    "Who controls what? The motor controller\n",
    "---------------------------------------\n",
    "\n",
    "*************\n",
    "![fig:MotorControllerDiagram](figures/control/motor_controller_diagram.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:MotorControllerDiagram\">Figure 1</a>\n",
    "    System diagram of a motor controller.\n",
    "    </div>\n",
    "    \n",
    "*************\n",
    "\n",
    "Rarely would anyone find a robot whose computer applies voltages or\n",
    "currents directly to the motors. Rather, between the CPU and the motors\n",
    "sits a device called the *motor controller* (or more simply, the\n",
    "controller) that takes digital inputs from the CPU and outputs analog\n",
    "voltages or currents to the motors. It may also have the responsibility\n",
    "of converting readings from the motor encoders back into position\n",
    "readings for the computer.\n",
    "\n",
    "The controller's electronics or microprocessor perform some simple\n",
    "transformations of the digital input to derive the voltage or current\n",
    "signal. In the simplest case, it may just act as a digital-to-analog\n",
    "converter, in which case it is called a *motor driver*. However, most\n",
    "controllers perform other operations as well, such as calibration, PID\n",
    "control, filtering, velocity limiting, temperature regulation, and/or\n",
    "joint trajectory interpolation. These are important functions to prevent\n",
    "damage to the motor, such as burnout, and can also be more convenient\n",
    "for the end user. The end user in this case would be an application\n",
    "designer or a control engineer, who is interested in higher-level\n",
    "concerns than how the motor is driven. However, this also adds to\n",
    "complexity, since the user must understand the controller's functions\n",
    "and limitations!\n",
    "\n",
    "In an ideal world, the designers of motor controllers should also\n",
    "understand and implement the end user's desired functionality of the\n",
    "motor. Institutions with dedicated mechatronics labs might be able to do\n",
    "this, but more likely the mechatronic specialists are separate from the\n",
    "control engineers. As a result the burden likely falls on the user to\n",
    "understand (and compensate for!) the choices made by the motor\n",
    "controller designer. It can be frustrating to plan for, say, controlling\n",
    "a robot with velocity inputs\\... and then to learn that the controller\n",
    "only supports Go-To commands.\n",
    "\n",
    "Therefore, the first step in deciding how to control a robot is to\n",
    "characterize the inputs and performance characteristics of the\n",
    "controller. Some of these characteristics include maximum executable\n",
    "velocity or acceleration, PID gains, frequency response, tracking error,\n",
    "nominal and peak load, etc. More sophisticated (and expensive)\n",
    "controllers may also provide fault detection and recovery, motion\n",
    "queues, and gravity compensation. Another degree of sophistication is\n",
    "whether the motor controller operates a single joint (requiring multiple\n",
    "controllers per robot) or multiple joints. The latter are known as\n",
    "multi-axis controllers, and can perform sophisticated operations\n",
    "including Cartesian control and synchronized Go-To movements onboard.\n",
    "This offloads work that would otherwise be performed on the CPU onto the\n",
    "controller.\n",
    "\n",
    "As a general rule of thumb, cheap motor controllers require the\n",
    "application developer to put in more effort to provide high-level\n",
    "functionality, like trajectory execution or Cartesian movement. But more\n",
    "expensive controllers require the application developer to spend more\n",
    "time reverse-engineering the controller's behavior. For example, how\n",
    "long will it take to go from configuration A to configuration B? The\n",
    "rest of this chapter will describe how high-level functionality might be\n",
    "provided --- whether it resides on the CPU or the motor controller.\n",
    "\n",
    "Trajectory following\n",
    "--------------------\n",
    "\n",
    "Articulated robots are an example of a second order dynamic system, and\n",
    "here we consider only fully-actuated articulated robots in which each\n",
    "configuration variable has a corresponding control variable. In other\n",
    "words, $n = 2m$, since the state variable $x=(q,\\dot{q})$ is the phase\n",
    "of the system, and the control $u$ is assumed to affect the rate of\n",
    "change of $\\dot{q}$.\n",
    "\n",
    "The basic idea of trajectory following is to define a desired joint\n",
    "space trajectory $q_D(t)$ and to have the robot's actual trajectory\n",
    "$q(t)$ follow it. Assuming the robot can precisely measure its\n",
    "configuration and velocity, the general method for doing so is simply to\n",
    "send the desired configuration $q_D(t)$ and velocity and $\\dot{q}_D(t)$\n",
    "to a lower level controller, such as a PID controller, while updating\n",
    "the setpoint at a sufficiently high rate. However, to obtain accurate\n",
    "tracking, the designer of the trajectory must be cognizant of the\n",
    "control performance of the robot as a second order system, since at the\n",
    "lowest level the motor controller must eventually convert setpoint\n",
    "commands into joint torques.\n",
    "\n",
    "First, discontinuities in $q_D(t)$ should be forbidden since the robot\n",
    "will accelerate quickly, and possibly overshoot the target. Similarly,\n",
    "discontinuities in $\\dot{q}_D(t)$ should be avoided if possible, because\n",
    "exact tracking would require instantaneous acceleration. Naively\n",
    "requesting sudden changes of velocity runs the risk of overheating\n",
    "motors and overshooting setpoints.\n",
    "\n",
    "Secondly, joint stops and self-collision should be avoided with some\n",
    "margin of error, since the controller will not execute the trajectory\n",
    "exactly, especially at high speed. The magnitude of the margins should\n",
    "be chosen proportionally to the magnitude of control errors, which\n",
    "depend on the stiffness of the underlying controller.\n",
    "\n",
    "There are several functions that a motor controller may provide to\n",
    "enforce trajectory smoothness and safety. Or, the end user might need to\n",
    "(or to prefer to) implement these his/herself. These functions will be\n",
    "discussed below.\n",
    "\n",
    "### Motion queues (motion generation)\n",
    "\n",
    "To improve the convenience for a robot application designer, and to\n",
    "avoid some of the risks of allowing execution of non-smooth\n",
    "trajectories, most motor controllers provide a Go-To command, in which\n",
    "the desired endpoint is reached gradually over time. In essence, the\n",
    "motor controller is generating the robot's reference trajectory $q_D(t)$\n",
    "as it goes.\n",
    "\n",
    "*************\n",
    "\n",
    "![fig:MotionQueue](figures/control/motion_queue.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:MotionQueue\">Figure 2</a>.\n",
    "Illustrating a 3-axis motion queue. The robot's future trajectory is\n",
    "created as requested by the application, and over time the controller\n",
    "incrementally feeds setpoints to a lower-level PID\n",
    "controller. \n",
    "</div>\n",
    "\n",
    "*************\n",
    "\n",
    "This is a simple form of a *motion queue* mode of operation in which the\n",
    "application requests milestones or trajectory segments to be queued up\n",
    "on the controller (i.e., multiple Go-To commands, or a Move-Path\n",
    "command). Over time, the controller will execute the requested\n",
    "trajectory sequence without further input from the application\n",
    "([Fig. 2](#fig:MotionQueue)). The convenience of this mode of operation\n",
    "is quite apparent when programming long sequences of instructions with\n",
    "discrete event logic.\n",
    "\n",
    "To implement such motion queues, the controller maintains a future\n",
    "trajectory in its memory. As in typical trajectory following, time is\n",
    "advanced on each control loop, and the desired setpoint is updated.\n",
    "Furthermore, the future trajectory can be modified at any time by an\n",
    "external request signal (e.g., a function call). For example, adding a\n",
    "Go-To request will append an interpolating curve from the end of the\n",
    "queued trajectory to the requested target position (a *waypoint*). If\n",
    "the current queue is empty, then the curve will originate from the\n",
    "current robot position. This type of asynchronous request is convenient\n",
    "because the application developer does not need to develop a fixed-rate\n",
    "control loop. Moreover, motion queues might be directed to cycle, making\n",
    "it convenient to define repetitive movements used in factory settings.\n",
    "\n",
    "Motion queue methods differ greatly in their interpolating curves, speed\n",
    "and velocity control, their ability to handle joint-space vs. Cartesian\n",
    "space targets, and whether an existing queue may be interrupted. Some of\n",
    "these differences are discussed below.\n",
    "\n",
    "### Interpolation profiles\n",
    "\n",
    "Given the robot at rest and a target configuration, an *interpolation\n",
    "profile* is a continuous trajectory that satisfies certain operational\n",
    "constraints on the robot's joint dynamics. A sequence of Go-To commands\n",
    "can be implemented by concatenating several interpolation profiles, in\n",
    "which the robot stops/starts at subsequent waypoint configurations.\n",
    "\n",
    "#### Piecewise linear\n",
    "\n",
    "The most basic interpolation profile is a piecewise linear curve.\n",
    "Additional milestones are added in a manner that limits the maximum\n",
    "absolute joint velocity\n",
    "([Fig. 3](#fig:PiecewiseLinearInterpolation)). However, the trajectory\n",
    "exhibits instantaneous changes of velocity. This type of profile is most\n",
    "often observed in inexpensive hobbyist controllers.\n",
    "\n",
    "*************\n",
    "![fig:PiecewiseLinearInterpolation](figures/control/linear_interpolation.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:PiecewiseLinearInterpolation\">Figure 3</a>.\n",
    "Illustrating the use of piecewise linear interpolation profiles. At\n",
    "time $t_0$, a commanded waypoint $\\theta_g$ is sent, and an\n",
    "interpolating curve is constructed (left). Then, at time $t_1$, a new\n",
    "waypoint command $\\theta_{g2}$ is sent that interrupts execution of the\n",
    "existing curve\n",
    "(right).\n",
    "    </div>\n",
    "\n",
    "*************\n",
    "\n",
    "#### Trapezoidal velocity profile\n",
    "\n",
    "A refinement of this approach is a bounded-acceleration,\n",
    "bounded-velocity curve. To arrive at a target configuration in a\n",
    "minimum-time fashion under these bounds, while also starting and\n",
    "stopping at 0 velocity, a *trapezoidal velocity profile* may be used\n",
    "([Fig. 4](#fig:TrapezoidalVelocityProfile)). Assume the waypoint joint\n",
    "value $\\theta_{g}$ is greater than the start value $\\theta_{s}$ (in the\n",
    "reverse case, the profile may simply be flipped). The velocity profile\n",
    "ramps up from 0 at maximum acceleration $a_{max}$ until the maximum\n",
    "velocity $v_{max}$ is reached, then proceeds at constant velocity for\n",
    "some time, and then decelerates to 0 velocity at minimum slope. If the\n",
    "target is close to the original configuration, the maximum velocity may\n",
    "not actually be reached, in which case the velocity profile takes on a\n",
    "triangular shape.\n",
    "\n",
    "*************\n",
    "\n",
    "![fig:TrapezoidalVelocityProfile](figures/control/trapezoidal_velocity_profile.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:TrapezoidalVelocityProfile\">Figure 4</a>.\n",
    "    Trapezoidal velocity profile interpolation. The joint trajectory\n",
    "switches between parabolic, linear, and parabolic\n",
    "segments.\n",
    "    </div>\n",
    "\n",
    "*************\n",
    "\n",
    "The duration $t_f$ of the 2 or 3 segments of the profile are determined\n",
    "analytically from $\\theta_{g}-\\theta_{s}$ and the acceleration /\n",
    "velocity bounds. Specifically, first consider the 2-segment case. The\n",
    "curve $\\theta(t)$ begins with an upwards-facing parabola for duration\n",
    "$t_f/2$ and ends with a downwards facing parabola with an equal\n",
    "duration. Hence, the displacement at the midpoint\n",
    "$(\\theta_{g}-\\theta_{s})/2$ must be equal to\n",
    "$\\frac{1}{2} a_{max} (t_f/2)^2$. In other words,\n",
    "$t_f = 2 \\sqrt{(\\theta_{g}-\\theta_{s})/a_{max}}$. If the midpoint\n",
    "velocity $a_{max} t_f /2 > v_{max}$ , then the velocity bound is reached\n",
    "and we must use the 3 segment case. In this case, the first segment\n",
    "accelerates at acceleration $a_{max}$ for time $t_1 = v_{max}/a_{max}$,\n",
    "and the last segment decelerates for the same amount of time\n",
    "$t_3 = t_1$. In this time they each cover distance\n",
    "$\\frac{1}{2} a_{max} (v_{max}/a_{max})^2 = v_{max}^2 /(2 a_{max})$, and\n",
    "hence the second segment must cover distance\n",
    "$\\theta_g - \\theta_s - v_{max}^2 /a_{max}$. It does so at speed\n",
    "$v_{max}$, so it takes time\n",
    "$t_2 = (\\theta_g - \\theta_s)/v_{max} - v_{max}/a_{max}$. Ultimately, the\n",
    "overall time is\n",
    "$t_f = 2t_1 + t_2 = (\\theta_g - \\theta_s)/v_{max} + v_{max}/a_{max}$.\n",
    "\n",
    "##### Bounded-jerk interpolation\n",
    "\n",
    "A second potential extension, which provides even smoother curves, is a\n",
    "bounded-jerk trajectory. *Jerk* is another name for the third time\n",
    "derivative of a function, and bounded-jerk paths will see the\n",
    "acceleration profile having a positive trapezoid and negative\n",
    "trapezoidal components. Determination of the number of segments, segment\n",
    "timing, and overall duration is similar to the above construction,\n",
    "although higher-order polynomials need to be solved.\n",
    "\n",
    "##### General methods and cubic curves\n",
    "\n",
    "Other potential interpolation profiles include cubic curves (Hermite\n",
    "interpolation) and sinusoidal curves (cosine smoothing). In each case,\n",
    "coefficients of the profile and the interpolation duration are\n",
    "determined by matching the boundary conditions and any operational\n",
    "constraints on the velocity or acceleration of the curve.\n",
    "\n",
    "For example, a cubic curve $\\theta(u)$ interpolating between $\\theta_s$\n",
    "and $\\theta_g$ over the range $u\\in [0,1]$ has the form\n",
    "$$\\theta(u) = \\theta_s + (\\theta_g-\\theta_s)(a + bu + cu^2 + du^3)$$\n",
    "Using the boundary conditions $\\theta(0) = \\theta_s$,\n",
    "$\\theta(1) = \\theta_g$, $\\theta'(0) = 0$, and $\\theta'(1)=0$, we have\n",
    "four linear constraints that must be satisfied: $a=0$, $a+b+c+d=1$,\n",
    "$b = 0$, and $b + 2c + 3d = 0$. The solution is $a=0$, $b=0$, $c=3$,\n",
    "$d=-2$, and hence\n",
    "$$\\theta(u) = \\theta_s + (\\theta_g-\\theta_s)(3u^2 - 2u^3)$$\n",
    "is a smooth interpolator known as the Hermite curve.\n",
    "\n",
    "The remaining question is how to set the time duration of the curve\n",
    "$t_f$ to maintain velocity and acceleration limits. We define\n",
    "$u = t/t_f$, giving the expression for velocity\n",
    "$$\\dot{\\theta}(t) = \\frac{d}{dt} \\theta(u(t)) = \\theta^\\prime(u(t)) u^\\prime(t) =  (\\theta_g-\\theta_s)(6u - 6u^2) /t_f = 6(\\theta_g-\\theta_s)(t/t_f^2 - t^2/t_f^3)$$\n",
    "This is maximized at $t=t_f/2$, which gives the maximum velocity along\n",
    "the curve as a function of $t_f$\n",
    "$$v^\\star(t_f) = \\frac{3}{2}(\\theta_g-\\theta_s)/t_f.$$\n",
    "The acceleration is\n",
    "$$\\ddot{\\theta}(t) = 6(\\theta_g-\\theta_s)(1/t_f^2 - 2t/t_f^3)$$\n",
    "which has maximum absolute value at the endpoints. Hence, the maximum\n",
    "acceleration along the curve is\n",
    "$$a^\\star(t_f) = 6(\\theta_g-\\theta_s)/t_f^2.$$\n",
    "It is then a simple\n",
    "matter to determine whether the curve is acceleration limited or\n",
    "velocity limited, and then to set $t_f$ appropriately.\n",
    "\n",
    "### Speed and velocity control\n",
    "\n",
    "Speed control is a fairly common addition for Go-To commands, and simply\n",
    "modulates the rate of progression of \"time\" along the trajectory.\n",
    "However, to avoid sudden changes in velocity it is important not to\n",
    "modify path speed instantaneously while the robot is moving.\n",
    "Alternatively, speed control may be provided by modulating the maximum\n",
    "velocity / acceleration used to derive the interpolation profile.\n",
    "\n",
    "Velocity control is a desirable capability for maximizing robot speed\n",
    "because the target joint velocities can be chosen so that movement to\n",
    "the next milestone does not require stopping. Trapezoidal velocity\n",
    "profiles and cubic curves are the most convenient forms for allowing\n",
    "waypoint velocities. To do so, the specified velocities should be\n",
    "incorporated as boundary conditions when solving for the form of the\n",
    "interpolation profile.\n",
    "\n",
    "### Cartesian commands\n",
    "\n",
    "More sophisticated multi-axis motor controllers, such as those found in\n",
    "industrial robots, may mix joint-space and Cartesian commands\n",
    "([Fig. 5](#fig:MixedCartesianQueuing)). \n",
    "\n",
    "*************\n",
    "\n",
    "![fig:MixedCartesianQueuing](figures/control/cartesian_queuing.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:MixedCartesianQueuing\">Figure 5</a>.\n",
    "A motor controller may mix Cartesian and joint-space motion queuing.\n",
    "Left: the end effector trace of a 2R manipulator with a mixture of joint\n",
    "space (solid) and Cartesian space (dotted) waypoint commands. Right: the\n",
    "same trace, but in joint\n",
    "space.\n",
    "</div>\n",
    "\n",
    "*************\n",
    "\n",
    "Cartesian motion queue\n",
    "functionality is useful to guide tools along workspace paths, while\n",
    "joint-space waypoints are superior for moving quickly between\n",
    "configurations. A motion queue can construct Cartesian interpolation\n",
    "profiles, with two caveats.\n",
    "\n",
    "First, IK solutions along the interpolation profiles should be verified\n",
    "to exist when given a Cartesian Go-To command. If no solution exists,\n",
    "the command should return an error code.\n",
    "\n",
    "Secondly, during execution, an IK solver must be used to find the joint\n",
    "space configuration and velocity for the interpolated Cartesian target\n",
    "position. These values will be then used as setpoints to the low level\n",
    "controller. Alternatively, operational space control methods can be used\n",
    "for direct Cartesian control without the use of an IK solver using only\n",
    "Jacobian information. Operational space control will be briefly covered\n",
    "below. In either case, error codes may be invoked during execution when\n",
    "no IK solution exists, the robot hits joint limits, or passes near\n",
    "singularities.\n",
    "\n",
    "### Interruption\n",
    "\n",
    "Interruption is a helpful feature for motion queues when the robot may\n",
    "need to react to unexpected conditions. As an example, a stop and reset\n",
    "may be triggered when the robot mistakenly drops an object during a\n",
    "transfer task.\n",
    "\n",
    "To stop movement an unspecified time, it is a simple matter to clear the\n",
    "remainder of the queue once a robot reaches a stopped waypoint. But if\n",
    "the robot should be stopped during the middle of movement, some care\n",
    "must be taken. One option could be to execute a maximal braking\n",
    "trajectory, which slows down each joint as quickly as possible. The\n",
    "issue with this approach is that the robot will likely drift from the\n",
    "desired trajectory because each joint is slowed at a different rate,\n",
    "which may cause collisions. An alternative is just to slow the robot\n",
    "down along its current trajectory by reducing the path speed to zero.\n",
    "\n",
    "Another mode of interruption is to instantaneously start moving to a new\n",
    "waypoint, clearing the remainder of the queue. This was illustrated for\n",
    "the simple piecewise linear interpolation profile in\n",
    "[Fig. 3](#fig:PiecewiseLinearInterpolation). However, to avoid sudden\n",
    "changes of velocity, the interpolation profile used in the queue should\n",
    "support nonzero initial velocities.\n",
    "\n",
    "The most sophisticated mode of interruption would to allow unlimited\n",
    "editing of the motion queue at any point in time. Providing such\n",
    "functionality usually requires a fair amount of bookkeeping on the\n",
    "controller, and hence is rarely implemented for most industrial robots.\n",
    "\n",
    "### State machines\n",
    "\n",
    "For many tasks, the robot controller should perform some notion of\n",
    "*switching* between discrete modes of operation. For example, to\n",
    "implement Cartesian commands and braking trajectories, the controller\n",
    "must activate different functions, accept different input, and report\n",
    "different status codes depending on which mode it is in. This type of\n",
    "functionality can be thought of as a *state machine*, and is usually\n",
    "implemented as one as well.\n",
    "\n",
    "A state machine prescribes several possible discrete *states*\n",
    "$S_0,S_1,\\ldots,S_N$ under which the controller can operate. (These are\n",
    "distinct from the robot's state in the context of dynamics, and order\n",
    "does not matter.) The controller can be in one state\n",
    "$S(t) \\in \\{ S_0,S_1,\\ldots,S_N \\}$ at any given time, starting with the\n",
    "initial state $S_0$. The controller still operates on a fixed control\n",
    "loop, and performs whatever processing is designated by state $S(t)$.\n",
    "This includes input-output processing, but also potentially reacting to\n",
    "signals which indicate that the state machine should change state. These\n",
    "signals $T_{ij}$ are known as *transitions*. If at time $t$,\n",
    "$S(t) = S_i$, and transition $T_{ij}$ becomes active, then we change the\n",
    "state $S(t+\\Delta t)\\gets S_j$ for the next time step. (If multiple\n",
    "transition signals are raised at the same time, the controller\n",
    "implements some tie-breaking policy.)\n",
    "\n",
    "*************\n",
    "![fig:StateMachine](figures/control/state_machine.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:StateMachine\">Figure 6</a>.\n",
    "Left: A state machine is a directed graph that describes the discrete\n",
    "modes that a controller can switch between. Right: An example state\n",
    "machine for a controller that detects hardware faults and can switch\n",
    "between joint space commands, Cartesian commands, and braking\n",
    "commands.\n",
    "</div>\n",
    "\n",
    "*************\n",
    "\n",
    "State machines are a very useful tool for visualizing the functionality\n",
    "of controllers from low-level motor controllers to high-level\n",
    "supervisors, and are typically drawn as directed graphs\n",
    "([Fig. 6](#fig:StateMachine)). We shall revisit these diagrams later\n",
    "when we discuss [system integration](SystemIntegration.ipynb).\n",
    "\n",
    "Feedforward torque control\n",
    "--------------------------\n",
    "\n",
    "Although PID control is sometimes adequate for strong motors or\n",
    "lightweight robots, it is not sufficient to achieve the level of\n",
    "trajectory tracking accuracy demanded by industrial applications. There\n",
    "are three reasons for this.\n",
    "\n",
    "1.  Gravity biases the torques needed to keep the robot upright. A PID\n",
    "    controller compensates for gravity via the integral term, but the\n",
    "    amount of compensation required is highly configuration dependent.\n",
    "    In fact, if a joint axis flips orientation, the torque needed to\n",
    "    resist a given load will need to entirely reverse sign.\n",
    "\n",
    "2.  The effective inertia of the subchain at each joint is highly\n",
    "    dependent on its configuration. For example, if an arm is\n",
    "    outstretched, the shoulder joint will need to apply stiffer torques\n",
    "    to track a trajectory than if it is tucked.\n",
    "\n",
    "3.  At high speed, the Coriolis term becomes significant, causing\n",
    "    biasing torques that must be compensated.\n",
    "\n",
    "As a result, many industrial robots use model-based techniques to\n",
    "calculate *feedforward torques* that reflect some of the biases coming\n",
    "from the robot's dynamics. The simplest form of feedforward torque\n",
    "computation is *gravity compensation*. In this mode of operation, the\n",
    "feedforward torques only include the generalized gravity term of the\n",
    "robot's dynamics equation: $$\\tau_{ff} = G(q)$$ where $q$ is the current\n",
    "sensed configuration. This torque is combined with the feedback torque\n",
    "$\\tau_{fb}$ resulting from the PID controller to obtain the overall\n",
    "torque: $$\\tau = \\tau_{fb} + \\tau_{ff}.$$ In this form of operation,\n",
    "when the robot is stationary, the force from gravity will be exactly\n",
    "compensated for by $\\tau_{ff}$, and hence $\\tau_{fb}=0$ since the PID\n",
    "controller will not observe any error. This does, however, assume that\n",
    "the masses, centers of masses, and link lengths of all robot links are\n",
    "measured precisely for the calculation of $G$. In practice, these\n",
    "estimates will have small errors and hence the feedback term is required\n",
    "for regulating the errors between the true $G(q)$ and the estimate\n",
    "$\\tilde{G}(q)$. Moreover, once the robot begins moving, the PID\n",
    "controller will need to take on the responsibility of compensating for\n",
    "inertial and Coriolis effects. See\n",
    "[Fig. 7](#fig:PIDGravityCompensation) for an illustration.\n",
    "\n",
    "*************\n",
    "![fig:PIDGravityCompensation](figures/control/robot_PID_control.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:PIDGravityCompensation\">Figure 7</a>.\n",
    "    A comparison of piecewise linear and piecewise cubic trajectories\n",
    "executed by PID control or gravity compensation. Left: the two\n",
    "trajectories. Right: trajectory tracking errors comparing PID control\n",
    "(PID) against gravity compensation (GC) for the two types of\n",
    "trajectories. Cubic trajectories demonstrate lower transient error\n",
    "during movement, while GC converges more quickly toward zero\n",
    "steady-state error.\n",
    "</div>\n",
    "\n",
    "*************\n",
    "\n",
    "A more sophisticated controller is *computed torque control*, which also\n",
    "accounts for inertial and Coriolis effects during movement. Supposing we\n",
    "have a desired acceleration $\\ddot{q}_D$. If we use inverse dynamics to\n",
    "calculate the necessary torque needed to guide the robot upon this\n",
    "acceleration:\n",
    "$$\\tau_{ID}(\\ddot{q}) = B(q) \\ddot{q} + C(q,\\dot q) + G(q)\n",
    "\\label{eq:InverseDynamics}$$\n",
    "then feeding this torque to the robot will\n",
    "generate the desired acceleration, assuming no disturbances and modeling\n",
    "errors. There are generally two methods for compensating for error. The\n",
    "first method uses $\\tau_{ID}$ as a feedforward control, combined with a\n",
    "PID feedback:\n",
    "$$\\tau = \\tau_{ID}(\\ddot{q}_D) + \\tau_{fb}.$$\n",
    "\n",
    "The second\n",
    "method, *computed torque control*, instead computes a desired\n",
    "acceleration as the sum of $\\ddot{q}_D$ with a feedback acceleration:\n",
    "$$\\ddot{q}_{fb} = - K_P(q-q_D) - K_I I(t) - K_D(\\dot{q}-\\dot{q}_D).$$\n",
    "\n",
    "The resulting controller uses the torques computed for the feedforward +\n",
    "feedback accelerations:\n",
    "$$\\tau = \\tau_{ID}(\\ddot{q}_D + \\ddot{q}_{fb}).$$\n",
    "\n",
    "This approach is\n",
    "appealing because with acceleration being the control variable, the PID\n",
    "constants can be chosen easily so the joint space dynamics are\n",
    "convergent. The main drawback of computed torque control methods is the\n",
    "additional computational expense of computing inverse dynamics, which\n",
    "must be recomputed on a per-timestep basis. It also faces the general\n",
    "difficulty of calibrating accurate inertial parameters of a robot.\n",
    "\n",
    "\n",
    "Operational space control\n",
    "-------------------------\n",
    "\n",
    "It is often desired to control the end effector of an articulated robot\n",
    "more accurately in Cartesian space than in joint space. *Operational\n",
    "space control* is a method for doing so without the use of inverse\n",
    "kinematics. Another application of operational space control is to apply\n",
    "or respond to forces in Cartesian space, which is a key step in *force\n",
    "control* and *impedance control*. Properly applying these concepts\n",
    "requires a deep understanding of how to transform forces and robot\n",
    "dynamics between joint space and the operational space.\n",
    "\n",
    "Here, operational space is synonymous with the definition of task space\n",
    "that we have used throughout this course. Specifically, a task space\n",
    "$x = \\phi(q)$ is defined with the Jacobian\n",
    "$J(q) = \\frac{\\partial \\phi}{\\partial q}(q)$. As usual, the task space\n",
    "is typically a combination of Cartesian position and/or orientation of\n",
    "an end effector. (Here we have chosen the notation $\\phi$ rather than\n",
    "the more typical $x = f(q)$ so as to not conflict with the $f(x,u)$\n",
    "notation used for a dynamic system.)\n",
    "\n",
    "### Mathematical derivation\n",
    "\n",
    "To get a sense for this approach, let us suppose that the robot's\n",
    "dynamics are first-order and velocity controlled. In other words, we\n",
    "wish to determine a control $u$ such that $\\dot{q} = u$. Let $x_D(t)$ be\n",
    "a task space trajectory that we wish to control, and let $x = \\phi(q)$\n",
    "be the current task space coordinate. We can define a commanded task\n",
    "velocity $\\dot{x}_{cmd} = -K_P (x - x_D(t)) + \\dot{x}_D(t)$ via a\n",
    "feedback position gain $K_P$ and the feedforward velocity\n",
    "$\\dot{x}_D(t)$. To convert from the task velocity to the control, we can\n",
    "use the Jacobian relationship $\\dot{x} = J(q)\\dot{q}$. Using the\n",
    "pseudoinverse, we have $u = \\dot{q} = J(q)^+ \\dot{x}_{cmd}$. If $x_D$ is\n",
    "stationary, then given any starting configuration, this will drive the\n",
    "task coordinate $x$ to converge toward $x_D$.\n",
    "\n",
    "Note that we could also use the Jacobian transpose rule\n",
    "$\\dot{q} = J(q)^T \\dot{x}_{cmd}$ and derive a stable controller in task\n",
    "space. To see this, observe that the dynamics of the task space error\n",
    "$e = x - x_D$ are given by $$\\dot{e} = J \\dot{q} = - J J^T K_P e.$$\n",
    "Hypothesizing the Lyapunov function $V(e) = \\frac{1}{2} e^T K_P e$, we\n",
    "can calculate\n",
    "$$\\dot{V}(e) = \\frac{\\partial V}{\\partial e}(e)^T \\dot{e}.$$\n",
    "Under the observation that $\\frac{\\partial V}{\\partial e}(e) = K_P e$, observe\n",
    "that $\\dot{V}(e) = -e^T K_P J J^T K_P e$. If $J$ and $K_P$ are\n",
    "nonsingular, then $K_P J J^T K_P$ is a symmetric positive definite\n",
    "matrix, and hence this is a negative definite function, i.e., negative\n",
    "for all $e\\neq 0$. Hence, this system is Lyapunov stable away from\n",
    "singularities.\n",
    "\n",
    "Typically, operational space control is applied with second-order\n",
    "dynamics and a torque-controlled robot. This makes it possible to treat\n",
    "the operational coordinate as a sort of virtual rigid body under\n",
    "spring-damper dynamics. Suppose we wish to control the dynamics of the\n",
    "task space coordinate according to the trajectory $x_D(t)$. Then the\n",
    "desired acceleration is\n",
    "$$\\ddot{x}_{cmd} = -K_P(x-x_D(t)) - K_D(\\dot{x} - \\dot{x}_D) + \\ddot{x}_D(t). \\label{eq:SpringDamperAcceleration}$$\n",
    "Computing the time derivative of\n",
    "the task space mapping twice, we obtain:\n",
    "$$\\ddot{x} = \\frac{d}{d t}(J(q)\\dot{q}) = J(q)\\ddot{q} + \\dot{J}(q)\\dot{q}$$\n",
    "Let us examine how the task space acceleration is affected by task space\n",
    "forces, $f_{cmd}$, such that the resulting torque is\n",
    "$\\tau = J^T f_{cmd}$. For convenience of the expression, we drop the\n",
    "dependence of several terms on $q$:\n",
    "$$\\begin{split}\n",
    "\\ddot{x} &= J(B^{-1}(\\tau - C(q,\\dot{q}) - G) + \\dot{J}\\dot{q} \\\\\n",
    "&= JB^{-1} J^T f_{ext} - JB^{-1} C(q,\\dot{q}) - JB^{-1} G  + \\dot{J}\\dot{q} \n",
    "\\end{split}$$ \n",
    "Recall from the [discussion on effective inertia](RobotDynamics.ipynb#Effective-inertia)\n",
    "that the matrix $\\tilde{B} = (JB^{-1}J^T)^{-1}$ is the effective inertia of the task\n",
    "space coordinate. Multiplying both sides by $\\tilde{B}$ and rearranging,\n",
    "we get\n",
    "$$\\tilde{B}\\ddot{x} + \\tilde{B}(JB^{-1} C(q,\\dot{q}) - \\dot{J}\\dot{q}) + \\tilde{B}JB^{-1}G =  f_{ext}.$$\n",
    "\n",
    "If we let\n",
    "$\\tilde{C}(q,\\dot{q}) = \\tilde{B}(JB^{-1} C(q,\\dot{q}) - \\dot{J}\\dot{q})$\n",
    "and $\\tilde{G} = \\tilde{B}JB^{-1}G$, we have the task space dynamics\n",
    "expressed in a familiar form:\n",
    "$$\\tilde{B}\\ddot{x} + \\tilde{C}(q,\\dot{q}) + \\tilde{G} = f_{ext}.$$\n",
    "Thus, with a little linear algebra, we can solve for $f_{ext}$ such that\n",
    "$\\ddot{x} = \\ddot{x}_{cmd}$. Applying $\\tau = J^T f_{cmd}$ we can\n",
    "finally derive the output joint torques $\\tau$.\n",
    "\n",
    "The result of operational space control (with $K_P$, $K_D$ diagonal) is\n",
    "that the system response to step changes in a Cartesian task space is\n",
    "linear in Cartesian space. If same step change was achieved by a change\n",
    "in configuration setpoint to a PID controller, the system response would\n",
    "be nonlinear.\n",
    "\n",
    "However, there are a number of difficulties in implementing operational\n",
    "space control. First, it is important not to let the Jacobian become\n",
    "singular, because the effective inertia will become infinite. Even if it\n",
    "is near singular, the inversion involved in computing the effective\n",
    "inertia matrix will cause unacceptably large torques to be computed.\n",
    "Second, if the configuration has met one or more joint limits, some\n",
    "directions in the task space will become uncontrollable. In light of\n",
    "these issues achieving stable operational control often requires a\n",
    "significant amount of tuning.\n",
    "\n",
    "### Force and impedance control\n",
    "\n",
    "Using operational space control and a force sensor, it is possible to\n",
    "obtain a number of interesting behaviors, including Cartesian force\n",
    "control where the end effector can apply a specified force, or \"zero\n",
    "gravity mode\" where the end effector behaves like a free-floating rigid\n",
    "body. These two behaviors are known, respectively, as *force control*\n",
    "and *impedance control*.\n",
    "\n",
    "In both cases, force sensing can be accomplished with a sensing element\n",
    "(a force/torque sensor) on the end effector, or indirectly via joint\n",
    "torque sensors. Joint torque sensing is generally less accurate at\n",
    "estimating the forces applied to the end effector due to loss of\n",
    "observability at singularities, but does not require external equipment.\n",
    "\n",
    "It should be noted that force sensors are prone to noise, requires\n",
    "frequent recalibration, and runs a risk of irreparable damage to force\n",
    "sensors by overloading. Moreover, to obtain stable control loops a great\n",
    "deal of tuning, calibration, and optimization of sensor signal filters,\n",
    "the force sensor's inertial parameters, the robot's inertial parameters,\n",
    "and time delays. Hence, implementing force and impedance control\n",
    "successfully in practice requires substantial amounts of effort from\n",
    "even experienced control engineers.\n",
    "\n",
    "Force control is the process of regulating a force applied to the\n",
    "environment in closed-loop fashion. To illustrate how this might works,\n",
    "consider a 1-D task space $x = \\phi(q)$ denoting the end effector\n",
    "position along some axis. Assume that the robot is contacting the\n",
    "environment at the initial configuration $q_0$, and that we would like\n",
    "to apply a desired force $f_D$ at the end effector. The basic idea of\n",
    "force control is to sense the external force $f_{ext}$ and if it is too\n",
    "high, to reduce $x$, and if is too low, to increase $x$. Although this\n",
    "is conceptually simple, the implementation is not!\n",
    "\n",
    "The main issues to address are that 1) the force sensor does not\n",
    "directly report $f_{ext}$ but rather a combination of external force,\n",
    "gravity, inertial forces due to end-effector acceleration, and noise, 2)\n",
    "it is not clear how much to increase or reduce $x$ to obtain a desired\n",
    "change in force, and 3) those changes in $x$ need to be converted into\n",
    "joint-level commands. Operational control handles issue 3, but not 1 or 2.\n",
    "To extract the external forces from the sensed forces, the operation\n",
    "of a force sensor should be modeled in detail. Accelerations will be\n",
    "observed as force spikes in the sensed force signal, and changes in the\n",
    "end effector orientation will be observed as a bias in the force signal:\n",
    "$$f_{sensed} = f_{ext} + f_{gravity} + f_{inertial}. \\label{eq:ForceSensor}$$\n",
    "The best way to estimate the external force is\n",
    "to model the force of gravity and inertia on the sensor by estimating\n",
    "the sensor's orientation and acceleration and subtracting their sum from\n",
    "the sensed force. Orientation and acceleration must be estimated from\n",
    "the robot's encoders or an inertial measurement unit.\n",
    "\n",
    "Next, we need some way to link the deviation of force $f_D - f_{ext}$\n",
    "and the control signal given to the operational space controller\n",
    "(usually $\\ddot{x}_{cmd}$ or if joint torque control is available, the\n",
    "commanded end effector force $f_{cmd}$). This can be done using a PID\n",
    "loop where $f_D - f_{ext}$ is the error signal and the operational space\n",
    "control signal is the output. In the case that $\\ddot{x}_{cmd}$ is the\n",
    "output, the gain should be on the order of the inverse of the stiffness\n",
    "of the environment - end effector system, while if it is the end\n",
    "effector force, the gains should be on the order of 1.\n",
    "\n",
    "Impedance control implements a behavior where the robot's end effector\n",
    "as though it were a mass-spring-damper (MSD) system subjected to an\n",
    "external force (usually without gravity). The equations of an MSD are as\n",
    "follows:\n",
    "$$\\ddot{x} = M^{-1} (f_{ext} - K (x_D-x) - D \\dot{x}_D) \\label{eq:MassSpringDamper}$$\n",
    "with $M$ the mass matrix, $K$ the\n",
    "stiffness matrix, and $D$ the damping matrix. For example, a\n",
    "zero-gravity mode sets $K$ and $D$ to 0 and the robot's end effector\n",
    "behaves like a free-floating mass in response to applied forces. To\n",
    "simulate MSD behavior on a robot's end effector, equation\n",
    "($\\ref{eq:MassSpringDamper}$) can be used in conjunction with\n",
    "($\\ref{eq:ForceSensor}$) to estimate $f_{ext}$, and the resulting\n",
    "$\\ddot{x}$ should be fed into an operational space controller as\n",
    "$\\ddot{x}_{cmd}$.\n",
    "\n",
    "An implementer of such systems should be aware that it is difficult to\n",
    "simulate a low-mass MSD with a high-mass robot, since the accelerations\n",
    "encountered by the mass in response to an applied force may exceed the\n",
    "limits of the robot's torques. The range of accelerations that a robot\n",
    "can perform is also known as its *control bandwidth*. Lighter robots,\n",
    "with stronger motors, and with low mass near the end effector tend\n",
    "to have larger control bandwidth than heavy ones."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Summary\n",
    "-------\n",
    "\n",
    "Key takeaways:\n",
    "\n",
    "-   To maximize performance of an articulated robot, its trajectories\n",
    "    should be designed with motor controller characteristics in mind\n",
    "    (and vice versa).\n",
    "\n",
    "-   A robot's commands from its CPU are processed through a motor\n",
    "    controller board, which may implement wildly differing amounts of\n",
    "    functionality for safety, accuracy, and convenience purposes.\n",
    "\n",
    "-   Motion queues are a common function of motor controllers that create\n",
    "    trajectory interpolation profiles from asynchronous commands.\n",
    "\n",
    "-   Gravity compensation and computed torque control are two forms of\n",
    "    feedforward torque control that improve the accuracy of trajectory\n",
    "    tracking.\n",
    "\n",
    "-   Operational space control redefines the PID control in some\n",
    "    Cartesian task space rather than joint space. This allows relating\n",
    "    torques to forces, as needed for applications like force and\n",
    "    impedance control."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Exercises\n",
    "---------\n",
    "\n",
    "1.  Suppose that your robot only has an ability to perform velocity\n",
    "    control, in which the CPU specifies the joints' desired velocities.\n",
    "    Commands are read in at a fixed rate (say, 100Hz). Describe how you\n",
    "    would implement a PID controller on the CPU that tracks a trajectory\n",
    "    $q_D(t)$. Keep in mind that velocities are not executed perfectly,\n",
    "    and that the robot should obtain 0 steady-state error if the\n",
    "    trajectory is stationary.\n",
    "\n",
    "2.  Propose a data representation and algorithms for a motion queue\n",
    "    controller such that updating the current setpoint takes $O(1)$\n",
    "    time, and such that an application on the robot's CPU can append to\n",
    "    it and clear it in $O(1)$ time. Your implementation should discard\n",
    "    portions of the queue that are in the past, either with a linked\n",
    "    list or circular buffer. (For simplicity, use piecewise linear\n",
    "    interpolation.)\n",
    "\n",
    "3.  Motor controllers have a limited amount of memory and must determine\n",
    "    desired setpoints at a very fast rate. Why must the motion queue\n",
    "    size, in practice, be limited? If interrupts are allowed, what else\n",
    "    limits the motion queue size?\n",
    "\n",
    "4.  Empirically study the performance of high-gain PID control on a\n",
    "    simulated 6DOF robot. First, tune the constants so that it is\n",
    "    stable. How does it react to gravity when it is first turned on? How\n",
    "    does it react to disturbances? How well does it track when commanded\n",
    "    to a very different configuration?\n",
    "\n",
    "5.  Consider a straight line movement in Cartesian space. In the absence\n",
    "    of disturbances, how would the output of an operational space\n",
    "    controller compare to that of a Cartesian motion queue executed with\n",
    "    a joint space PID controller? How would they differ in rejecting\n",
    "    disturbances?\n",
    "\n",
    "6.  **Implementation.** Implement a Cartesian velocity motion queue\n",
    "    controller for a planar 2R manipulator. As commands, the controller\n",
    "    should take velocities $(v_x,v_y)$ and movement durations $t$, and\n",
    "    simply use piecewise linear interpolation. Develop some sensible\n",
    "    scheme to handle singularities and joint limits by stopping motion.\n",
    "    Test its performance in simulation with a variety of command\n",
    "    sequences.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.3"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": true,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": true,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}