# Kinematics


```python
import numpy as np
from sympy import *
from IPython.display import display
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

%matplotlib notebook
init_printing()
```

### Helpers

Create rotational matrix generator.


```python
def rotate(p, theta, o, direction):
    """
    Rotate a point about an direction through a point.
    Represents a counterclockwise rotation by theta about the line passing through point pointing in direction.
    :param p: The point to be rotated.
    :param theta: How much to rotate p.
    :param o: The origin of the rotational axis.
    :param direction: The direction of the rotational axis.
    """
    
    # Ensure direction is matrix (for norm).
    direction = Matrix(direction)
    
    # Unpack.
    u, v, w = direction / direction.norm()
    x, y, z = p
    a, b, c = o
    
    x1 = (a*(v**2+w**2)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos(theta))+x*cos(theta)+(-c*v+b*w-w*y+v*z)*sin(theta)
    y1 = (b*(u**2+w**2)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cos(theta))+y*cos(theta)+(c*u-a*w+w*x-u*z)*sin(theta)
    z1 = (c*(u**2+v**2)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos(theta))+z*cos(theta)+(-b*u+a*v-v*x+u*y)*sin(theta)
    
    return Matrix((x1, y1, z1))
```

Create pretty simplification function.


```python
def pretty(eqn, expressions, symbols):
    """
    Does pretty simplification by replacing expressions in a expression or matrix with symbols.
    :param eqn: The desired input expression or matrix.
    :param expressions: The expressions to replace.
    :param symbols: The symbols to use.
    """
    
    eqn = eqn.copy()

    for expr, sub in zip(expressions, symbols):
        eqn = eqn.subs(expr, sub)
        
    return simplify(eqn)
```

Create plotting functions.


```python
def equal_ax(ax):
    extents = np.array([getattr(ax, 'get_{}lim'.format(dim))() for dim in 'xyz'])
    sz = extents[:,1] - extents[:,0]
    centers = np.mean(extents, axis=1)
    maxsize = max(abs(sz))
    r = maxsize/2
    for ctr, dim in zip(centers, 'xyz'):
        getattr(ax, 'set_{}lim'.format(dim))(ctr - r, ctr + r)
     
    
def plot_arm(points):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    
    ax.set_xlabel('X axis')
    ax.set_ylabel('Y axis')
    ax.set_zlabel('Z axis')
    
    xs, ys, zs = zip(*points)
    ax.plot(xs, ys, zs, 'go-')
    
    ax.view_init(30, 150)
    equal_ax(ax)
    
    plt.show()
```

### Preliminary Tests

Run test cases for rotations about origin. Ensure that the rotate function works.


```python
theta = Symbol('theta')
nx = (1, 0, 0)
ny = (0, 1, 0)
nz = (0, 0, 1)
o = (0, 0, 0)
```


```python
# Rotate p about x.
p = Matrix((1, 0, 0))
rotate(p, theta, o, nx)
```




$$\left[\begin{matrix}1\\0\\0\end{matrix}\right]$$




```python
# Rotate p about y.
p = Matrix((1, 0, 0))
rotate(p, theta, o, ny)
```




$$\left[\begin{matrix}\cos{\left (\theta \right )}\\0\\- \sin{\left (\theta \right )}\end{matrix}\right]$$




```python
# Rotate p about z.
p = Matrix((1, 0, 0))
rotate(p, theta, o, nz)
```




$$\left[\begin{matrix}\cos{\left (\theta \right )}\\\sin{\left (\theta \right )}\\0\end{matrix}\right]$$



### Forward Kinematics

Define lengths for arm segments. Graph points for base condition.


```python
l1 = 10
l2 = 10
l3 = 2
l4 = 5

p0 = (0, 0, 0)
p1 = (0, 0, l1)
p2 = (0, 0, l1 + l2)
p3 = (0, l3, l1 + l2)
p4 = (0, l3, l1 + l2 + l4)

points = [p0, p1, p2, p3, p4]

plot_arm(points)
```


    <IPython.core.display.Javascript object>






Define all variables and angles. $l_3$ can be negative depending on design and assembly.


```python
theta1, theta2, theta3, theta4, l3 = symbols('theta_1 theta_2 theta_3 theta_4 l_3', real=True)
l1, l2, l4 = symbols('l_1 l_2 l_4', positive=True)

o0 = (0, 0, 0)
o1 = (0, 0, l1)
o3 = (0, l3, l1 + l2)
```

Solving for $p_0$ is trivial. It is always at the defined origin.


```python
p0 = Matrix((0, 0, 0))
p0
```




$$\left[\begin{matrix}0\\0\\0\end{matrix}\right]$$



To solve for $p_1$, account for $\theta_1$ and $\theta_2$.


```python
p1 = Matrix((0, 0, l1))
p1_sym = symbols('p_1x p_1y p_1z')

p1 = rotate(p1, theta2, o0, ny)
p1 = rotate(p1, theta1, o0, nz)

p1 = p1.expand()

simplify(p1)
```




$$\left[\begin{matrix}l_{1} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )}\\l_{1} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )}\\l_{1} \cos{\left (\theta_{2} \right )}\end{matrix}\right]$$



To solve for $p_2$, account for $\theta_1$ through $\theta_3$ (inclusive).


```python
p2 = Matrix((0, 0, l1 + l2))
p2_sym = symbols('p_2x p_2y p_2z')

p2 = rotate(p2, theta3, o1, ny)
p2 = rotate(p2, theta2, o0, ny)
p2 = rotate(p2, theta1, o0, nz)

p2 = p2.expand()

pretty(p2, p1, p1_sym)
```




$$\left[\begin{matrix}l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} + p_{1x}\\l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} + p_{1y}\\l_{2} \cos{\left (\theta_{2} + \theta_{3} \right )} + p_{1z}\end{matrix}\right]$$



To solve for $p_3$, account for the same rotations as $p_2$.


```python
p3 = Matrix((0, l3, l1 + l2))
p3_sym = symbols('p_3x p_3y p_3z')

p3 = rotate(p3, theta3, o1, ny)
p3 = rotate(p3, theta2, o0, ny)
p3 = rotate(p3, theta1, o0, nz)

p3 = p3.expand()

pretty(p3, p2, p2_sym)
```




$$\left[\begin{matrix}- l_{3} \sin{\left (\theta_{1} \right )} + p_{2x}\\l_{3} \cos{\left (\theta_{1} \right )} + p_{2y}\\p_{2z}\end{matrix}\right]$$



To solve for $p_4$, account for the all rotations.


```python
p4 = Matrix((0, l3, l1 + l2 + l4))

p4 = rotate(p4, theta4, o3, ny)
p4 = rotate(p4, theta3, o1, ny)
p4 = rotate(p4, theta2, o0, ny)
p4 = rotate(p4, theta1, o0, nz)

p4 = p4.expand()

pretty(p4, p3, p3_sym)
```




$$\left[\begin{matrix}l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} \cos{\left (\theta_{1} \right )} + p_{3x}\\l_{4} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} + p_{3y}\\l_{4} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} + p_{3z}\end{matrix}\right]$$



Graph some test conditions.


```python
def graph_fk(lengths, angles):
    sub = [(theta1, angles[0]), (theta2, angles[1]), (theta3, angles[2]), (theta4, angles[3]),
           (l1, lengths[0]), (l2, lengths[1]), (l3, lengths[2]), (l4, lengths[3])]
    points = [p0, p1.subs(sub), p2.subs(sub), p3.subs(sub), p4.subs(sub)]
    
    plot_arm(points)
    
    return points
```


```python
lengths = (10, 10, 2, 5)
angles = (0, 0, 0, 0)
graph_fk(lengths, angles)
```


    <IPython.core.display.Javascript object>









$$\left [ \left[\begin{matrix}0\\0\\0\end{matrix}\right], \quad \left[\begin{matrix}0\\0\\10\end{matrix}\right], \quad \left[\begin{matrix}0\\0\\20\end{matrix}\right], \quad \left[\begin{matrix}0\\2\\20\end{matrix}\right], \quad \left[\begin{matrix}0\\2\\25\end{matrix}\right]\right ]$$



### Inverse Kinematics

Given a 4 DOF system, there needs to be additional constraints aside from a point in space to avoid obtaining infinite solutions. For a general hand-like robot, it is helpful to define the direction of the vector created by the last two points. Redefine points $p_0$ to $p_4$ as $A$ to $E$.

The directional constraint, $\phi$, defines the angle formed by the vector $\overrightarrow{DE}$ and the normal of the XY-plane (Z axis). The point $(x, y, z)$ defines the desired target point in space.


```python
x, y, z, phi = symbols('x y z phi', real=True)
```

Given $\phi$, it is possible to solve for $C$. Drawing out the diagram basically reveals two similar right triangles. Connecting all of the vertices reveals a trapazoid. The two diagonals are known. Two sides are known. Let the sides be $(a,b,c,d)$. $a = l_4\sin{\left(\phi\right)}$ and $d = \sqrt{a^2 + l_3^2}$. The diagonals are $l_3$ and the distance of the target from the origin. $c$ can be solved using the equations detailed [here](http://mathworld.wolfram.com/Trapezoid.html).


```python
a, b, c, d = symbols('a, b, c, d', real=True)
c1 = solve(l3**2 - (a*b**2-a**2*b-a*c**2+b*d**2)/(b-a), c)[1]
c1
```


```python
c2 = solve(x**2+y**2 - (a*b**2-a**2*b-a*d**2+b*c**2)/(b-a), c)[1]
c2
```

Setting both of the above equations equal to each other allows the solution for $b$ to be found. We need to find $\theta_0$, the angle difference caused by $l_3$. We can solve for the opposite leg of the right triangle and use it along with $b$ to find the desired angle.


```python
u = Symbol('u', positive=True)
u_soln = solve(a / (l3 - u) - b / u, u)[0]
u_soln
```


```python
b_soln = solve(c1 - c2, b)[1]
simplify(b_soln.expand())
```


```python
theta_0 = atan(u_soln.subs(b, b_soln) / b_soln).subs([(a, l4*sin(phi)),(d, sqrt(l3**2+(l4*sin(phi))**2))])
simplify(theta_0.expand())
```

We need to rewrite this equation because $\phi$ can cause this equation to be undefined and it is known analytically that $\phi$ does not play a role here.


```python
theta_0 = atan2(l3, sqrt(x**2+y**2-l3**2))
theta_0
```

The next step is to actually solve for C. We can define the vector $\overrightarrow{V} = \overrightarrow{CE}$. $C$ is really just $\overrightarrow{OC}$. Therefore, $\overrightarrow{C} = \overrightarrow{T} - \overrightarrow{CE}$. The vector $\overrightarrow{CE}$ can be described as two rotations of $\overrightarrow{{CE}_{0}}$ -- one rotation about the Z-axis and another rotation about a rotated Y-axis vector. $\overrightarrow{D}$ can be solved in a similar manner. Let $\overrightarrow{n}$ be the the Y-axis normal vector rotated by $R = \operatorname{atan2}{\left(y, x\right)} - \theta_0$.

The interesting thing to realize here is that there are two possible cases. In one case, the arm is not flipped. In the other case it is. We need to account for both cases.


```python
theta0 = Symbol('theta_0', real=True)

# Define CD vector.
CD = Matrix((0, 0, l4))

# Define D.
D = Matrix((x, y, z)) - CD
D_sym = symbols('D_x D_y D_z', real=True)

# Define oE.
oE = (x, y, z)

# Rotate ny about Z-axis.
R1 = atan2(y, x) - theta0
R2 = atan2(y, x) + theta0

n1 = rotate(ny, R1, o, nz)
n2 = rotate(ny, R2, o, nz)
n = [n1, n2]

# Rotate D about rotated Y-normal.
D1 = rotate(D, phi, oE, n1)
D2 = rotate(D, phi, oE, n2)

D1 = simplify(D1.expand())
D2 = simplify(D2.expand())

D = [D1, D2]

# Output.
D
```




$$\left [ \left[\begin{matrix}- l_{4} \sin{\left (\phi \right )} \cos{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )} + x\\l_{4} \sin{\left (\phi \right )} \sin{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )} + y\\- l_{4} \cos{\left (\phi \right )} + z\end{matrix}\right], \quad \left[\begin{matrix}- l_{4} \sin{\left (\phi \right )} \cos{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )} + x\\- l_{4} \sin{\left (\phi \right )} \sin{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )} + y\\- l_{4} \cos{\left (\phi \right )} + z\end{matrix}\right]\right ]$$




```python
n
```




$$\left [ \left[\begin{matrix}\sin{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\\cos{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\0\end{matrix}\right], \quad \left[\begin{matrix}- \sin{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\\cos{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\0\end{matrix}\right]\right ]$$




```python
# Define CE vector.
CE1 = Matrix((0, l3, l4))
CE2 = Matrix((0, -l3, l4))

# Define C.
C1 = Matrix((x, y, z)) - CE1
C2 = Matrix((x, y, z)) - CE2

C_sym = symbols('C_x C_y C_z', real=True)

# Rotate C about Z-axis.
C1 = rotate(C1, R1, oE, nz)
C2 = rotate(C2, R2, oE, nz)

# Rotate C about rotated Y-normal.
C1 = rotate(C1, phi, oE, n1)
C2 = rotate(C2, phi, oE, n2)

C1 = simplify(C1.expand())
C2 = simplify(C2.expand())

C = [C1, C2]

# Output.
[pretty(C[0], D[0], D_sym), pretty(C[1], D[1], D_sym)]
```




$$\left [ \left[\begin{matrix}D_{x} - l_{3} \sin{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\D_{y} - l_{3} \cos{\left (\theta_{0} - \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\D_{z}\end{matrix}\right], \quad \left[\begin{matrix}D_{x} - l_{3} \sin{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\D_{y} + l_{3} \cos{\left (\theta_{0} + \operatorname{atan_{2}}{\left (y,x \right )} \right )}\\D_{z}\end{matrix}\right]\right ]$$



Given $C$, it is possible to use the law of cosines to solve for $\theta_3$. $\theta_3$ can either be positive or negative.


```python
theta_3 = pi - acos((l1**2 + l2**2 - C[1].norm()**2) / (2 * l1 * l2))

sol1 = pretty(theta_3, C[1], C_sym)
sol2 = -sol1
theta_3 = [sol1, sol2]
theta_3
```

We can solve for $\theta_2$ by assuming that $B$ still rests on the Z-axis. Define $C^{\prime\prime}$ as the point $C$ with only $\theta_3$. Define $C^{\prime}$ as the point $C$ with only $\theta_2$ and $\theta_3$. $C^{\prime}_z$ must be equal to $C_z$ to form a radius for $\theta_1$ to complete. It is better to solve for the equation involving z rather than x and y, because $\theta_1$ does not impact z.


```python
C_pp = (l2 * sin(theta3), 0, l1 + l2 * cos(theta3))
C_p = rotate(C_pp, theta2, o, ny)
theta_2 = solve(C_p[2] - C_sym[2], theta2)
theta_2
```

It is not immediately evident which solution to pick for the non-flipped vs. flipped. In fact, it basically is not evident at all. Test shows that the first solution works for non-flipped and the second for flipped. However, they flip when $x^2 + y^2 < l_3^2 + (l_4 \sin{(\phi)})^2$ and $\sin{(\phi)} > 0$. This is an observed result from extensive testing. I am sure there is good mathematical justification, but it is probably to difficult to derive from equations.

$\theta_1$ can actually be by solved using the forward kinematic equations for $C$. Assuming $\theta_1$ is 0, compute the current angle with the x-axis. Then, simply swing around. It might be tempting to use $C$, but there can be issues because it can sometimes be on the Z-axis. Using $D$ is better as it cannot be on the Z-axis unless $l_3$ is zero (in that case it does't matter anyway since $\theta_0$ will be zero).


```python
theta_1 = atan2(D_sym[1], D_sym[0]) - atan2(p3[1].subs(theta1, 0), simplify(p3[0].subs(theta1, 0)))
theta_1
```

We can now solve for $\theta_4$. Define two vectors, $\overrightarrow{DE}$ and $\overrightarrow{BC}$. $B$ can be solved using the forward kinematics equation. The dot product can be used to determine the angle. The sign can be determined using the cross product. Define $s$ as the sign. Account for multiple solutions due to inverse cosine. Testing reveals that the second solution applies to the flipped case.

$$s = \operatorname{sgn}{\left[ \overrightarrow{n} \cdot \left( \overrightarrow{BC} \times \overrightarrow{DE} \right) \right] }$$

$$
\left[ 
s \cdot \operatorname{acos}{\left( \frac{\overrightarrow{BC} \cdot \overrightarrow{DE} }{l_2 l_4} \right)}, \quad
-s \cdot \operatorname{acos}{\left( \frac{\overrightarrow{BC} \cdot \overrightarrow{DE} }{l_2 l_4} \right)}
\right]
$$

For most constraint conditions of $(x, y, z, \phi)$, there are at most four solutions (and usually four solutions). The kinematic solution is stable around singularities. In cases where less than four solutions exist (i.e. one or two), all four equations will agree on these solutions. There is no known case of three unique solutions.

### Jacobian Analysis

We can define the forward Jacobian matrix to assist in computing velocities and torques. First, we need to compute the Jacobian for the linear velocity, $J_v(\theta)$.


```python
wrt = Matrix((theta1, theta2, theta3, theta4))
J_v = simplify(p4).jacobian(wrt)
J_v
```




$$\left[\begin{matrix}- l_{1} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} - l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{3} \cos{\left (\theta_{1} \right )} - l_{4} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{1} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + l_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\\l_{1} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} + l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} - l_{3} \sin{\left (\theta_{1} \right )} + l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} \cos{\left (\theta_{1} \right )} & l_{1} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + l_{2} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{2} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\\0 & - l_{1} \sin{\left (\theta_{2} \right )} - l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & - l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\end{matrix}\right]$$



We also need to define the Jacobian of angular velocities, $J_\omega(\theta)$.


```python
w = Matrix((0, theta2 + theta3 + theta4, theta1))
J_w = w.jacobian(wrt)
J_w
```




$$\left[\begin{matrix}0 & 0 & 0 & 0\\0 & 1 & 1 & 1\\1 & 0 & 0 & 0\end{matrix}\right]$$



We can combine the two results to get the full Jacobian matrix, $J_{ee}(\theta)$.


```python
J_ee = Matrix((J_v, J_w))
J_ee
```




$$\left[\begin{matrix}- l_{1} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} - l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{3} \cos{\left (\theta_{1} \right )} - l_{4} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{1} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + l_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{4} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\\l_{1} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} + l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} - l_{3} \sin{\left (\theta_{1} \right )} + l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} \cos{\left (\theta_{1} \right )} & l_{1} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + l_{2} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{2} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & l_{4} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\\0 & - l_{1} \sin{\left (\theta_{2} \right )} - l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & - l_{2} \sin{\left (\theta_{2} + \theta_{3} \right )} - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )} & - l_{4} \sin{\left (\theta_{2} + \theta_{3} + \theta_{4} \right )}\\0 & 0 & 0 & 0\\0 & 1 & 1 & 1\\1 & 0 & 0 & 0\end{matrix}\right]$$



From the Jacobian, the velocities and torque can be related. This ensures if a particular operation is possible.

$$\dot{x} = J \dot{\theta}$$

$$\tau = J^T F$$

262a261f46f905a0a0a9028ba2526148f1edccb3
ipynb
Jupyter Notebook
notebook/Kinematics.ipynb
bobbyluig/6.A01
16dd8963951eca4a1312a15c216d0cc3c117d063
MIT
notebook/Kinematics.ipynb
bobbyluig/6.A01
16dd8963951eca4a1312a15c216d0cc3c117d063
MIT
notebook/Kinematics.ipynb
bobbyluig/6.A01
16dd8963951eca4a1312a15c216d0cc3c117d063
MIT
2021-02-24T07:13:01.000Z
2021-02-24T07:13:01.000Z
Qwen/Qwen-72B
1. YES
2. YES