<jupyter_start><jupyter_text>**Introduction**--- **TA Instructor**Name: <NAME>Feel free to contact me in person (office 105) or via [telegram](https://t.me/simkasimka) and [mail](https://t.me/simkasimka) if you face any problems with the course or would like to do research and work on hardware **Prerequisites for practice**There are no strong prerequisites for the practice class, however:* **Linear Algebra**, **Linear Control Theory** as well as **Dynamics** (Mechanics, Physics) and **Calculus** course will be really helpfull* As before we will use a bit of **Python** and maybe **C++** in this class* We will often use [mathematical quantifiers](https://en.wikipedia.org/wiki/Quantifier_(logic)). Revise them by going over [exercises](https://www.whitman.edu/mathematics/higher_math_online/section01.02.html) if you forgot some. **Notation**We will use the following notation in "notebooks":> A key points or equations looks like this >**QUESTION**: These questions are related to lecture materials, answering them will help you to develop solid understanding of the course.>**HW EXERCISE**: This is home assignment which I **strongly** recommend to complete right  after the class. You have to create a `*yourname_№ofclass.ipynb*` and share it with me through [Colab](https://colab.research.google.com/).>**BONUS:** Problems that will be graded separately and will give you bonus points on final exam  **Practice 1: Fundamental Subspaces and Jacobian Mapping** **Goals for today**---During today practice we will:*   Exploit a structure of linear mapping between Joint and Task spaces velocities.*   Understand what is physical interpretation of fundamental subspaces of the manipulator Jacobian.*   Obtain null space motion of 3 DoF plane manipulator with fixed end effector  **Four Fundamental Subspaces. Recall**--->As we have studied on the lectures there are four fundamental subspaces accompanying any linear operator (matrix) $\mathbf{A}^{m \times n}$, namely:>*   **Column** space (range, image): $\mathcal{C}(\mathbf{A}) \in \mathbb{R}^m$ >*   **Null** space (kernel): $\mathcal{N}(\mathbf{A}) \in \mathbb{R}^n$>*   **Row** space: $\mathcal{R}(\mathbf{A}) = \mathcal{C}(\mathbf{A}^T)  \in \mathbb{R}^n$>*   **Left null** space: $\mathcal{N}(\mathbf{A}^T) \in \mathbb{R}^m$--->**QUESTION**: Why the null space $\mathcal{N}(\mathbf{A}^T)$ is called left? Denoted above subspaces are widely used in various applications, today we are going to apply them to exploit an inner structure of kinematics and statics of redundant manipulators.  **Jacobian Velocity Mapping**>Let us consider differential kinematic relationship:\begin{equation}  \dot{\boldsymbol{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\end{equation}where*   $\boldsymbol{x} \in \mathbb{R}^m$ task space variables (for instance Cartesian coordinates)*   $\mathbf{q} \in \mathbb{R}^n$ joint space variables (positions of joints)*   $\mathbf{J} \in \mathbb{R}^{m\times n}$ mainpulator Jacobian As one can note relationship between joint and task velocity is linear (for particular $\mathbf{q}$) thus, the differential kinematics equation can be characterized in terms of the columns space $\mathcal{C}(\mathbf{J})$ (range) and null space $\mathcal{N}(\mathbf{J})$ of the mapping $\mathbf{J}$*   The column space of $\mathbf{J}$ is the subspace $\mathcal{C}(\mathbf{J}) \in \mathbb{R}^m$ of the end-effector velocities $\dot{\mathbf{x}}$ that can be generated by the joint velocities $\dot{\mathbf{q}}$, in the given manipulatorposture $\mathbf{q}$*   The null space of $\mathbf{J}$ is the subspace $\mathcal{N}(\mathbf{J}) \in \mathbb{R}^n$ of joint velocities $\dot{\mathbf{q}}$ that do not produce any end-effector velocity $\dot{\mathbf{x}}$, in the given manipulator posture $\mathbf{q}$.If the Jacobian has full rank, one has $\text{dim}\{\mathcal{C}(\mathbf{J})\} = m,\text{dim}\{\mathcal{N}(\mathbf{J})\} = n - m $. In case of not reduntant manipulator $m = n$ H dimension of null space increases only in the singular configurations. However, in the reduntant case condition  $\mathcal{N}(\mathbf{J}) \neq \emptyset$ may be satisfied not only in singular configurations, which allow determination of systematic techniques to handle redundant DOFs.<!-- For convenience let us define **null space projector** $\mathbf{P}\in \mathbb{R}^{m \times m}$:\begin{equation}\mathcal{C}(\mathbf{P}) \triangleq \mathcal{N}(\mathbf{J})\end{equation}Thus, if one denote a joint velocity vector $\dot{\mathbf{q}}^*$ that produce a desired velocity $\dot{\boldsymbol{x}}_d$ the following holds:\begin{equation}\dot{\boldsymbol{x}}_d = \mathbf{J}\dot{\mathbf{q}}^* + \mathbf{P}\dot{\mathbf{q}}_0\end{equation}for any $\dot{\mathbf{q}}_0 \in \mathbb{R}^n$>**HW EXERCISE**: Proof the statement aboveLet us now consider an practical application of the Jacobian null-space.  -->  **Case Study: Reduntand Manipulation**Consider the 3-DoF planar manipulator:A problem is to find the motion $\mathbf{q}^*(t),\mathbf{\dot{q}}^*(t)$ of the manipulator from the initial pose $\mathbf{q}_0$ such that end effector remain in the rest (as shown [here](https://youtu.be/sZYBC8Lrmdo?t=11)).To do so we are going to find joint velocity vector that is in null-space of Jacobian and integrate it from initial state to obtain a desired joint angles.A first step is to find forward kinematics:\begin{equation}\boldsymbol{x} = \sum^n_{i=1}\ell_i\begin{bmatrix} \cos \sum^i_{j=1}q_j\\ \sin \sum^i_{j=1}q_j\end{bmatrix}  \end{equation}Thus the $i$-th column of Jacobian calculated recursevely as:\begin{equation}\mathbf{J}_i =\frac{\partial \boldsymbol{x}}{\partial q_i} =\sum^n_{j=i}\ell_j\begin{bmatrix} -\sin \sum^j_{k=1} q_k\\ \cos \sum^j_{k=1} q_k\end{bmatrix}  \end{equation} Let's implement Jacobian matrix:<jupyter_code>from numpy import cos, sin, zeros, sum, pi, dot, array

def jacobian_column(q, l, i):
    jac_i = zeros(2)
    for j in range(i,len(q)):
      jac_i+=l[j]*array([-sin(sum(q[:j+1])), cos(sum(q[:j+1]))])
    return jac_i
  
def jacobian(q, l):
    jac = zeros((2,len(q)))
    for i in range(len(q)):
      jac[:,i] = jacobian_column(q, l, i)
    return jac<jupyter_output><empty_output><jupyter_text>To find a desired joint speeds that do not produce any motion on the end-effector one can solve:\begin{equation}\mathbf{J}\dot{\mathbf{q}}_0 = \dot{\boldsymbol{x}}_d = \mathbf{0}\end{equation}which is nothing but $\mathcal{N}(\mathbf{J})$Let us now find the basis of null space for particular initial pose:<jupyter_code>q0 = [pi/2, -pi/4, 0]
l = [0.5, 0.5, 0.5]

from scipy.linalg import null_space
null_dq = null_space(jacobian(q0,l))
print('Basis of null space:\n', null_dq)<jupyter_output>Basis of null space:
 [[ 2.54873123e-17]
 [-4.47213595e-01]
 [ 8.94427191e-01]]<jupyter_text>One can multiply obtained basis on any particular value or function and use naive integration to calculate joint angles from "null space speed", for instance let us consider periodic motion in joint angles:<jupyter_code>from numpy import linspace, shape
from scipy.integrate import odeint

def null_vel(q, l, v):
    null_dq = null_space(jacobian(q,l))*v
    return null_dq

def sysode(pos, t, l):
    v = 3*sin(6*t)
    vel = null_vel(pos, l, v)
    return vel[:,0]

# Integration
t0 = 0 # Initial time 
tf = 2 # Final time
N = 1E2 # Numbers of points in time span
t = linspace(t0, tf, int(N)) # Create time span

q_sol = odeint(sysode, q0, t, args=(l,))

# Plotting
from matplotlib.pyplot import *
colors = ('r','g','b')
for i in range(len(q0)):
  plot(t, q_sol[:,i], label = r'$q_' + str(i+1) + '$', color = colors[i])
legend(loc = 'upper right')
xlim([0, tf])
grid(color='black', linestyle='--', linewidth=1.0, alpha = 0.7)
ylabel(r'Joint angles $q$ (rad)')
xlabel(r'Time $t$ (s)')
show()<jupyter_output><empty_output><jupyter_text>Let's check either end effector moved or not, to do so we use solution of forward kinematics:<jupyter_code>from numpy import shape

# Forward kinematics
def fk(q, l):
    x = zeros((shape(q)[0], 2))
    for k in range(shape(q)[0]):
      for i in range(shape(q)[1]):
        x[k, :]+=l[i]*array([cos(sum(q[k,:i+1])), sin(sum(q[k,:i+1]))])
    return x
# Get solution of forward kinematics 
pos = fk(q_sol,l)

colors = ('r','b')
labels = (r'$x$',r'$y$')

for i in range(2):
  plot(t, pos[:,i], label = labels[i], color = colors[i])
  
legend(loc = 'upper right')
xlim([0, tf])
grid(color='black', linestyle='--', linewidth=1.0, alpha = 0.7)
ylabel(r'Joint angles $q$ (rad)')
xlabel(r'Time $t$ (s)')
show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1558319643276226, 'fraction_numerical': 0.011734334663224596, 'mean_word_length': 5.079172610556348, 'pattern_counts': {'":': 1, '<': 18, '<?xml version=': 0, '>': 30, 'https://': 6, 'lorem ipsum': 0, 'www.': 1, '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': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '394408', 'license': 'MIT', 'n_tokens_mistral': 2861, 'n_tokens_neox': 2719, 'n_words': 951, 'path': 'Google Colab notebooks/Practice Fundumental Subspaces 2020 part 1/practice_01_fss_jacobian.ipynb', 'repo_name': 'phoenixfury/Computational-Intelligence-Slides-Spring-2021', 'size': '8532'}