# Forward kinematics of 2dof planar robots

## Case 1) Two revolute joints



## Case 2) Revolute joint followed by prismatic joint





```python
import numpy as np
import pandas as pd
import plotly.express as px
import plotly.graph_objects as go
import doctest
import spatialmath as sm
import sympy as sy
import sys
```


```python
 def fwd_kinematics_2rev(th1, th2, l1=2, l2=1):
    '''
    Implements the forward kinematics of a robot with two revolute joints. 
    
    Arguments
    ---------
    th1, th2 : float
       Angle in radians of the two degree of freedoms, respectively.
    l1, l2 : float
       Length of the two links, respectively.
 
    Returns
    -------
    x : float
       The position in the global x-direction of the end-effector (tool point)
    y : float
       The position in the global y-direction of the end-effector (tool point)
    theta : float
       The orientation of the end-effector with respect to the positive global x-axis.
       The angle returned is in the range [-np.pi, np.pi]
   j : tuple with 2 elements
       The position of the joint between the two links
    
    Tests
    ------
    1) End-effector pose at default position
    >>> x, y, th, j = fwd_kinematics_2rev(0, 0)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(3.00, 0.00, 0.00)'
    
    2) End-effector pose at 90 degrees in both joints
    >>> x, y, th, j = fwd_kinematics_2rev(np.pi/2, np.pi/2)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(-1.00, 2.00, 3.14)'
    
    3) End-effector pose at 0 degress in first joint and 90 degress in second
    >>> x, y, th, j = fwd_kinematics_2rev(0, np.pi/2)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(2.00, 1.00, 1.57)'
    
    4) End-effector position is always inside a circle of a certain radius
    >>> poses = [fwd_kinematics_2rev(th1_, th2_, 3, 2)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2) 
    ... for th2_ in np.arange(0, 2*np.pi, 0.2)]
    >>> distances = np.array([np.sqrt(x_**2 + y_**2) for x_, y_, th_, j_ in poses])
    >>> max_radius = 5 + 1e-12 # Add a small tolerance
    >>> np.any(distances > max_radius)
    False
    
    5) Joint is always at constant distance from the origin
    >>> poses = [fwd_kinematics_2rev(th1_, 0, 3, 2)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2) ] 
    >>> distances = np.array([np.sqrt(j_[0]**2 + j_[1]**2) for x_, y_, th_, j_ in poses])
    >>> np.any(np.abs(distances - 3) > 1e-12)
    False
    
    '''
    
    # Static transformation between frame 2 and frame E
    g_2e = sm.SE3()
    g_2e.A[0,3] = l1+l2
    
    # Transformation betwen frame 1 and frame 2
    g_I = sm.SE3() # Identity transformation
    g_12 = sm.SE3.Rz(th2) # Rotation about z-axis
    q = [l1,0,0]
    d_12 = g_I*q - g_12*q
    g_12.A[:3, 3] = d_12.ravel()
    
    # Transformation between frame S and frame 1
    g_s1 = sm.SE3.Rz(th1)
    
    # Chain of transformations
    g_se = g_s1 * g_12 * g_2e


    x = g_se.A[0,3]
    y = g_se.A[1, 3]
    
    theta = th1+th2
    #print(np.arccos(g_se[0,0]), theta)

    #assert(np.abs(theta-np.arccos(g_se[0,0])) < 1e-8)
    
    j_s = g_s1 * [l1,0,0]

    j = tuple(j_s[:2])

    return (x, y, theta, j)

```


```python
# Case 1)
doctest.run_docstring_examples(fwd_kinematics_2rev, globals(), verbose=True)
```

    Finding tests in NoName
    Trying:
        x, y, th, j = fwd_kinematics_2rev(0, 0)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(3.00, 0.00, 0.00)'
    ok
    Trying:
        x, y, th, j = fwd_kinematics_2rev(np.pi/2, np.pi/2)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(-1.00, 2.00, 3.14)'
    ok
    Trying:
        x, y, th, j = fwd_kinematics_2rev(0, np.pi/2)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(2.00, 1.00, 1.57)'
    ok
    Trying:
        poses = [fwd_kinematics_2rev(th1_, th2_, 3, 2)
        for th1_ in np.arange(0, 2*np.pi, 0.2) 
        for th2_ in np.arange(0, 2*np.pi, 0.2)]
    Expecting nothing
    ok
    Trying:
        distances = np.array([np.sqrt(x_**2 + y_**2) for x_, y_, th_, j_ in poses])
    Expecting nothing
    ok
    Trying:
        max_radius = 5 + 1e-12 # Add a small tolerance
    Expecting nothing
    ok
    Trying:
        np.any(distances > max_radius)
    Expecting:
        False
    ok
    Trying:
        poses = [fwd_kinematics_2rev(th1_, 0, 3, 2)
        for th1_ in np.arange(0, 2*np.pi, 0.2) ] 
    Expecting nothing
    ok
    Trying:
        distances = np.array([np.sqrt(j_[0]**2 + j_[1]**2) for x_, y_, th_, j_ in poses])
    Expecting nothing
    ok
    Trying:
        np.any(np.abs(distances - 3) > 1e-12)
    Expecting:
        False
    ok



```python
 def fwd_kinematics_2rev_symbolic(th1, th2, 
                                  l1=sy.symbols('l1'), l2=sy.symbols('l2')):
    '''
    Implements the forward kinematics of a robot with two revolute joints. 
    
    Arguments
    ---------
    th1, th2 : sympy symbols
       Symbol representing the angle in radians of the two degree of freedoms, respectively.
    l1, l2 : sympy symbols
       Symbol representing the length of the two links, respectively.
 
    Returns
    -------
    x : sympy expression
       The position in the global x-direction of the end-effector (tool point)
    y : sympy expression
       The position in the global y-direction of the end-effector (tool point)
    theta : sympy expression
       The orientation of the end-effector with respect to the positive global x-axis.
    j : tuple with 2 elements, each is a sympy expression
       The position of the joint between link1 and link2
       
    Tests
    ------
    1) End-effector pose at default position
    >>> th1, th2, l1, l2 = sy.symbols('th1, th2, l1, l2') 
    >>> x, y, th, j = fwd_kinematics_2rev_symbolic(th1, th2, l1, l2)
    >>> subsdict = {th1: 0, th2: 0, l1: 2, l2: 1}
    >>> xn = x.evalf(subs=subsdict)
    >>> yn = y.evalf(subs=subsdict)
    >>> thn = th.evalf(subs=subsdict)
    >>> "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    '(3.00, 0.00, 0.00)'
    
    2) End-effector pose at 90 degrees in both joints
    >>> th1, th2, l1, l2 = sy.symbols('th1, th2, l1, l2') 
    >>> x, y, th, j = fwd_kinematics_2rev_symbolic(th1, th2, l1, l2)
    >>> subsdict = {th1: np.pi/2, th2: np.pi/2, l1: 2, l2: 1}
    >>> xn = x.evalf(subs=subsdict)
    >>> yn = y.evalf(subs=subsdict)
    >>> thn = th.evalf(subs=subsdict)
    >>> "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    '(-1.00, 2.00, 3.14)' 
    '''
    
    # Static transformation between frame 2 and frame E
    g = sy.eye(4)
    g[0, 3] = l1+l2
    g_2e = sm.SE3(np.array(g), check=False)
    
    # Transformation betwen frame 1 and frame 2
    g_I = sm.SE3(np.array(sy.eye(4)), check=False) # Identity transformation
    g_12 = sm.SE3.Rz(th2) # Rotation about z-axis
    q = [l1,0,0]
    d_12 = g_I*q - g_12*q
    g_12.A[:3, 3] = d_12.ravel()
    
    # Transformation between frame S and frame 1
    g_s1 = sm.SE3.Rz(th1)
    
    # Chain of transformations
    g_se = g_s1 * g_12 * g_2e


    x = g_se.A[0,3]
    y = g_se.A[1, 3]
    
    theta = th1+th2
    #print(np.arccos(g_se[0,0]), theta)

    #assert(np.abs(theta-np.arccos(g_se[0,0])) < 1e-8)
    
    j_s = g_s1 * [l1,0,0]

    j = tuple(j_s[:2])

    return (x, y, theta, j)
 
```


```python
doctest.run_docstring_examples(fwd_kinematics_2rev_symbolic, globals())
```

    **********************************************************************
    File "__main__", line 43, in NoName
    Failed example:
        "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    Expected:
        '(-1.00, 2.00, 3.14)' 
    Got:
        '(-1.00, 2.00, 3.14)'



```python
def fwd_kinematics_rev_prism(th1, th2, l1=2):
    '''
    Implements the forward kinematics of a robot with one revolute joint and one prismatic. 
    
    Arguments
    ---------
    th1 : float
       Angle in radians of the first degree of freedom.
    th2 : float
       Displacement in meter of the second degree of freedom.
    l1 : float
       Length of the first link.
 
    Returns
    -------
    x : float
       The position in the global x-direction of the end-effector (tool point)
    y : float
       The position in the global y-direction of the end-effector (tool point)
    theta : float
       The orientation of the end-effector with respect to the positive global x-axis
    
    Tests
    ------
    1) End-effector pose at default position
    >>> "(%0.2f, %0.2f, %0.2f)" %fwd_kinematics_rev_prism(0, 0)
    '(2.00, 0.00, 0.00)'
    
    2) End-effector pose at 90 degrees in first joint and 0.6m in second
    >>> "(%0.2f, %0.2f, %0.2f)" %fwd_kinematics_rev_prism(np.pi/2, 0.6)
    '(0.00, 2.60, 1.57)'
    
    4) End-effector orientation is always the same as the angle of the first dof
    >>> angles = np.array( [th1_ for th1_ in np.arange(0, 2*np.pi, 0.2)
    ... for th2_ in np.arange(-1, 1, 0.2)])
    >>> poses = [fwd_kinematics_rev_prism(th1_, th2_)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2)
    ... for th2_ in np.arange(-1, 1, 0.2)]
    >>> orientations = np.array([th_ for x_, y_, th_ in poses])
    >>> np.any(np.abs(angles-orientations) > 1e-12)
    False
    '''
    x = 0
    y = 0
    theta = 0
    return (x, y, theta)

```

## Run doctests
If tests pass, no output is generated.


```python
# Case 2)
doctest.run_docstring_examples(fwd_kinematics_rev_prism, globals())
```

## Visualize the work space of the robot


```python
th1 = np.arange(0, 2*np.pi, 0.1)
th2 = np.arange(-np.pi, np.pi, 0.1)
xythetaj =[ fwd_kinematics_2rev(th1_, th2_) for th1_ in th1 for th2_ in th2] 
xytheta = np.array([ (x_, y_, th_) for x_, y_, th_, j_ in xythetaj])
df = pd.DataFrame(data=np.reshape(xytheta, (-1,3)), columns=['x', 'y', 'theta'])
```


```python
fig = px.scatter_3d(df, x='x', y='y', z='theta')
camera = dict(
    up=dict(x=0, y=1, z=0),
    center=dict(x=0, y=0, z=0),
    eye=dict(x=0, y=0, z=4)
)
fig.update_scenes(camera_projection_type="orthographic")
fig.update_layout(scene_camera=camera)
fig.show()


```

## Visualize movement of the manipulator


```python
poses = [ fwd_kinematics_2rev(th1_, th2_) for th1_, th2_ in zip(th1, th2)]
endeff_trajectory = np.array([ [x_, y_]  for x_, y_, th_, j_ in poses])
joint_trajectory = np.array([ j_  for x_, y_, th_, j_ in poses])
```


```python
fig = go.Figure(
    data=[go.Scatter(x=[0, joint_trajectory[0,0]], y=[0, joint_trajectory[0,1]], 
          name="First link", mode="lines",
          line=dict(width=6, color="blue")),
          go.Scatter(x=[joint_trajectory[0,0], endeff_trajectory[0,0]], 
                     y=[joint_trajectory[0,1], endeff_trajectory[0,1]], 
          name="Second link", mode="lines",
          line=dict(width=5, color="red")),
         go.Scatter(x=joint_trajectory[:,0], y=joint_trajectory[:,1], 
          name="Joint trajectory", mode="lines",
          line=dict(width=1, color="lightblue")),
          go.Scatter(x=endeff_trajectory[:,0], y=endeff_trajectory[:,1], 
          name="End-point trajectory", mode="lines",
          line=dict(width=1, color="red"))],
    layout=go.Layout( width=700, height=600,
        xaxis=dict(range=[-4, 4], autorange=False),
        yaxis=dict(range=[-4, 4], autorange=False),
        title="End-effector trajectory",
        updatemenus=[dict(
            type="buttons",
            buttons=[dict(label="Play",
                          method="animate",
                          args=[None])])]
    ),
    frames=[go.Frame(data=[go.Scatter(x=[0, xj_], y=[0, yj_]),
                          go.Scatter(x=[xj_, xe_], y=[yj_, ye_])]) 
            for xj_, yj_, xe_, ye_ in np.hstack((joint_trajectory, endeff_trajectory))]
)

fig.show()
```


```python
?px.scatter_3d
```


```python

```


```python

```

53258ad5c6e639e22de9d7ce898d4068d0c6d577
ipynb
Jupyter Notebook
cinematica-directa-2D/robot_2dof_sm.ipynb
robotica-cem/cinematica-notebooks
34d3ea26844dabc80fce770f85360d0b8e4ff332
BSD-2-Clause
cinematica-directa-2D/robot_2dof_sm.ipynb
robotica-cem/cinematica-notebooks
34d3ea26844dabc80fce770f85360d0b8e4ff332
BSD-2-Clause
cinematica-directa-2D/robot_2dof_sm.ipynb
robotica-cem/cinematica-notebooks
34d3ea26844dabc80fce770f85360d0b8e4ff332
BSD-2-Clause
Qwen/Qwen-72B
1. YES
2. YES