# 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