I have succesfully gotten my inverse kinematics method working using damped least squares and it has some really good results.

My target position also includes a target angle for the end effector, which i calculate in in forward kinematics by adding the angles of the three elevation joints. The problem that occurs though is that when i set an unrealistic target angle it tries to accomplish it before actually getting to the target.

So for instance, if i tell it to go to the a point (for example (2,-2,0) as showing below) with a Z height of zero and also tell it that it needs to come in at an angle of pi/2 (facing +Z direction) it has a ton of trouble because it's not physically possible for it to do so. Animation.

But if i tell it to go to the same point and come in at a more reasonable angle of -pi/3 (facing downward towards -Z with some angle) it does a lot better because thats a physically realizable angle. Animation.

I've tested the same algorithm without the target angle (which changes the jacobian because the target angle is no longer a part of the forward kinematics) and it works great, but i want to be able to have some control over the target angle.

How do i implement logic that tells it to focus on getting to the target position first and THEN attempt to get as close to the target angle as possible without compromising the position?


You have to enforce a prioritization between the two tasks at hand:

  • the primary task is reaching in position;
  • the secondary task is reaching in orientation.

Finally, you can aggregate the two tasks using the null-space projection of the Jacobian.

In formulas: $$ \dot{\theta}=J^\#\cdot\left(x_d-x\left(\theta\right)\right)+\left(I_n-J^\#\cdot J\right)\cdot g, $$ where:

  • $\theta \in \mathbf{R}^n$ is the vector of the $n$ joints;
  • $\dot{\theta} \in \mathbf{R}^n$ is the resulting vector of joint velocities to be sent to the manipulator's actuators;
  • $x\left(\theta\right)$ is the Cartesian position of the tip of the manipulator in the joint configuration given by $\theta$;
  • $x_d$ is the target tip position;
  • $J \in \mathbf{R}^{3\times n}$ is the Jacobian operator for what concerns only the position part of the kinematic map;
  • $J^\# \in \mathbf{R}^{n\times 3}$ is the DLS-inverse of $J$;
  • $I_n-J^\#\cdot J$ is the null-space operator;
  • $g \in \mathbf{R}^n$ is the gradient of a suitable cost function that determines how far we are from the target orientation.

In this setting, the manipulator will be instructed to reach for the target position $x_d$ while employing its redundancy (if there is any) to also comply with the secondary task without disrupting the primary task.

A very similar Q&A can be found here.

Interestingly, we came up with an assignment on this exact IK problem for a course we gave on Robotics, whose solution is given below in form of a patch:

index 7bccd6a..44330c3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -33,6 +33,35 @@ class Controller : public RFModule
     Vector encoders;
     Vector target;

+    // compute the 2D position of the tip of the manipulator
+    Vector forward_kinematics_position(const Vector &j) const
+    {
+        Vector ee(2);
+        ee[0]=link_length*(cos(j[0])+cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        ee[1]=link_length*(sin(j[0])+sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        return ee;
+    }
+    // compute the scalar orientation of the tip of the manipulator
+    double forward_kinematics_orientation(const Vector &j) const
+    {
+        return (j[0]+j[1]+j[2]);
+    }
+    // compute the Jacobian of the tip's position
+    Matrix jacobian_position(const Vector &j) const
+    {
+        Matrix J(2,3);
+        J(0,1)=-link_length*(sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        J(0,2)=-link_length*sin(j[0]+j[1]+j[2]);
+        J(1,1)=link_length*(cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        J(1,2)=link_length*cos(j[0]+j[1]+j[2]);
+        return J;
+    }
     bool configure(ResourceFinder &rf)override
@@ -45,8 +74,9 @@ public:
         // init the encoder values of the 3 joints

-        // init the target
-        target=zeros(3);
+        // init the target at the tip of the manipulator
+        target=forward_kinematics_position(encoders);
+        target.push_back(forward_kinematics_orientation(encoders));

         return true;
@@ -81,10 +111,23 @@ public:
         // retrieve the current target orientation
         double phi_d=target[2];

+        // compute quantities for differential kinematics
+        Vector ee=forward_kinematics_position(encoders);
+        double phi=forward_kinematics_orientation(encoders);
+        Matrix J=jacobian_position(encoders);
+        Vector err=ee_d-ee;
         Vector &vel=portMotors.prepare();
-        vel=zeros(3);

-        // FILL IN THE CODE
+        // resolve the primary task
+        double k1=10.0;
+        Matrix Jsharp=J.transposed()*pinv(J*J.transposed()+k1*k1*eye(2,2));
+        // resolve the secondary task
+        Vector g=2.0*(phi_d-phi)*ones(3);
+        // aggregate tasks using null-space projection
+        vel=Jsharp*err+(eye(3,3)-Jsharp*J)*g;

         // deliver the computed velocities to the actuators

Your Answer

