/// <summary>
/// This returns an array of joint angles from an input XYZ position
/// </summary>
/// <param name="Position"></param>
/// <returns></returns>
protected override double[] getJointAngles(Point3D Position)
{
if(!Initialized)
{
radAngle = new double[N + 1];
radAngle.Initialize();
thetaOffset = new double[N + 1];
thetaOffset[0] = 0;
for (int i = 1; i <= N; i++)
{
thetaOffset[i] = DHparameters[i - 1, 3] * Math.PI / 180;
}
Initialized = true;
}
// create desired position vector
Pd = new Vector3D(Position.X, Position.Y, Position.Z);
// create desired orientation vector
Rd = new Vector3D[3];
Rh = new Vector3D[3];
// declare 3D array for each joint frame axis (xi, yi, zi, Pi)
frame = new Vector3D[N + 1, 4];
frame.Initialize();
// set base frame
frame[0, 0].X = 1;
frame[0, 1].Y = 1;
frame[0, 2].Z = 1;
Vector3D[] Pstar = new Vector3D[N];
Pstar.Initialize();
int link = N;
int tries = 0;
// begin Cyclic Coordinate Descent loop
do
{
// initialize frame positions
for (int i = 0; i < N + 1; i++)
{
frame[i, 3].X = 0;
frame[i, 3].Y = 0;
frame[i, 3].Z = 0;
}
// forward recurrsion formulas for frame position and orientation
for (int i = 1; i < N + 1; i++)
{
// x(i) orientation vector
frame[i, 0] = Vector3D.Add((Vector3D.Multiply(Math.Cos(radAngle[i - 1]), frame[(i - 1), 0])), (Vector3D.Multiply(Math.Sin(radAngle[i - 1]), frame[(i - 1), 1])));
// z(i) orientation vector
frame[i, 2] = Vector3D.Add((Vector3D.Multiply(Math.Cos(DHparameters[i - 1, 0] * Math.PI / 180), frame[(i - 1), 2])), Vector3D.Multiply(Math.Sin(DHparameters[(i - 1), 0] * Math.PI / 180), Vector3D.CrossProduct(frame[i, 0], frame[(i - 1), 2])));
// y(i) orientation vector
frame[i, 1] = Vector3D.CrossProduct(frame[i, 2], frame[i, 0]);
}
// P* --> relative positions of next frame wrt present frame
for (int i = 0; i < N; i++)
{
Pstar[i] = Vector3D.Add(Vector3D.Multiply(DHparameters[i, 2], frame[i, 2]), Vector3D.Multiply(DHparameters[i, 1], frame[i + 1, 0]));
}
//P(i) --> frame positions wrt base frame
for (int i = 1; i < N + 1; i++)
{
frame[i, 3] = frame[i - 1, 3] + Pstar[i - 1];
}
// set position of end effector
Ph = frame[N, 3];
// compute relative positions
Pih = new Vector3D[N + 1];
for(int i = N - 1; i >= 0; i--)
{
Pih[i] = Vector3D.Subtract(Ph, frame[i, 3]);
}
// set end effector orientation
Rh[0] = frame[N, 0];
Rh[1] = frame[N, 1];
Rh[2] = frame[N, 2];
// calculate orientation error
Eo = 0;
for (int i = 0; i < 3; i++)
{
if(Sigma[i])
{
Eo += Math.Pow((Vector3D.DotProduct(Rd[i], Rh[i]) - 1), 2);
}
}
// calculate current position error
Ec = Eo + Math.Pow(Vector3D.DotProduct(Vector3D.Subtract(Pd, Ph), Vector3D.Subtract(Pd, Ph)), 2);
//if ((Ec > IK_POS_THRESH) && (Ec < BETA) && (Ec > Math.Pow(Ep, 2))) // begin BFGS optimization
//{
// double epsg = 0.0000000001;
// double epsf = 0;
// double epsx = 0;
// int maxits = 0; // maximum number of iterations, for unlimited = 0
// double[] optiAngle = new double[N];
// for(int i = 0; i < N; i++)
// {
// optiAngle[i] = radAngle[i + 1];
// }
// alglib.minlbfgsstate state;
// alglib.minlbfgsreport rep;
// alglib.minlbfgscreate(N, 4, optiAngle, out state); // create optimizer with current joint angles for initial values
// alglib.minlbfgssetcond(state, epsg, epsf, epsx, maxits); // set optimizer options
// alglib.minlbfgsoptimize(state, function1_grad, null, null); // optimize
// alglib.minlbfgsresults(state, out optiAngle, out rep); // get results
// for(int i = 0; i < N; i++)
// {
// radAngle[i + 1] = optiAngle[i];
// }
//}
if (Ec > IK_POS_THRESH) // begin Cyclic Coordinate Descent loop
{
// create target effector position vector
Vector3D Pid = Vector3D.Subtract(Pd, frame[link, 3]);
//Pid.Normalize();
//Pih[link].Normalize();
double wp = 1; // position weight
double wo = 1; // orientation weight
double k1 = 0;
for(int i = 0; i < 3; i++)
{
if(Sigma[i])
k1 += wo * Vector3D.DotProduct(Rd[i], frame[link, 2]) * Vector3D.DotProduct(Rh[i], frame[link, 2]);
}
k1 += wp * Vector3D.DotProduct(Pid, frame[link-1, 2]) * Vector3D.DotProduct(Pih[link], frame[link-1, 2]);
double k2 = 0;
for (int i = 0; i < 3; i++)
{
if(Sigma[i])
k2 += wo * Vector3D.DotProduct(Rd[i], Rh[i]);
}
k2 += wp * Vector3D.DotProduct(Pid, Pih[link]);
double k3 = 0;
Vector3D ko3 = new Vector3D();
for (int i = 0; i < 3; i++)
{
if (Sigma[i])
ko3 = Vector3D.Add(ko3, wo * Vector3D.CrossProduct(Rh[i], Rd[i]));
}
k3 = Vector3D.DotProduct(frame[link-1, 2], Vector3D.Add(wp * Vector3D.CrossProduct(Pih[link], Pid), ko3));
// minimize position and orientation error
double turnAngle = Math.Atan(-k3 / (k1 - k2));
radAngle[link] += turnAngle;
/*
// use the dot product to calculate the cos of the desired angle
double cosAngle = Vector3D.DotProduct(Pid,Pih[link]);
// check if we need to rotate the link
if(cosAngle < 0.99999)
{
// use cross product to check rotation direction
Vector3D crossResult = Vector3D.CrossProduct(Pih[link], Pid);
crossResult.Normalize();
double turnAngle = Vector3D.AngleBetween(Pid, Pih[link]) * Math.PI / 180;
double sign = Vector3D.DotProduct(frame[link, 2], crossResult);
if (sign < 0)
turnAngle = -turnAngle;
radAngle[link] += turnAngle;
*/
// adjust angle based on joint limits
if (radAngle[link] < (MinMax[link - 1].X * Math.PI / 180))
radAngle[link] = MinMax[link - 1].X * Math.PI / 180;
else if (radAngle[link] > (MinMax[link - 1].Y * Math.PI / 180))
radAngle[link] = MinMax[link - 1].Y * Math.PI / 180;
if (double.IsNaN(radAngle[link]))
radAngle[link] = 0;
//}
// backward recurssion through joints for CCD
if (link-- < 2) link = N;
}
// set previous error value for next loop
Ep = Ec;
}
while (tries++ < IK_MAX_TRIES && Ec > IK_POS_THRESH);
double[] angles;
// check if we are outputting workspace forces
if(OutputWorkspace)
{
double forceGain = 0.5;
angles = new double[N + 2];
// calculate workspace forces if our position error is greater than the threshold
if (Ec > IK_POS_THRESH)
{
Vector3D forces = Vector3D.Multiply(forceGain, Vector3D.Subtract(Pd, Ph));
// invert forces if desired
angles[N - 1] = InvertForces[0] ? -forces.X : forces.X;
angles[N] = InvertForces[1] ? -forces.Y : forces.Y;
angles[N + 1] = InvertForces[2] ? -forces.Z : forces.Z;
for(int i = N-1; i < N+2; i++)
{
if (angles[i] > maxForce) angles[i] = maxForce;
else if (angles[i] < -maxForce) angles[i] = -maxForce;
}
}
else
{
// no workspace force if we can reach desired point
angles[N - 1] = 0;
angles[N] = 0;
angles[N + 1] = 0;
}
}
else
angles = new double[N - 1];
// change output angles based on joint coupling
switch(Coupling)
{
case CouplingType.None:
//convert angles to degrees
for (int i = 0; i < N - 1; i++)
{
angles[i] = radAngle[i + 1] * 180 / Math.PI;
}
break;
case CouplingType.ShoulderTwoDOF:
angles[0] = (radAngle[1] + radAngle[2]) * 180 / Math.PI ;
angles[1] = (radAngle[1] - radAngle[2]) * 180 / Math.PI;
for (int i = 2; i < N - 1; i++)
{
angles[i] = radAngle[i + 1] * 180 / Math.PI;
}
break;
}
return angles;
}
public void function1_grad(double[] q, ref double func, double[] grad, object obj)
{
Vector3D[] Pstar = new Vector3D[N];
Pstar.Initialize();
// initialize frame positions
for (int i = 0; i < N + 1; i++)
{
frame[i, 3].X = 0;
frame[i, 3].Y = 0;
frame[i, 3].Z = 0;
}
// forward recurrsion formulas for frame position and orientation
for (int i = 1; i < N + 1; i++)
{
// x(i) orientation vector
frame[i, 0] = Vector3D.Add((Vector3D.Multiply(Math.Cos(radAngle[i - 1]), frame[(i - 1), 0])), (Vector3D.Multiply(Math.Sin(radAngle[i - 1]), frame[(i - 1), 1])));
// z(i) orientation vector
frame[i, 2] = Vector3D.Add((Vector3D.Multiply(Math.Cos(DHparameters[i - 1, 0] * Math.PI / 180), frame[(i - 1), 2])), Vector3D.Multiply(Math.Sin(DHparameters[(i - 1), 0] * Math.PI / 180), Vector3D.CrossProduct(frame[i, 0], frame[(i - 1), 2])));
// y(i) orientation vector
frame[i, 1] = Vector3D.CrossProduct(frame[i, 2], frame[i, 0]);
}
// P* --> relative positions of next frame wrt present frame
for (int i = 0; i < N; i++)
{
Pstar[i] = Vector3D.Add(Vector3D.Multiply(DHparameters[i, 2], frame[i, 2]), Vector3D.Multiply(DHparameters[i, 1], frame[i + 1, 0]));
}
//P(i) --> frame positions wrt base frame
for (int i = 1; i < N + 1; i++)
{
frame[i, 3] = frame[i - 1, 3] + Pstar[i - 1];
}
// set position of end effector
Ph = frame[N, 3];
// compute relative positions
Pih = new Vector3D[N + 1];
for (int i = N - 1; i >= 0; i--)
{
Pih[i] = Vector3D.Subtract(Ph, frame[i, 3]);
}
// set end effector orientation
Rh[0] = frame[N, 0];
Rh[1] = frame[N, 1];
Rh[2] = frame[N, 2];
// calculate orientation error
Eo = 0;
for (int i = 0; i < 3; i++)
{
if (Sigma[i])
{
Eo += Math.Pow((Vector3D.DotProduct(Rd[i], Rh[i]) - 1), 2);
}
}
// function to be minimized
func = Eo + Math.Pow(Vector3D.DotProduct(Vector3D.Subtract(Pd, Ph), Vector3D.Subtract(Pd, Ph)), 2);
// declare gradient vector elements for each joint
for (int i = 0; i < N-1; i++)
{
Vector3D gradO = new Vector3D();
for (int j = 0; j < 3; j++ )
{
if (Sigma[j])
gradO = Vector3D.Add(gradO, (Vector3D.DotProduct(Rd[j], Rh[j]) - 1) * Vector3D.CrossProduct(Rh[j], Rd[j]));
}
grad[i] = Vector3D.DotProduct(Vector3D.Multiply(2, frame[i, 2]), Vector3D.Add((Vector3D.CrossProduct(Vector3D.Subtract(Pd, Ph), Pih[i])), gradO));
}
}
