0
$\begingroup$

I'm attempting to create a simulation of a 3 (rotational) DOF robotic arm in 3D space. So far I've had success getting to the Jacobian. What seems to be the issue is that the pseudo inverse of the Jacobian produces illogical values the second time it's calculated. I've reviewed the code and the Jacobian seems to make sense each time it's calculated, and after the second iteration of the pseudo inverse the values make sense so I'm unable to identify the specific cause.

Code provided below.

Simulation example

clc;
clear;
clearvars;

function H = Hg(th,a,r,d) % Creates homogenous transform
  H = [cosd(th) -sind(th)*cosd(a) sind(th)*sind(a) r*cosd(th);
      sind(th) cosd(th)*cosd(a) -cosd(th)*sind(a) r*sind(th);
      0 sind(a) cosd(a) d;
      0 0 0 1];
end

function drawLeg(t, xp, yp, zp, Body, CoxaS, FemurS, TibiaS, EndEfS)
  CoxaV = [CoxaS(1), FemurS(1); CoxaS(2), FemurS(2); CoxaS(3), FemurS(3)];
  FemurV = [FemurS(1), TibiaS(1); FemurS(2), TibiaS(2); FemurS(3), TibiaS(3)];
  TibiaV = [TibiaS(1), EndEfS(1); TibiaS(2), EndEfS(2); TibiaS(3), EndEfS(3)];
  
  
  plot3(xp, yp, zp, 'b.') %draw path
  hold on
  plot3(CoxaV(1,:), CoxaV(2,:), CoxaV(3,:), 'm', 'LineWidth', 3) %draw coxa
  plot3(FemurV(1,:), FemurV(2,:), FemurV(3,:), 'g', 'LineWidth', 3) %draw femur
  plot3(TibiaV(1,:), TibiaV(2,:), TibiaV(3,:), 'r', 'LineWidth', 3) %draw tibia
  axis([Body(1)-5 Body(1)+5 Body(2)-5 Body(2)+7 Body(3)-5 Body(3)+5])
  axis equal
  %body centered graph to make sure it stays in view while walking
  grid on
  hold off
  pause(t)
end

function [Th1, Th2, Th3] = stepLeg (t, Th1, Th2, Th3, x, y, z)
  ### DESIGN ROBOT ###
  % 3DOF articulting arm
  % Start with only z offset for center of robot.
  x00 = 0; 
  y00 = 0;
  z00 = .5;

  % Link lengths
  a0 = 2; %distance from body center to leg 1, UPDATE FROM CAD
  a1 = .5; %length of coxa in cm aka base (joint 0) to joint 1 in z direction
  a2 = 1; %length of femur
  a3 = 1; %length of tibia

  ## DH Parameters ##
  Th0 = 30; %UPDATE FROM CAD
  A0 = 0;
  A1 = 90;
  A2 = 0;
  A3 = 0;
  r0 = a0;
  r1 = 0;
  r2 = a2;
  r3 = a3;
  d0 = 0;
  d1 = a1;
  d2 = 0;
  d3 = 0;
  
  ## Build vectors for each segment ##
  % Each line has an extra element to help the matrix math
  Body = [x00; y00; z00; 1];
  Coxa = [0; a1; 0; 1];
  Femur = [a2; 0; 0; 1];
  Tibia = [a3; 0; 0; 1];

  ## Generate all homogenous transforms ##
  Hb0 = Hg(Th0, A0, r0, d0);
  H01 = Hg(Th1, A1, r1, d1);
  H12 = Hg(Th2, A2, r2, d2);
  H23 = Hg(Th3, A3, r3, d3);
  Hb1 = Hb0*H01;
  Hb2 = Hb0*H01*H12;
  Hb3 = Hb0*H01*H12*H23;

  ## Calculate starting locations for each segment ##
  CoxaS = Hb0*Body;
  FemurS = Hb0*H01*Coxa;
  TibiaS = Hb0*H01*H12*Femur;
  EndEfS = Hb0*H01*H12*H23*Tibia;

  ## Draw the leg ##
  drawLeg(t, x, y, z, Body, CoxaS, FemurS, TibiaS, EndEfS)
  
  ### Now to give a target for the leg ###
  ## Generate Jacobian ##
  # Rotation matrices #
  Rbb = [1, 0, 0; 0, 1, 0; 0, 0, 1];
  Rb0 = Hb0(1:3, 1:3); %use first three rows and first three columns
  Rb1 = Hb1(1:3, 1:3);
  Rb2 = Hb2(1:3, 1:3);

  # Displacement vectors #
  dbb = 0;
  db0 = Hb0(1:3, 4); %use first three rows of fourth column
  db1 = Hb1(1:3, 4);
  db2 = Hb2(1:3, 4);
  db3 = Hb3(1:3, 4);

  # Joint rotation velocity matrices #
  rbb = Rbb*[0;0;1];
  rb0 = Rb0*[0;0;1];
  rb1 = Rb1*[0;0;1];
  rb2 = Rb2*[0;0;1];

  # Create the culumns representing partial diffferentials for each joint #
  %zeroth = [cross(rbb, db3-dbb); rbb] % not needed since it's a fixed joint
  first = [cross(rb0, db3-db0); rb0];
  second = [cross(rb1, db3-db1); rb1];
  third = [cross(rb2, db3-db2); rb2];

  # Concatinate into the jacobian #
  fullJ = [first, second, third];
  % only need the cartesian portion
  J = fullJ(1:3, 1:3)
  Jinv = pinv(J) %Print for troubleshooting
  
  ## Calculate the velocity of the end effector ##
  EndEfS %Print for troubleshooting
  TargetX = x(1) %Print for troubleshooting
  TargetY = y(1) %Print for troubleshooting
  TargetZ = z(1) %Print for troubleshooting
  Err =[x(1); y(1); z(1); 0] - EndEfS %Print for troubleshooting
  fullV = ([x(1); y(1); z(1); 0] - EndEfS)*t;% Calculate velocity of EE
  V = fullV(1:3); % Removing the last element since it's trash info

  # Calc new joint angles #
  w = pinv(J)*V; % Calc joint velocities
  delTh = w.'/t % Calc change in joint angles
  Th0 = Th0; %this should never change?
  Th1 = Th1 + delTh(1);
  Th2 = Th2 + delTh(2);
  Th3 = Th3 + delTh(3);
end


### TIME STEP ###
t = .001

### BUILD PATH ###
% test to determine how piecewise funcitons would work to create path.
% This square looks really nice for now
for i = 1:96
  if i <= 24
    x(i) = i/6;
    y(i) = 3;
    z(i) = 1;
  elseif 24 < i  && i <= 48
    x(i) = x(i-1);
    z(i) = z(i-1)+1/6;
    y(i) = 3;
  elseif 48 < i && i <= 72
    x(i) = x(i-1) - 1/6;
    z(i) = z(i-1);
    y(i) = 3;
  elseif 72 < i && i <= 96
    x(i) = x(i-1);
    z(i) = z(i-1) - 1/6;
    y(i) = 3;
  end
end

### Set Up Initial Angles ###
% Starting angles
Th1 = 60; %UPDATE FROM CAD
Th2 = 0;
Th3 = 0;

### Time Loop ###
for i = 1:10
  [Th1, Th2, Th3] = stepLeg(t, Th1, Th2, Th3, x, y, z)
end
Improve this question
$\endgroup$

1 Answer 1

Reset to default
4
$\begingroup$

After running your code, it looks like everything is behaving how you have defined it to. The problem you are running into has more to do with the Jacobian itself and less to do with any mistakes you have made.

Consider what the Jacobian is: $$ J(\mathbf{q})=\left[\begin{array}{ccc} \dfrac{\partial \mathbf{f}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial \mathbf{f}(\mathbf{q})}{\partial q_{n}} \end{array}\right]=\left[\begin{array}{ccc} \dfrac{\partial f_{1}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial f_{1}(\mathbf{q})}{\partial q_{n}} \\ \vdots & \ddots & \vdots \\ \dfrac{\partial f_{m}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial f_{m}(\mathbf{q})}{\partial q_{n}} \end{array}\right] $$

where $\mathbf{q} \in \mathbb{R}^{n}$ is your current joint configuration and $f_{i}(\mathbf{q})$ is the forward kinematics map of your $i^{th}$ workspace degree of freedom. So, conceptually this is a mapping from joint velocity to end-effector velocity, given the current joint configuration. Now, consider what happens when two joint axes are parallel and there respective vectors to the end-effector are colinear. We have something like: $$J(\mathbf{q}) = \left[\begin{array}{ccc} \mathbf{j}_1 \cdots \mathbf{j}_i \cdots \alpha \mathbf{j}_i \cdots \mathbf{j}_n \end{array} \right]$$ where $\mathbf{j}_i \in \mathbb{R}^{m\times1}$ and $\alpha \ne 0$. Now, let's try to derive some joint angles that would move our end-effector in a desired way. To do so, we need an inverse of our map $\dot{\mathbf{x}} = J(\mathbf{q})\dot{\mathbf{q}}$. In the case that $n = m$, the above case should be a good enough example to demonstrate that taking the inverse of $J(\mathbf{q})$ is not a good idea. Thus, we can take the pseudo-inverse instead - which conveniently generalizes to the case where $n \ne m$. Before we define the pseudo-inverse $J^{+}(\mathbf{q})$, let's take a look at the singular value decomposition of $J(\mathbf{q})$. The SVD theorem states that any matrix (we'll use the Jacobian here) may be decomposed into the following form: $$J(\mathbf{q}) = UDV^{T}$$ where $U \in \mathbb{R}^{m \times m}$ and $V^{T} \in \mathbb{R}^{n \times n}$ are orthonormal matrices, and $D \in \mathbb{R}^{m \times n}$ has non-zero entries only along its diagonal. Intuitively we can think of this equation as decoupling our configuration space from our workspace with some scaling factors that connect the two ($d_{i,i}$ are the only entries in $D$ that may not be zero). Now, going back to the case of two parallel joint axes, we make the intuitive magic-wand-waving jump to the fact that at least one $d_{i,i} = 0$. We can rationalize this with the fact that the end-effector velocity with one of these joints moving will be a scalar multiple of the velocity caused by the other joint moving.

Thus, two dimensions in our configuration space essentially reduce to one in the workspace. This phenomenon is known as a singularity - or a singular configuration. They cause plenty of headache and should be handled with care. Now, let's define the pseudo-inverse of our Jacobian and see what happens around singular configurations.

Without getting into any derivations, the pseudo-inverse of $J(\mathbf{q})$ can be defined as:

$$J^{+}(\mathbf{q}) := \sum_{i}^{r}\frac{1}{d_{i,i}}\mathbf{v}_{i}\mathbf{u}_{i}^{T} $$ where $r$ is the rank of $J(\mathbf{q})$ or, equivalently, the number of non-zero entries in $D$. So, given a singular configuration, we should not see much out of the ordinary. However, consider the configurations in a close proximity to the singular configuration. Remember, $U$ and $V^{T}$ are orthonormal matrices. Thus, as the Jacobian has two almost (but not) linearly dependent column vectors, $U$ and $V^{T}$ will not represent that we are no longer in a singular configuration. Instead, one of the $d_{i,i}$ entries will. This $d_{i,i}$ will be very small as it now represents that we have some newly realized direction we can move in our workspace, even if it is not by much (i.e. the linearly independent Jacobian column vectors return our ability to move in two directions, even if they are almost exactly the same).

Using what is surely abusive notation, we note that at least one $d_{i,i}$ now satisfies the condition that $d_{i,i} \approx 0, d_{i,i} \ne 0$. Thus, when we return to our pseudo-inverse definition, we see why your problems are arising with the pseudo-inverse of the Jacobian: $\frac{1}{d_{i,i}}$ grows arbitrarily large as we approach singular configurations. Of course, this term is never actually infinity as this singular value would not be considered in the psuedo-inverse calculations if it is zero - but an argument could be made that $\lim\limits_{d_{i,i} \to 0} \frac{1}{d_{i,i}} = \infty$ (assuming you subscribe to the limit's school of math). Thus, we find ourselves with an ill-conditioned way of "inverting" our Jacobian.

Luckily, there exists a nice, relatively simple solution to this problem. I won't get into the math on why this works, but the general idea is to convert the end-effector motion problem into a damped least-squares optimization problem. Using the above derivation of the pseudo-inverse, we get the following formula to find a sufficient joint velocity for a requested end-effector velocity: $$\dot{\mathbf{q}} = J^{+}(\mathbf{q})\dot{\mathbf{x}}$$ So, as we approach singular configurations, we will see some $\dot{q}_i \to \infty$. To alleviate this issue, we introduce the damped least-squares solution by defining some $\lambda$ such that $0 \lt \lambda \lt 1$. Now we solve for $\dot{\mathbf{q}}$ as follows:

$$\dot{\mathbf{q}} = J^{T}[JJ^{T} + \lambda^{2}I]^{-1}\dot{\mathbf{x}}$$ where $J = J(\mathbf{q})$ and $I \in \mathbb{R}^{m \times m}$ is the $m \times m$ identity matrix. Scaling lambda will allow you to choose how large joint velocities may be around singular configurations. The basic idea of the adjustment here is to keep our "inversion" technique well-conditioned at all configurations by "damping" the small singular values such that some $\dot{q_i} \to 0$, instead of $\infty$, as we approach singular configurations. Just as an aside, the above formula is optimal for the case where $n \ge m$.

Here are some good sources for the curious:

NOTE: There are more than just this one presented "type" of singular that can occur with revolute joint manipulators. The other cases can be both more restrictive (i.e. consider $\alpha = 0$ in the original case given) and less restrictive. Their effects on the Jacobian vary, the given case was just a sufficient case to demonstrate what was necessary.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.