Robotics 1

notes notes Icon, released 30. 11. 2022, updated 1. 3. 2023


This website contains my lecture notes from a lecture by Lorenzo Masia from the academic year 2022/2023 (University of Heidelberg). If you find something incorrect/unclear, or would like to contribute, feel free to submit a pull request (or let me know via email).


Thanks to Pascal Hansen and Kiryl Kiril for corrections.

Lecture overview

  1. Introduction [slides] [tutorial – basics]
  2. Robotics Components [slides] [tutorial – rotations, Grübler]
  3. Kinematics [slides] [tutorial – Grübler, DH]
  4. Homogeneous Transformation and DH [slides] [tutorial – DH]
  5. Inverse Kinematics [slides] [tutorial – DH, Inverse Kinematics]
  6. Inverse Kinematics (numerical methods) [slides] [tutorial – Geometric Jacobian]
  7. Differential Kinematics [slides]
  8. Inverse Differential Kinematics [slides] [tutorial – trajectory planning]
  9. Trajectory Planning in Joint Space [slides] [tutorial – inverse kinematics]
  10. Trajectory Planning in Cartesian Space [slides] [tutorial – Grübler, DH]
  11. Manipulability [slides] [tutorial – Jacobian, Lagrangian]

An excellent resource for more formal an in-depth study is Robot modeling and Control by Mark W. Spong, which the lecture frequently references. The first edition can be downloaded for free, while the second needs to be purchased (or downloaded too, if you know where to look).

Robotics Components

Definition (link/member): individual bodies making up a mechanism

Definition (joint): connection between multiple links

Definition (kinematic pair): two links in contact such that it limits their relative movement

Definition (kinematic chain): assembly of links connected by joints

Open and closed kinematic chains

Degrees of Freedom

This is pretty important and will most likely be on the final, so it is best to practice this. Here are some of the examples from the exercises with my solutions (sorry for my terrible drawing).

Definition (degree of freedom) of a mechanical system is the number of independent parameters that define its configuration. It can be calculated by the Grübler Formula:

DOF=λ(n1)i=1jci\text{DOF} = \lambda (n - 1) - \sum_{i = 1}^{j} c_i


For example, the following has 3(41)24=13 ({\color{red}{4}} - 1) - 2 \cdot {\color{blue}{4}} = 1 DOF: DOF

If you’re viewing this in dark theme, the links are colored light blue (red in the equation) and the joints are colored yellow (blue in the equation) – sorry!

Definition (coincident joints): when there are more than two kinematic pairs in the same joint

Coincident joints.


Definition (actuator): a mechanical device for moving or controlling something

DC motor illustration.


Incremental and absolute optical encoders.


Kinematic spaces.

Forward (direct) Kinematics

Definition (forward/direct kinematics): the process of finding the position/orientation of the end-effector (r1,,rm)(r_1, \ldots, r_m) given a set of joint parameters (q1,,qn)(q_1, \ldots, q_n).

(r1,,rm)=F(q1,,qn)(r_1, \ldots, r_m) = F(q_1, \ldots, q_n)

Body Pose

The pose/frame of a rigid body can be described by its position and orientation (wrt. a frame)

R=[xyz]=[xxyxzxxyyyzyxzyzzz]R = \begin{bmatrix} x' & y' & z' \end{bmatrix} = \begin{bmatrix} x_x' & y_x' & z_x' \\ x_y' & y_y' & z_y' \\ x_z' & y_z' & z_z' \end{bmatrix}

The elementary rotations about each of the axes are the following:

Z rotation.

Rx(α)=[1000cosαsinα0sinαcosα]Ry(α)=[cosα0sinα010sinα0cosα]Rz(α)=[cosαsinα0sinαcosα0001] \begin{aligned} R_x(\alpha) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{bmatrix} \\ R_y(\alpha) &= \begin{bmatrix} \cos \alpha & 0 & \sin \alpha \\ 0 & 1 & 0 \\ -\sin \alpha & 0 & \cos \alpha \end{bmatrix} \\ R_z(\alpha) &= \begin{bmatrix} \cos \alpha & -\sin \alpha & 0 \\ \sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ \end{aligned}

When discussing multiple frames, we use the following notation:

tofromR=toRfrom=Rtofrom{\scriptstyle \text{to} \atop \scriptstyle\text{from}} R = {\scriptstyle \text{to} \atop \scriptstyle\text{}} R {\scriptstyle \text{} \atop \scriptstyle\text{from}} = R {\scriptstyle \text{to} \atop \scriptstyle\text{from}}

E.g. if we have the same point p0,p1,p2p_0, p_1, p_2 in three different frames, we know that

p1=R21p2p0=R10p1p0=R20p2=R10(R21p2) \begin{aligned} p_1 &= R^1_2 \cdot p_2 \\ p_0 &= R^0_1 \cdot p_1 \\ p_0 &= R^0_2 \cdot p_2 = R^0_1 \left(R^1_2 \cdot p_2\right) \end{aligned}

For representing any rotation, we use Euler’s ZYZ angles, which composes three rotations (Z, Y, Z):

R=Rz(φ)Ry(ϑ)Rz(ψ)=[cφcϑcψsφsψcφcϑsψsφcψcφsϑsφcϑcψ+cφsψsφcϑsψ+cφcψsφsϑsϑcψsϑsψcϑ] \begin{aligned} R &= R_z (\varphi) R_{y'} (\vartheta) R_{z''} (\psi) \\ &= \begin{bmatrix} c_{\varphi} c_{\vartheta} c_{\psi} - s_{\varphi} s_{\psi} & -c_{\varphi} c_{\vartheta} s_{\psi} - s_{\varphi} c_{\psi} & c_{\varphi} s_{\vartheta} \\ s_{\varphi} c_{\vartheta} c_{\psi} + c_{\varphi} s_{\psi} & -s_{\varphi} c_{\vartheta} s_{\psi} + c_{\varphi} c_{\psi} & s_{\varphi} s_{\vartheta} \\ -s_{\vartheta} c_{\psi} & s_{\vartheta} s_{\psi} & c_{\vartheta} \end{bmatrix} \end{aligned}

For the inverse problem (calculating angles from a matrix of numbers), we can do φ=atan2(r2,3,r1,3)ϑ=atan2(r1,32+r2,32,r3,3)ϑ(0,π) since we took + signψ=atan2(r3,2,r3,1) \begin{aligned} \varphi &= \mathrm{atan}2 (r_{2,3}, r_{1, 3}) \\ \vartheta &= \mathrm{atan}2 \left(\sqrt{r_{1,3}^2 + r_{2,3}^2}, r_{3, 3}\right) \quad \vartheta \in (0, \pi)\ \text{since we took + sign} \\ \psi &= \mathrm{atan}2 \left(r_{3,2}, -r_{3,1}\right) \end{aligned}

Denavit & Hartenberg Notation

This is a really important concept and will 99% be on the final, so it is best to practice this. Here is a useful PDF with examples (starting at page 13) where you can practice creating the DH tables and Forward Kinematics matrices.

For relating the base and the end effector, we need to both rotate and translate, so we’ll use homogeneous coordinates (matrix is 4×44 \times 4), encoding both rotation and translation:

AEEA=[AEERAEEP0 0 01]=[r1r2r3Δxr4r5r6Δyr7r8r9Δz0001] {\scriptstyle \text{A} \atop \scriptstyle\text{EE}} A = \begin{bmatrix} {\scriptstyle \text{A} \atop \scriptstyle\text{EE}} R & {\scriptstyle \text{A} \atop \scriptstyle\text{EE}} P \\[0.7em] 0\ 0\ 0 & 1\end{bmatrix} = \begin{bmatrix} r_1 & r_2 & r_3 & \Delta x \\ r_4 & r_5 & r_6 & \Delta y \\ r_7 & r_8 & r_9 & \Delta z \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}

To create the homogeneous coordinates in a standardized way, we use the Denavit & Hartenberg (DH) notation which systematically relates the frames of two consecutive links. We have 4 parameters, each of which relates frame ii to frame i1i - 1:

parameter meaning
link length aia_i distance between ziz_i and zi1z_{i - 1} along xix_i
link offset did_i distance between xix_i and xi1x_{i - 1} along zi1z_{i - 1}
link twist αi\alpha_i angle between ziz_i and zi1z_{i - 1} around xix_i
joint angle ϑi\vartheta_i angle between xix_i and xi1x_{i - 1} around zi1z_{i - 1}

Denavit & Hartenberg Notation

A mnemonic to remember which parameter relates which is that it’s from a to z (i.e. aa relates zzs).

Anthropomorphic arm.

Example: anthropomorphic arm (3 revolute joins):

  aia_i did_i αi\alpha_i ϑi\vartheta_i
1 00 00 π/2\pi / 2 ϑ1\vartheta_1^*
2 a2a_2 00 00 ϑ2\vartheta_2^*
3 a3a_3 00 00 ϑ3\vartheta_3^*

We then get the following transformations:

A10(ϑ1)=[c10s10s10c1001000001]R=Rz(ϑ1)Rx(π/2)Aii1(ϑ1)=[cisi0aicisici0aisi00100001]i=2,3 \begin{aligned} A^0_1 (\vartheta_1) &= \begin{bmatrix} c_1 & 0 & s_1 & 0 \\ s_1 & 0 & -c_1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \quad R = R_z(\vartheta_1) \cdot R_x(\pi / 2) \\ A^{i-1}_i (\vartheta_1) &= \begin{bmatrix} c_i & -s_i & 0 & a_i c_i \\ s_i & c_i & 0 & a_i s_i \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \quad i = 2, 3\\ \end{aligned}

To get the final transformations, we can multiply the matrices and get T30(q)=A10A21A32T^0_3 (q) = A^0_1 A^1_2 A^2_3 This will yield the entire homogeneous matrix – if we only want position (in this case a homogeneous one with the last element being 11), we can only focus on the last row, which can simplify the computation: p30(q)=A10A21A32(0,0,0,1)Tp^0_3 (q) = A^0_1 A^1_2 A^2_3 \cdot (0, 0, 0, 1)^T

Things to keep in mind:

Inverse Kinematics

Definition (inverse kinematics): the process of finding a set of joint parameters (q1,,qn)(q_1, \ldots, q_n) given the position/orientation of the end-effector (r1,,rm)(r_1, \ldots, r_m).

(q1,,qn)=G(r1,,rm)(q_1, \ldots, q_n) = G(r_1, \ldots, r_m)

Definition (redundancy): arises when there are multiple inverse solutions

When dealing with inverse kinematics, an important notion are workspaces:

Workspaces example.

Analytical solution (closed form)

Spherical wrist.

Example: spherical wrist (3 revolute joins):

T63(q)=[c4c5c6s4s6c4c5s6s4c6c4s5c4s5d6s4c5c6+c4s6s4c5s6+c4c6s4s5s4s5d6s5c6s5s6c5c5d60001]T_6^3(q) = \begin{bmatrix} c_4 c_5 c_6 - s_4 s_6 & -c_4 c_5 s_6 - s_4 c_6 & c_4 s_5 & c_4 s_5 d_6 \\ s_4 c_5 c_6 + c_4 s_6 & -s_4 c_5 s_6 + c_4 c_6 & s_4 s_5 & s_4 s_5 d_6 \\ -s_5 c_6 & s_5 s_6 & c_5 & c_5 d_6 \\ 0 & 0 & 0 & 1 \end{bmatrix}

The matrix RR is a ZYZ Euler rotation matrix and can be solved as such (see Body pose).

Law of cosines illustration.

For solving various manipulators (SCARA, for example), the law of cosines might also be quite useful: c2=a2+b22 abcosγa2=b2+c22 bccosαb2=a2+c22 accosβ \begin{aligned} c^2 &= a^2 + b^2 - 2\ ab \cos \gamma \\ a^2 &= b^2 + c^2 - 2\ bc \cos \alpha \\ b^2 &= a^2 + c^2 - 2\ ac \cos \beta \end{aligned}

Definition (decoupling): dividing inverse kinematics problem for two simpler problems, inverse position kinematics and inverse orientation kinematics

Numerical solution (iterative form)

Two major ways of solving it, namely:

  1. Newton method:
    • uses Taylor series around point qkq_k to expand the forward kinematics r=F(q)=F(qk)+F(qk)qJacobian J(qk)(qqk)+O(qqk2)neglectedr = F(q) = F(q_k) + \underbrace{\frac{\partial F(q_k)}{\partial q}}_{\text{Jacobian}\ J(q_k)}(q - q_k) + \underbrace{\mathcal{O}(||q-q_k||^2)}_{\text{neglected}}
    • stripping the higher-order terms and solving for qq, we get r=F(qk)+J(qk)(qqk)rF(qk)=J(qk)(qqk)J1(qk)(rF(qk))=(qqk)q=qk+1=qk+Jr1(qk)(rF(qk)error)\begin{aligned} r &= F(q_k) + J(q_k)(q - q_k) \\ r - F(q_k) &= J(q_k)(q - q_k) \\ J^{-1}(q_k)(r - F(q_k)) &= (q - q_k) \\ q = q_{k + 1} &= q_k + J^{-1}_r(q_k) \underbrace{(r - F(q_k)}_{\text{error}}) \end{aligned}
    • is nice when n=mn = m, otherwise we have to use the pseudo-inverse
    • bad for problems near singularities (becomes unstable)
    • note: we’re using the Jacobian as a function here but it’s just a matrix
  2. Gradient method:
    • aims to minimize the squared error (the 1/21/2 is there for nicer calculations): H(q)=12F(q)r=12[F(q)r]T[F(q)r]H(q) = \frac{1}{2} ||F(q) - r|| = \frac{1}{2} [F(q) - r]^T [F(q) - r]
    • the gradient of qH(q)\nabla_q H(q) is the steepest direction to minimize this error: qH(q)=J(q)T[F(q)r] \nabla_q H(q) = J(q)^T [F(q) - r]
    • to move in this direction (given the current solution), we do the following: qk+1=qkαqH(qk)=qkαJ(q)T[F(q)r]\begin{aligned} q_{k + 1} &= q_k - \alpha \nabla_q H(q_k) \\ &= q_k - \alpha J(q)^T [F(q) - r] \end{aligned}
      • α\alpha should be chosen appropriately (dictates the size of iteration steps)
    • we don’t use the inverse but the transpose, which is nice!
    • may not converge but never diverges

The methods could also be combined – gradient first (slow but steady), then Newton.

Velocity Kinematics

Two main cases when calculating the geometric Jacobian are the following:

  Prismatic Joint ii  
JLi(q)J_{L_i} (q) zi1z_{i - 1} changes linear velocity (next joint moves linearly)
JAi(q)J_{A_i} (q) 00 doesn’t change the angular velocity
  Angular Joint ii  
JLi(q)J_{L_i} (q) zi1×pi1,EEz_{i - 1} \times p_{i - 1, EE} changes linear velocity too – moves next link
pp is the vector from i1i - 1 to EEEE
JAi(q)J_{A_i} (q) zi1z_{i - 1} changes angular velocity (next joint rotates linearly)


And they can be calculated from the forward kinematics matrix (namely the DH matrices AA): zi1=(R10(q1,,qn)rotation partof matrix A10Ri1i2(q1,,qn)rotation partof matrix Ai1i2)(0,0,1)Tjust get zpi1,EE=p0,EE(q1,,qn)in last column ofmatrix AEE0p0,i1(q1,,qi1in last column ofmatrix AEEi1) \begin{aligned} z_{i - 1} &= (\overbrace{R^0_1(q_1, \ldots, q_n)}^{\text{rotation part} \atop {\text{of matrix}\ A^0_1} } \cdot \ldots \cdot \overbrace{R^{i-2}_{i-1}(q_1, \ldots, q_n)}^{\text{rotation part} \atop {\text{of matrix}\ A^{i-2}_{i-1}} }) \overbrace{(0, 0, 1)^T}^{\text{just get\ }z} \\ p_{i - 1, EE} &= \underbrace{p_{0,EE} (q_1, \ldots, q_n)}_{\text{in last column of} \atop {\text{matrix}\ A^0_{EE} } } - \underbrace{p_{0, i - 1}(q_1, \ldots, q_{i - 1}}_{\text{in last column of} \atop {\text{matrix}\ A^{i - 1}_{EE} } }) \end{aligned}

Planar manipulator illustration.

Example: planar 2D arm:

  aia_i did_i αi\alpha_i ϑi\vartheta_i
1 a1a_1 00 00 ϑ1=q1\vartheta_1^* = q_1
2 a2a_2 00 00 ϑ2=q2\vartheta_2^* = q_2

J=(z0×p0,EEz1×p1,EEz0z1)J = \begin{pmatrix} z_0 \times p_{0,EE} & z_1 \times p_{1, EE} \\ z_0 & z_1 \end{pmatrix}

We now calculate zzs and pps from the DH matrices:

A10=(c1s10a1c1s1c10a1s100100001)A20=(c12s120a1c1+a2c12s12c120a1s1+a2s1200100001)A^0_1 = \begin{pmatrix} c_1 & -s_1 & 0 & a_1 c_1 \\ s_1 & c_1 & 0 & a_1 s_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} \quad A^0_2 = \begin{pmatrix} c_{12} & -s_{12}& 0 & a_1 c_1 + a_2 c_{12} \\ s_{12} & c_{12} & 0 & a_1 s_1 + a_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}

We therefore get p0,1=(a1c1,a1s1,0)Tp0,EE=(a1c1+a2c12,a1s1+a2s12,0)Tp_{0,1} = (a_1 c_1, a_1 s_1, 0)^T \qquad p_{0, EE} = (a_1 c_1 + a_2 c_{12}, a_1 s_1 + a_2 s_{12}, 0)^T

Since p1,EE=p0,EEp0,1p_{1,EE} = p_{0,EE} - p_{0,1}, we get the pps and, as we see, z0=z1=z2=(0,0,1)Tz_0 = z_1 = z_2 = (0, 0, 1)^T

The full Jacobian matrix looks like this: J=(z0×p0,EEz1×p1,EEz0z1)=(a1s1a2s12a2s12a1c1+a2c12a2c1200000011)J = \begin{pmatrix} z_0 \times p_{0,EE} & z_1 \times p_{1, EE} \\ z_0 & z_1 \end{pmatrix} = \begin{pmatrix} -a_1 s_1 - a_2 s_{12} & -a_2 s_{12} \\ a_1 c_1 + a_2 c_{12} & a_2 c_{12} \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1\end{pmatrix}

The rank of the matrix is between 11 and 22, depending on the joint configuration.


For calculating other differential relations, we can just continue to derivate:

Relation Equation
velocity r˙=Jr(q)q˙\dot{r} = J_r(q) \dot{q}
acceleration r¨=Jr(q)q¨+Jr˙(q)q˙\ddot{r} = J_r(q) \ddot{q} + \dot{J_r}(q) \dot{q}
jerk r=Jr(q)q+Jr˙(q)q¨+Jr¨(q)q˙\overset{\ldots}{r} = J_r(q) \overset{\ldots}{q} + \dot{J_r}(q) \ddot{q} + \ddot{J_r}(q) \dot{q}
snap \ldots

Inverse Velocity Kinematics


In configurations where the Jacobian loses rank, we’re in a kinematic singularity – we cannot find a joint velocity that realizes the desired end-effector velocity in an arbitrary direction of the task space. Close to the singularity, the joint velocities might be very large and exceed the limits.

Singularity example.

To remedy this, certain methods exist that balance error and velocity to make the robot movement smoother, like damped least squares, which minimizes the sum of minimum error norm on achieved end-effector velocity and minimum norm of joint velocity.


In the case that there is no solution to Jq˙=p˙J\dot{q} = \dot{p} (singularities, the robot simply can’t move that way, etc.), we still want to find a vector q˙\dot{q} that minimizes Jq˙p˙|| J \dot{q} - \dot{p} ||

This turns out to be the vector q˙=J#p˙\dot{q} = J^\# \dot{p}, where J#J^\# is the pseudo-inverse:

Force Kinematics

We can again use the Jacobian to relate forces of the end effector to those of the joints.

Given τRn×1\tau \in \mathbb{R}^{n \times 1} as the vector of infinitesimal joint forces/torques and γRm×1\gamma \in \mathbb{R}^{m \times 1} as the vector of infinitesimal end effector forces/torques. Applying the principle of virtual work (the work of the forces applied to the system are zero: (dW=dF dx=0dW = dF\ dx = 0)), we get dWτ=τT dq# jointsdWγ=γeTJ(q) dq# EEδqδWτ=δWγ# virtual work \begin{aligned} dW_\tau &= \tau^T\ dq \qquad & \text{\# joints} \\ dW_\gamma &= \gamma^T_e J(q)\ dq \qquad & \text{\# EE} \\ \forall \delta q \quad \delta W_\tau &= \delta W_\gamma \qquad & \text{\# virtual work} \end{aligned}

Simplifying, we get τ=JT(q)γ\tau = J^T(q) \gamma

This creates kinetostatic duality of the Jacobian

Kinetostatic Duality illustration.

As a linear algebra reminder:

Note that the problems are switched: compare the two equations p˙=J(q)q˙velocity kinematicsτ=JT(q)γ(inverse) force kinematics\overbrace{\dot{p} = J(q)\dot{q}}^{\text{velocity kinematics}} \qquad \overbrace{\tau = J^T(q) \gamma}^{\text{(inverse) force kinematics}}

For completeness, here are some properties of the Jacobian and its transpose: range(J)={p˙Rmq˙Rn,Jq˙=p˙}range(JT)={τRnγRm,τ=JTγ}kernel(J)={q˙RnJq˙=0}kernel(JT)={γRmJTγ=0}range(J)+kernel(JT)=Rmrange(JT)+kernel(J)=Rn\begin{aligned} \mathrm{range}(J) &= \left\{\dot{p} \in \mathbb{R}^m \mid \exists \dot{q} \in \mathbb{R}^n, J \dot{q} = \dot{p}\right\} \\ \mathrm{range}(J^T) &= \left\{\tau \in \mathbb{R}^n \mid \exists \gamma \in \mathbb{R}^m, \tau = J^T \gamma \right\} \\[0.5em] \mathrm{kernel}(J) &= \left\{\dot{q} \in \mathbb{R}^n \mid J \dot{q} = \mathbf{0}\right\} \\ \mathrm{kernel}(J^T) &= \left\{\gamma \in \mathbb{R}^m \mid J^T \gamma = \mathbf{0}\right\} \\[0.5em] \mathrm{range}(J) + \mathrm{kernel}(J^T) &= \mathbb{R}^m \\ \mathrm{range}(J^T) + \mathrm{kernel}(J) &= \mathbb{R}^n \end{aligned}

Trajectory Planning

Task: generate reference inputs to the controller when given:

  1. only start and end defined (minimum requirements)
  2. sequence of points defined (more difficult)

Definition (path): set of points in the joint space which the manipulator has to follow in the execution of the assigned motion (purely geometric).

Definition (trajectory): a path on which a timing is specified (velocities, accelerations, etc.).

To plan the trajectory, we do the following:

  1. get the sequence of pose points (i.e. knots) in the Cartesian space
  2. create a geometric path linking the knots (interpolation)
  3. sample the path and transform the sequence to the Joint space
  4. interpolate in the Joint space

Trajectory Planning example.

We do this because planning in the Cartesian space can easily move the robot into configurations that can’t be realized, which isn’t the case for the Joint space.

The time law is chosen based on task specification:

In Joint Space

Task: smoothly interpolate between points in the joint space

Cubic polynomial

Useful for interpolating between two positions (q0,q1q_0, q_1) given the start and end velocity (v0,v1v_0, v_1): q(0)=q0q(1)=q1q(0)=v0q(1)=v1q(0) = q_0 \quad q(1) = q_1 \quad q'(0) = v_0 \quad q'(1) = v_1

The cubic polynomial in the normalized form is the following: q(λ)=q0+q[aλ3+bλ2+cλ+d]for q=q1q0;λ[0,1]q(\lambda) = q_0 + \nabla q[a \lambda^3 + b \lambda^2 + c \lambda + d] \qquad \text{for}\ \nabla q = q_1 - q_0; \lambda \in \left[0, 1\right]

Plugging for known values helps us solve the polynomial:

If we don’t care about normalization, the general form is the following: q(λ)=aλ3+bλ2+cλ+dq(\lambda) = a \lambda^3 + b \lambda^2 + c \lambda + d


When we have NN knots and want continuity up to the second derivative, we can concatenate N1N - 1 cubic polynomials, which minimizes the curvature among all interpolating functions (with 22nd derivatives)

Cubic spline illustration

In Cartesian Space

Notation here is p=p(s)s=s(t)p = p(s) \rightarrow s = s(t) (as opposed to q=q(λ)λ=λ(t)q = q(\lambda) \rightarrow \lambda = \lambda(t)), since we’re planning the position of the end-effector, not the position in the joint space.

Has advantages and disadvantages:

When calculating the path:


Given pi,pf,vmax,amaxp_i, p_f, v_{\mathrm{max}}, a_{\mathrm{max}} and vi,vfv_{\mathrm{i}}, v_{\mathrm{f}} (usually 00), we want to analyze the trajectory.

For a linear path from pip_i to pfp_f:

For the motion, we use the trapezoidal speed (bang-coast-bang):

Bang-coast-bang analysis illustration.

To generalize this (i.e. concatenate the paths), we can over-fly the point in between. A specific example of how this would look like is the following:

Fly-over analysis illustration.

  1. use minimal representations (e.g. ZYZ Euler angles) and plan the trajectory independently
  2. use axis/angle representation:
    • determine a neutral axis and an angle by which to rotate
    • plan a timing law to interpolate the angle and the axis
Optimal trajectories

For Cartesian robots (PPP):

  1. straight line joining the two positions is one of those
    • can be executed in minimum time under velocity-acceleration constraints
  2. optimal timing law is the bang-coast-bang

For articulated robots (at least one R):

Uniform time scaling

Say we want to slow a given timing law down or speed it up because of constraints, we don’t need to recalculate all of the motion profiles: velocity scales linearly, acceleration scales quadratically: dp/dt=dp/dsds/dτ1/Td2p/dt2=(d2p/ds2(ds/dτ)2+dp/dsd2s/dτ2)1/T2 \begin{aligned} dp/dt &= dp/ds \cdot ds/d\tau \cdot {\color{red}{1/T}} \\ d^2p/dt^2 &= (d^2p/ds^2 \cdot (ds/d\tau)^2 + dp/ds \cdot d^2s/d\tau^2) \cdot {\color{red}{1/T^2}} \end{aligned}

Example (max relative violations) with T=1[s]T = 1 [\mathrm{s}]:

Minimum uniform time scaling is therefore k=max{1,kvel,kacc}k = \max \left\{1, k_{\text{vel}}, \sqrt{k_{\text{acc}}}\right\}


Velocity manipulability

In a given configuration, we want to evaluate how „effective“ is the mechanical transformation between joint velocities and EE velocities (how „easily“ can EE move in various directions of the task space).

q˙Tq˙=vTJ#TJ#vellipsoid=1\dot{q}^T \dot{q} = \underbrace{v^T J^{\#T}J^\#v}_{\text{ellipsoid}} = 1

Velocity and force manipulability.

Force manipulability

We can apply the exact same concept to forces:

τTτ=FTJJTFellipsoid=1\tau^T \tau = \underbrace{F^TJJ^{T}F}_{\text{ellipsoid}} = 1

The comparison between the two is interesting – they are orthogonal (velocity left; force right):

Cubic spline illustration

The explanation that the teacher gave in class is pretty good: if your arm is almost stretched, it can move very quickly up and down but not too quickly forward/backward. On the other hand (pun not intended), the forces you can apply up/down are not nearly as large as when pushing/pulling.