# How to implement end effector orientation in generic IK solver

I am trying to script a basic, lightweight IK solver with a Jacobian Transpose method. I have gotten a basic script that reach the desired position very quickly. What I am not able to do is achieve a desired orientation as well. I want to avoid the spherical wrist model in order to test a wide variety of robot configurations, so I need to include orientation as a general kinematic parameter. I haven't taken linear algebra in 14 years and my matrix math is VERY elementary, so ideally, I would love some help finding a more geometrical approach to finding the rotational values for the various axes.

I tried to do it by having three points on a target plane that correspond to points on the end effector thich through iterative calculation approach a solution. However this would get stuck when the rotation was such that the error vectors of the three points added up to less than my convergence threshhold. The working code is here:

        public List<double> axisDelta(List<Line> axesTemp, Plane eePl, Plane target, double step)
{
Point3d targetPoint = target.Origin;
Point3d eePoint = eePl.Origin;
Vector3d error = targetPoint - eePoint;
List<double> deltaTemp = new List<double>();

for (int i = 0; i < axesTemp.Count; i++)
{
Vector3d jointToEE = eePoint - axesTemp[i].ClosestPoint(eePoint, false);
Vector3d jakobian = Vector3d.CrossProduct(axesTemp[i].Direction, jointToEndCP);

jakobian.Unitize();
double dot = Vector3d.Multiply(error, jakobian);

double theta = 0;
if (jointToEndCP.Length > 0) theta = Math.Atan(dot / jointToEndCP.Length); //avoid dividing by 0

deltaTemp.Add(theta * step);
}
return deltaTemp;
}


Please help me understand the best way to implement orientation into the script to find the axis rotation values to align the End-Effector plane with the target plane. Thanks!!

• This paper provides a step-by-step approach to a geometric solution for the 6-DOF IK problem. – MindS1 Dec 4 '18 at 15:55
• Thanks for the response, MindS1. That paper unfortunately is using a robot with a spherical wrist, and is able to take advantage of kinematic decoupling, where the effector orientation can be changed with axes 4-6, independently of joints 1-3. I'm looking for a strategy that doesn't rely on that for orientation. – Oliver Appling Bucklin Dec 4 '18 at 16:49