text: ## Operational space control of 6DOF robot arm with spiking cameras part 2: Deriving the Jacobian
  
  In the previous exciting post in this series I outlined the project, which is in the title, and we worked through getting access to the arm through Python. The next step was deriving the Jacobian, and that’s what we’re going to be talking about in this post!
  
  Alright.
  This was a time I was very glad to have a previous post talking about generating transformation matrices, because deriving the Jacobian for a 6DOF arm in 3D space comes off as a little daunting when you’re used to 3DOF in 2D space, and I needed a reminder of the derivation process. The first step here was finding out which motors were what, so I went through and found out how each motor moved with something like the following code:
  
  for ii in range(7):
  target_angles = np.zeros(7, dtype='float32')
  target_angles[ii] = np.pi / 4.0
  rob.move(target_angles)
  time.sleep(1)
  
  
  and I found that the robot is setup in the figures below
  
  this is me trying my hand at making things clearer using Inkscape, hopefully it’s worked. Displayed are the first 6 joints and their angles of rotation, $q_0$ through $q_5$. The 7th joint, $q_6$, opens and closes the gripper, so we’re safe to ignore it in deriving our Jacobian. The arm segment lengths $l_1, l_3,$ and $l_5$ are named based on the nearest joint angles (makes easier reading in the Jacobian derivation).
  
  Find the transformation matrix from end-effector to origin
  
  So first thing’s first, let’s find the transformation matrices. Our first joint, $q_0$, rotates around the $z$ axis, so the rotational part of our transformation matrix $^0_\textrm{org}\textbf{T}$ is
  
  $^0_\textrm{org}\textbf{R} = \left[ \begin{array}{ccc} \textrm{cos}(q_0) & -\textrm{sin}(q_0) & 0 \\ \textrm{sin}(q_0) & \textrm{cos}(q_0) & 0 \\ 0 & 0 & 1 \end{array} \right],$
  
  and $q_0$ and our origin frame of reference are on top of each other so we don’t need to account for translation, so our translation component of $^0_\textrm{org}\textbf{T}$ is
  
  $^0_\textrm{org}\textbf{D} = \left[ \begin{array}{c} 0 \\ 0 \\ 0 \end{array} \right].$
  
  Stacking these together to form our first transformation matrix we have
  
  $^0_\textrm{org}\textbf{T} = \left[ \begin{array}{cc} ^0_\textrm{org}\textbf{R} & ^0_\textrm{org}\textbf{D} \\ 0 & 1 \end{array} \right] = \left[ \begin{array}{cccc} \textrm{cos}(q_0) & -\textrm{sin}(q_0) & 0 & 0\\ \textrm{sin}(q_0) & \textrm{cos}(q_0) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  So now we are able to convert a position in 3D space from to the reference frame of joint $q_0$ back to our origin frame of reference. Let’s keep going.
  
  Joint $q_1$ rotates around the $x$ axis, and there is a translation along the arm segment $l_1$. Our transformation matrix looks like
  
  $^1_0\textbf{T} = \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \textrm{cos}(q_1) & -\textrm{sin}(q_1) & l_1 \textrm{cos}(q_1) \\ 0 & \textrm{sin}(q_1) & \textrm{cos}(q_1) & l_1 \textrm{sin}(q_1) \\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  Joint $q_2$ also rotates around the $x$ axis, but there is no translation from $q_2$ to $q_3$. So our transformation matrix looks like
  
  $^2_1\textbf{T} = \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \textrm{cos}(q_2) & -\textrm{sin}(q_2) & 0 \\ 0 & \textrm{sin}(q_2) & \textrm{cos}(q_2) & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  The next transformation matrix is a little tricky, because you might be tempted to say that it’s rotating around the $z$ axis, but actually it’s rotating around the $y$ axis. This is determined by where $q_3$ is mounted relative to $q_2$. If it was mounted at 90 degrees from $q_2$ then it would be rotating around the $z$ axis, but it’s not. For translation, there’s a translation along the $y$ axis up to the next joint, so all in all the transformation matrix looks like:
  
  $^3_2\textbf{T} = \left[ \begin{array}{cccc} \textrm{cos}(q_3) & 0 & -\textrm{sin}(q_3) & 0\\ 0 & 1 & 0 & l_3 \\ \textrm{sin}(q_3) & 0 & \textrm{cos}(q_3) & 0\\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  And then the transformation matrices for coming from $q_4$ to $q_3$ and $q_5$ to $q_4$ are the same as the previous set, so we have
  
  $^4_3\textbf{T} = \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \textrm{cos}(q_4) & -\textrm{sin}(q_4) & 0 \\ 0 & \textrm{sin}(q_4) & \textrm{cos}(q_4) & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  and
  
  $^5_4\textbf{T} = \left[ \begin{array}{cccc} \textrm{cos}(q_5) & 0 & -\textrm{sin}(q_5) & 0 \\ 0 & 1 & 0 & l_5 \\ \textrm{sin}(q_5) & 0 & \textrm{cos}(q_5) & 0\\ 0 & 0 & 0 & 1 \end{array} \right] .$
  
  Alright! Now that we have all of the transformation matrices, we can put them together to get the transformation from end-effector coordinates to our reference frame coordinates!
  
  $^\textrm{ee}_\textrm{org}\textbf{T} = ^0_\textrm{org}\textbf{T} \; ^1_0\textbf{T} \; ^2_1\textbf{T} \; ^3_2\textbf{T} \; ^4_3\textbf{T} \; ^5_4\textbf{T}.$
  
  At this point I went and tested this with some sample points to make sure that everything seemed to be being transformed properly, but we won’t go through that here.
  
  Calculate the derivative of the transform with respect to each joint
  
  The next step in calculating the Jacobian is getting the derivative of $^\textrm{ee}_\textrm{org}T$. This could be a big ol’ headache to do it by hand, OR we could use SymPy, the symbolic computation package for Python. Which is exactly what we’ll do. So after a quick
  
  sudo pip install sympy
  
  
  I wrote up the following script to perform the derivation for us
  
  import sympy as sp
  
  def calc_transform():
  # set up our joint angle symbols (6th angle doesn't affect any kinematics)
  q = [sp.Symbol('q0'), sp.Symbol('q1'), sp.Symbol('q2'), sp.Symbol('q3'),
  sp.Symbol('q4'), sp.Symbol('q5')]
  # set up our arm segment length symbols
  l1 = sp.Symbol('l1')
  l3 = sp.Symbol('l3')
  l5 = sp.Symbol('l5')
  
  Torg0 = sp.Matrix([[sp.cos(q[0]), -sp.sin(q[0]), 0, 0,],
  [sp.sin(q[0]), sp.cos(q[0]), 0, 0],
  [0, 0, 1, 0],
  [0, 0, 0, 1]])
  
  T01 = sp.Matrix([[1, 0, 0, 0],
  [0, sp.cos(q[1]), -sp.sin(q[1]), l1*sp.cos(q[1])],
  [0, sp.sin(q[1]), sp.cos(q[1]), l1*sp.sin(q[1])],
  [0, 0, 0, 1]])
  
  T12 = sp.Matrix([[1, 0, 0, 0],
  [0, sp.cos(q[2]), -sp.sin(q[2]), 0],
  [0, sp.sin(q[2]), sp.cos(q[2]), 0],
  [0, 0, 0, 1]])
  
  T23 = sp.Matrix([[sp.cos(q[3]), 0, sp.sin(q[3]), 0],
  [0, 1, 0, l3],
  [-sp.sin(q[3]), 0, sp.cos(q[3]), 0],
  [0, 0, 0, 1]])
  
  T34 = sp.Matrix([[1, 0, 0, 0],
  [0, sp.cos(q[4]), -sp.sin(q[4]), 0],
  [0, sp.sin(q[4]), sp.cos(q[4]), 0],
  [0, 0, 0, 1]])
  
  T45 = sp.Matrix([[sp.cos(q[5]), 0, sp.sin(q[5]), 0],
  [0, 1, 0, l5],
  [-sp.sin(q[5]), 0, sp.cos(q[5]), 0],
  [0, 0, 0, 1]])
  
  T = Torg0 * T01 * T12 * T23 * T34 * T45
  
  # position of the end-effector relative to joint axes 6 (right at the origin)
  x = sp.Matrix([0,0,0,1])
  
  Tx = T * x
  
  for ii in range(6):
  print q[ii]
  print sp.simplify(Tx[0].diff(q[ii]))
  print sp.simplify(Tx[1].diff(q[ii]))
  print sp.simplify(Tx[2].diff(q[ii]))
  
  
  And then consolidated the output using some variable shorthand to write a function that accepts in joint angles and generates the Jacobian:
  
  def calc_jacobian(q):
  J = np.zeros((3, 7))
  
  c0 = np.cos(q[0])
  s0 = np.sin(q[0])
  c1 = np.cos(q[1])
  s1 = np.sin(q[1])
  c3 = np.cos(q[3])
  s3 = np.sin(q[3])
  c4 = np.cos(q[4])
  s4 = np.sin(q[4])
  
  c12 = np.cos(q[1] + q[2])
  s12 = np.sin(q[1] + q[2])
  
  l1 = self.l1
  l3 = self.l3
  l5 = self.l5
  
  J[0,0] = -l1*c0*c1 - l3*c0*c12 - l5*((s0*s3 - s12*c0*c3)*s4 + c0*c4*c12)
  J[1,0] = -l1*s0*c1 - l3*s0*c12 + l5*((s0*s12*c3 + s3*c0)*s4 - s0*c4*c12)
  J[2,0] = 0
  
  J[0,1] = (l1*s1 + l3*s12 + l5*(s4*c3*c12 + s12*c4))*s0
  J[1,1] = -(l1*s1 + l3*s12 + l5*s4*c3*c12 + l5*s12*c4)*c0
  J[2,1] = l1*c1 + l3*c12 - l5*(s4*s12*c3 - c4*c12)
  
  J[0,2] = (l3*s12 + l5*(s4*c3*c12 + s12*c4))*s0
  J[1,2] = -(l3*s12 + l5*s4*c3*c12 + l5*s12*c4)*c0
  J[2,2] = l3*c12 - l5*(s4*s12*c3 - c4*c12)
  
  J[0,3] = -l5*(s0*s3*s12 - c0*c3)*s4
  J[1,3] = l5*(s0*c3 + s3*s12*c0)*s4
  J[2,3] = -l5*s3*s4*c12
  
  J[0,4] = l5*((s0*s12*c3 + s3*c0)*c4 + s0*s4*c12)
  J[1,4] = l5*((s0*s3 - s12*c0*c3)*c4 - s4*c0*c12)
  J[2,4] = -l5*(s4*s12 - c3*c4*c12)
  
  return J
  
  
  Alright! Now we have our Jacobian! Really the only time consuming part here was calculating our end-effector to origin transformation matrix, generating the Jacobian was super easy using SymPy once we had that.
  
  Hack position control using the Jacobian
  
  Great! So now that we have our Jacobian we’ll be able to translate forces that we want to apply to the end-effector into joint torques that we want to apply to the arm motors. Since we can’t control applied force to the motors though, and have to pass in desired angle positions, we’re going to do a hack approximation. Let’s first transform our forces from end-effector space into a set of joint angle torques:
  
  $\textbf{u} = \textbf{J}^T \; \textbf{u}_\textbf{x}.$
  
  To approximate the control then we’re simply going to take the current set of joint angles (which we know because it’s whatever angles we last told the system to move to) and add a scaled down version of $\textbf{u}$ to approximate applying torque that affects acceleration and then velocity.
  
  $\textbf{q}_\textrm{des} = \textbf{q} + \alpha \; \textbf{u},$
  
  where $\alpha$ is the gain term, I used .001 here because it was nice and slow, so no crazy commands that could break the servos would be sent out before I could react and hit the cancel button.
  
  What we want to do then to implement operational space control here then is find the current $(x,y,z)$ position of the end-effector, calculate the difference between it and the target end-effector position, use that to generate the end-effector control signal $u_x$, get the Jacobian for the current state of the arm using the function above, find the set of joint torques to apply, approximate this control by generating a set of target joint angles to move to, and then repeat this whole loop until we’re within some threshold of the target position. Whew.
  
  So, a lot of steps, but pretty straight forward to implement. The method I wrote to do it looks something like:
  
  def move_to_xyz(self, xyz_d):
  """
  np.array xyz_d: 3D target (x_d, y_d, z_d)
  """
  count = 0
  while (1):
  count += 1
  # get control signal in 3D space
  xyz = self.calc_xyz()
  delta_xyz = xyz_d - xyz
  ux = self.kp * delta_xyz
  
  # transform to joint space
  J = self.calc_jacobian()
  u = np.dot(J.T, ux)
  
  # target joint angles are current + uq (scaled)
  self.q[...] += u * .001
  self.robot.move(np.asarray(self.q.copy(), 'float32'))
  
  if np.sqrt(np.sum(delta_xyz**2)) < .1 or count > 1e4:
  break
  
  
  And that is it! We have successfully hacked together a system that can perform operational space control of a 6DOF robot arm. Here is a very choppy video of it moving around to some target points in a grid on a cube.
  
  So, granted I had to drop a lot of frames from the video to bring it’s size down to something close to reasonable, but still you can see that it moves to target locations super fast!
  
  Alright this is sweet, but we’re not done yet. We don’t want to have to tell the arm where to move ourselves. Instead we’d like the robot to perform target tracking for some target LED we’re moving around, because that’s way more fun and interactive. To do this, we’re going to use spiking cameras! So stay tuned, we’ll talk about what the hell spiking cameras are and how to use them for a super quick-to-setup and foolproof target tracking system in the next exciting post!
  
  ## Robot control part 6: Handling singularities
  
  We’re back! Another exciting post about robotic control theory, but don’t worry, it’s short and ends with simulation code. The subject of today’s post is handling singularities.
  
  What is a singularity
  
  This came up recently when I had build this beautiful controller for a simple two link arm that would occasionally go nuts. After looking at it for a while it became obvious this was happening whenever the elbow angle reached or got close to 0 or $\pi$. Here’s an animation:
  
  What’s going on here? Here’s what. The Jacobian has dropped rank and become singular (i.e. non-invertible), and when we try to calculate our mass matrix for operational space
  
  $\textbf{M}_\textbf{x}(\textbf{q}) = (\textbf{J} (\textbf{q}) \; \textbf{M}^{-1} (\textbf{q}) \; \textbf{J}^T(\textbf{q}))^{-1},$
  
  the values explode in the inverse calculation. Dropping rank means that the rows of the Jacobian are no longer linearly independent, which means that the matrix can be rotated such that it gives a matrix with a row of zeros. This row of zeros is the degenerate direction, and the problems come from trying to send forces in that direction.
  
  To determine when the Jacobian becomes singular its determinant can be examined; if the determinant of the matrix is zero, then it is singular. Looking the Jacobian for the end-effector:
  
  $\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & -L_1 sin(q_0 + q_1) \\ L_0 cos(q_0) + L_1 cos(q_0 + q_1) & L_1 cos(q_0 + q_1) \end{array} \right].$
  
  When $q_1 = 0$ it can be that $sin(q_0 + 0) = sin(q_0)$, so the Jacobian becomes
  
  $\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} (L_0 + L_1)(-sin(q_0)) & -L_1 sin(q_0) \\ (L_0 + L_1) cos(q_0) & L_1 cos(q_0) \end{array} \right],$
  
  which gives a determinant of
  
  $(L_0 + L_1)(-sin(q_0))(L_1)(cos(q_0)) - (L_1)(-sin(q_0))(L_0 + L_1)(cos(q_0)) = 0.$
  
  Similarly, when $q_1 = \pi$, where $sin(q_0 + \pi) = -sin(q_0)$ and $cos(q_0 + \pi) = -cos(q_0)$, the Jacobian is
  
  $\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} -(L_0 - L_1) sin(q_0) & L_1 sin(q_0) \\ (L_0 + L_1) cos(q_0) & - L_1 cos(q_0) \end{array} \right].$
  
  Calculating the determinant of this we get
  
  $(L_0 + L_1)(-sin(q_0))(L_1)(-cos(q_0)) - (L_1)(sin(q_0))(L_0 + L_1)(-cos(q_0)) = 0.$
  
  Note that while in these cases the Jacobian is a square matrix in the event that it is not a square matrix, the determinant of $\textbf{J}(\textbf{q})\;\textbf{J}^T(\textbf{q})$ can be found instead.
  
  Fixing the problem
  
  When a singularity is occurring it can be detected, but now it must be handled such that the controller behaves appropriately. This can be done by identifying the degenerate dimensions and setting the force in those directions to zero.
  
  First the SVD decomposition of $\textbf{M}_\textbf{x}^{-1}(\textbf{q}) = \textbf{V}\textbf{S}\textbf{U}^T$ is found. To get the inverse of this matrix (i.e. to find $\textbf{M}_\textbf{x}(\textbf{q})$) from the returned $\textbf{V}, \textbf{S}$ and $\textbf{U}$ matrices is a matter of inverting the matrix $\textbf{S}$:
  
  $\textbf{M}_\textbf{x}(\textbf{q}) = \textbf{V} \textbf{S}^{-1} \textbf{U}^T,$
  
  where $\textbf{S}$ is a diagonal matrix of singular values.
  
  Because $\textbf{S}$ is diagonal it is very easy to find its inverse, which is calculated by taking the reciprocal of each of the diagonal elements.
  
  Whenever the system approaches a singularity some of the values of $\textbf{S}$ will start to get very small, and when we take the reciprocal of them we start getting huge numbers, which is where the value explosion comes from. Instead of allowing this to happen, a check for approaching the singularity can be implemented, which then sets the singular values entries smaller than the threshold equal to zero, canceling out any forces that would be sent in that direction.
  
  Here’s the code:
  
  Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
  if abs(np.linalg.det(np.dot(JEE,JEE.T))) > .005**2:
  # if we're not near a singularity
  Mx = np.linalg.inv(Mx_inv)
  else:
  # in the case that the robot is entering near singularity
  u,s,v = np.linalg.svd(Mx_inv)
  for i in range(len(s)):
  if s[i] < .005: s[i] = 0
  else: s[i] = 1.0/float(s[i])
  Mx = np.dot(v, np.dot(np.diag(s), u.T))
  
  
  And here’s an animation of the controlled arm now that we’ve accounted for movement when near singular configurations:
  
  As always, the code for this can be found up on my Github. The default is to run using a two link arm simulator written in Python. To run, simply download everything and run the run_this.py file.
  
  Everything is also included required to run the MapleSim arm simulator. To do this, go into the TwoLinkArm folder, and run python setup.py build_ext -i. This should compile the arm simulation to a shared object library that Python can now access on your system. To use it, edit the run_this.py file to import from TwoLinkArm/arm_python to TwoLinkArm/arm and you should be good to go!
  More details on getting the MapleSim arm to run can be found in this post.
metadata: {"found_math": true, "script_math_tex": 0, "script_math_asciimath": 0, "math_annotations": 0, "math_alttext": 0, "mathml": 0, "mathjax_tag": 0, "mathjax_inline_tex": 0, "mathjax_display_tex": 0, "mathjax_asciimath": 1, "img_math": 67, "codecogs_latex": 0, "wp_latex": 0, "mimetex.cgi": 0, "/images/math/codecogs": 0, "mathtex.cgi": 0, "katex": 0, "math-container": 0, "wp-katex-eq": 0, "align": 0, "equation": 0, "x-ck12": 0, "texerror": 0}