<jupyter_start><jupyter_text>Forward kinematics of manipulator arms  Example: Scara manipulatorWe consider a simplified Scara manipulator. The robot consists of a base and three links. The two first degrees of freedom are rotatins about vertical axes, and the last degree of freedom is a prismatic joint that determines the height of the tool-tip. In the full version of the Scara manipulator there is an additional rotational degree of freedom with axis coinciding with the prismatic joint.   The kinematics of the first two degrees of freedom are equivalent to this planar robot:<jupyter_code>import numpy as np
import doctest
import spatialmath as sm
def fk_scara(th, l0=1, l1=1, l2=1):
    '''
    Forward kinematics map of the scara robot
    
    Arguments
    ---------
    th : array-like of length 3
       The two joint angles and the displacement of the final prismatic joint
    l0 : float
       Height of the base
    l1, l2 : float
       Lengths of link 1 and 2, respectively
    
    Returns
    -------
    tpc : numpy array of length 3
       The position of the tool point center.
       
    Tests
    -----
    
    1) Tool tip at default position
    >>> tpc = fk_scara((0, 0, 0))
    >>> "(%0.2f, %0.2f, %0.2f)" %tuple(tpc)
    '(2.00, 0.00, 1.00)'
    
    2) Tool tip at 90 degrees in first two joints and 0.1 in last
    >>> tpc = fk_scara((np.pi/2, np.pi/2, 0.1))
    >>> "(%0.2f, %0.2f, %0.2f)" %tuple(tpc)
    '(-1.00, 1.00, 0.90)'
    
    4) Tool tip position is always inside a circle of a certain radius
    >>> positions = [fk_scara((th1_, th2_, 0))
    ... 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(p_[0]**2 + p_[1]**2) for p_ in positions])
    >>> max_radius = 2 + 1e-12 # Add a small tolerance
    >>> np.any(distances > max_radius)
    False
    
    '''
    
    # Center and direction of the joints 
    w = np.array([0,0,1]) # Same for all
    q1 = np.array([0,0,l0])
    q2 = np.array([l1,0,l0])
    v1 = -np.cross(w, q1)
    v2 = -np.cross(w, q2)
    
    # The twists
    xi1 = sm.Twist3(v1, w)
    xi2 = sm.Twist3(v2, w)
    xi3 = sm.Twist3(-w, [0,0,0])

    g_st = sm.SE3( xi1.exp(th[0]) @ xi2.exp(th[1]) @ xi3.exp(th[2]) )
    
    # Initial position of the tool tip
    tpc0 = [l1+l2, 0, l0]
    
    return g_st*tpc0
    
    
# Doctests for forward kinematics functions
doctest.run_docstring_examples(fk_scara, globals())<jupyter_output><empty_output><jupyter_text>The Stanford armThe stanford manipulator arm is a 6dof arm with two revolute joint, followed by a prismatic joint and finally a wrist with 3dof.The kinematics of the first two degrees of freedom are equivalent to this planar robot:Image from Murray, <NAME>stry "A Mathematical Introduction to Robotic Manipulation"The tool center point is assumed to be at a length $l_2$ away from the wrist joint center along the final axis of rotation $\xi_6$.The spatial reference frame fixed to the robot base has x-axis pointing toward the viewer, y-axis towards the right, and z-axis pointing vertically.<jupyter_code>def fk_stanford(th, l0=1, l1=1, l2=0.3):
    '''
    Forward kinematics map of the stanford manipulator arm
    
    Arguments
    ---------
    th : array-like of length 6
       The joint angles
    l0 : float
       Height of the base
    l1: float
       Length of link 2
    l2: float
       Distance of tool center point from wrist center
    
    
    Returns
    -------
    tcp : numpy array of length 3
       The position of the tool center point.
    R_st   : numpy array (3, 3)
       Rotation matrix giving the orientation of the tool frame with respect to the
       spatial frame (robot base)
    
    Tests
    -----
    
    1) Tool tip at default position
    >>> tcp, R_st = fk_stanford((0, 0, 0, 0, 0, 0))
    >>> "(%0.2f, %0.2f, %0.2f)" %tuple(tcp)
    '(0.00, 1.30, 1.00)'
    
    2) Tool tip at 90 degrees in first and fourth joints
    >>> tcp, R_st = fk_stanford((np.pi/2, 0, 0, np.pi/2, 0, 0))
    >>> "(%0.2f, %0.2f, %0.2f)" %tuple(tcp)
    '(-1.00, -0.30, 1.00)'
    
    3) Orientation of gripper at -90 degrees in second and fifth joints
    >>> tcp, R_st = fk_stanford((0, -np.pi/2, 0,  0, -np.pi/2, 0))
    >>> R_expected = [[1, 0, 0],[0, -1, 0], [0, 0, -1]]
    >>> np.allclose(R_st, R_expected)
    True
    
    '''
    
    # Define all the twists xi1, xi2, ..., xi6
    
    # Then the product of exponential formula gives
    g_st = sm.SE3( xi1.exp(th[0]) @ xi2.exp(th[1]) @ xi3.exp(th[2]) \
                 @ xi4.exp(th[3]) @ xi5.exp(th[4]) @ xi6.exp(th[5]) )

    # Initial position of the tool tip
    # tcp0 = 
    
    return (g_st*tcp0, g_st.R)
    
     
doctest.run_docstring_examples(fk_stanford, globals(), verbose=False)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.12328482328482329, 'fraction_numerical': 0.045322245322245325, 'mean_word_length': 2.5193855157278713, 'pattern_counts': {'":': 0, '<': 10, '<?xml version=': 0, '>': 56, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '2', 'dir': 'jupyter-structured-clean-dedup', 'id': '512295', 'license': 'BSD-2-Clause', 'n_tokens_mistral': 1820, 'n_tokens_neox': 1702, 'n_words': 589, 'path': 'cinematica-directa-3D/fwd-kinematics-scara-and-stanford.ipynb', 'repo_name': 'robotica-cem/cinematica-notebooks', 'size': '4810'}