/// <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)
{
double px = Position.X;
double py = Position.Y;
double pz = Position.Z;
double[] angles = new double[3];
double[] kineAngle = new double[3];
double[] radianAngle = new double[3];
double Lmax = LengthUpperArm + LengthForearm;
double L12 = Math.Sqrt(Math.Pow(px, 2) + Math.Pow(py, 2) + Math.Pow(pz, 2));
double Lratio = Lmax / L12;
double argument1 = (Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - Math.Pow(L12, 2)) / (2 * LengthUpperArm * LengthForearm);
kineAngle[2] = Math.PI - Math.Acos(argument1);
double argument2 = py / (LengthUpperArm + LengthForearm * Math.Cos(kineAngle[2]));
kineAngle[1] = Math.Atan2(argument2, Math.Sqrt(1 - Math.Pow(argument2, 2)));
double argument3 = LengthUpperArm * Math.Cos(kineAngle[1]) + LengthForearm * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]);
double test = Math.Atan2(px, pz);
kineAngle[0] = -Math.Atan2(pz, px) + Math.Atan2(argument3, Math.Sqrt(Math.Pow(px, 2) + Math.Pow(pz, 2) - Math.Pow(argument3, 2)));
if(px < 0 && pz < 0)
{
kineAngle[0] = kineAngle[0] - 2 * Math.PI;
}
radianAngle[0] = kineAngle[0] + kineAngle[1];
radianAngle[1] = kineAngle[0] - kineAngle[1];
radianAngle[2] = kineAngle[2];
angles[0] = radianAngle[0] * 180 / Math.PI;
angles[1] = radianAngle[1] * 180 / Math.PI;
angles[2] = radianAngle[2] * 180 / Math.PI;
if (double.IsNaN(angles[0]) || double.IsNaN(angles[1]) || double.IsNaN(angles[2]))
{
angles[0] = oldAngle[0];
angles[1] = oldAngle[1];
angles[2] = oldAngle[2];
Debug.WriteLine("Out of workspace!");
}
oldAngle[0] = angles[0];
oldAngle[1] = angles[1];
oldAngle[2] = angles[2];
return angles;
}
/// <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)
{
double px = Position.X;
double py = Position.Y;
double pz = Position.Z;
double[] angles = new double[6];
double[] kineAngle = new double[3];
double[] radianAngle = new double[3];
double[] minAngle = new double[3];
double[] maxAngle = new double[3];
double[] dummyAngle = { 0, 0, 0 };
bool angleLimited = false;
minAngle[0] = Theta1Min * Math.PI / 180;
minAngle[1] = Theta2Min * Math.PI / 180;
minAngle[2] = Theta3Min * Math.PI / 180;
maxAngle[0] = Theta1Max * Math.PI / 180;
maxAngle[1] = Theta2Max * Math.PI / 180;
maxAngle[2] = Theta3Max * Math.PI / 180;
double Lmax = LengthUpperArm + LengthForearm;
double Lmin = Math.Sqrt(Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - 2 * LengthUpperArm * LengthForearm * Math.Cos(Math.PI - maxAngle[2]));
double L12 = Math.Sqrt(Math.Pow(px, 2) + Math.Pow(py, 2) + Math.Pow(pz, 2));
double Lratio = Lmax / L12;
if(L12 > Lmax)
{
px = px * Lratio;
py = py * Lratio;
pz = pz * Lratio;
L12 = Lmax;
kineAngle[2] = 0;
}
else if (L12 < Lmin)
{
Lratio = Lmin / L12;
px = px * Lratio;
py = py * Lratio;
pz = pz * Lratio;
L12 = Lmin;
kineAngle[2] = maxAngle[2];
}
else
{
double argument1 = (Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - Math.Pow(L12, 2)) / (2 * LengthUpperArm * LengthForearm);
kineAngle[2] = Math.PI - Math.Acos(argument1);
}
double argument2 = py / (LengthUpperArm + LengthForearm * Math.Cos(kineAngle[2]));
if (argument2 > 1)
{
argument2 = 1;
angleLimited = true;
}
kineAngle[1] = Math.Atan2(argument2, Math.Sqrt(1 - Math.Pow(argument2, 2)));
double argument3 = LengthUpperArm * Math.Cos(kineAngle[1]) + LengthForearm * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]);
kineAngle[0] = -Math.Atan2(pz, px) + Math.Atan2(argument3, Math.Sqrt(Math.Pow(px, 2) + Math.Pow(pz, 2) - Math.Pow(argument3, 2)));
if(px < 0 && pz < 0)
{
kineAngle[0] = kineAngle[0] - 2 * Math.PI;
}
for (int i = 0; i < 3; i++ )
{
if (kineAngle[i] < minAngle[i])
{
kineAngle[i] = minAngle[i];
angleLimited = true;
}
else if (kineAngle[i] > maxAngle[i])
{
kineAngle[i] = maxAngle[i];
angleLimited = true;
}
}
radianAngle[0] = kineAngle[0] + kineAngle[1];
radianAngle[1] = kineAngle[0] - kineAngle[1];
radianAngle[2] = kineAngle[2];
//radianAngle[0] = kineAngle[0];
//radianAngle[1] = kineAngle[1];
//radianAngle[2] = kineAngle[2];
angles[0] = radianAngle[0] * 180 / Math.PI;
angles[1] = radianAngle[1] * 180 / Math.PI;
angles[2] = radianAngle[2] * 180 / Math.PI;
// calculate forward kinematics and haptic forces
double kineZ = LengthUpperArm * Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) - LengthForearm * (Math.Sin(kineAngle[0]) * Math.Sin(kineAngle[2]) - Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
double kineY = LengthUpperArm * Math.Sin(kineAngle[1]) + LengthForearm * Math.Sin(kineAngle[1]) * Math.Cos(kineAngle[2]);
double kineX = LengthUpperArm * Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) + LengthForearm * (Math.Cos(kineAngle[0]) * Math.Sin(kineAngle[2]) + Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
if(!angleLimited)
{
oldPoint.X = kineX;
oldPoint.Y = kineY;
oldPoint.Z = kineZ;
}
double barrierSpring = 0.5;
angles[3] = (oldPoint.X - Position.X) * barrierSpring;
angles[4] = (oldPoint.Y - Position.Y) * -barrierSpring;
angles[5] = (oldPoint.Z - Position.Z) * -barrierSpring;
for (int i = 3; i < 6; i++)
{
if (angles[i] > 4)
angles[i] = 4;
else if (angles[i] < -4)
angles[i] = -4;
}
for (int i = 0; i < 6; i++)
{
if (double.IsNaN(angles[i]))
{
angles[i] = oldAngle[i];
}
oldAngle[i] = angles[i];
}
return angles;
}
public void UpdateOutput()
{
// Only update the output if we've set our kinematic model
if (model == null)
return;
Point3D point = new Point3D();
point.X = InvertX ? -x : x;
point.Y = InvertY ? -y : y;
point.Z = InvertZ ? -z : z;
double[] angles = model.GetJointAngles(point);
for(int i = 0; i< angles.Length; i++)
{
if(ArmSide == 1 && i == 3)
{
Outputs[model.OutputNames[i]].Value = -angles[i];
}
else
Outputs[model.OutputNames[i]].Value = angles[i];
}
byte r = map(Math.Pow(angles[angles.Length - 3], 2) + Math.Pow(angles[angles.Length - 2], 2) + Math.Pow(angles[angles.Length - 1], 2), 0, 48, 50, 255);
HapticColor = new SolidColorBrush(Color.FromRgb(r, 50, 50));
}
/// <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)
{
double px = Position.X;
double py = Position.Y;
double pz = Position.Z;
double[] angles = new double[6];
double[] kineAngle = new double[3];
double[] radianAngle = new double[3];
double[] minAngle = new double[3];
double[] maxAngle = new double[3];
double[] dummyAngle = { 0, 0, 0 };
bool angleLimited = false;
minAngle[0] = Theta1Min * Math.PI / 180;
minAngle[1] = Theta2Min * Math.PI / 180;
minAngle[2] = Theta3Min * Math.PI / 180;
maxAngle[0] = Theta1Max * Math.PI / 180;
maxAngle[1] = Theta2Max * Math.PI / 180;
maxAngle[2] = Theta3Max * Math.PI / 180;
double Lmax = LengthUpperArm + LengthForearm;
double Lmin = Math.Sqrt(Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - 2 * LengthUpperArm * LengthForearm * Math.Cos(Math.PI - maxAngle[2]));
double L12 = Math.Sqrt(Math.Pow(px, 2) + Math.Pow(py, 2) + Math.Pow(pz, 2));
double Lratio = Lmax / L12;
if (L12 > Lmax)
{
px = px * Lratio;
py = py * Lratio;
pz = pz * Lratio;
L12 = Lmax;
kineAngle[2] = 0;
}
else if (L12 < Lmin)
{
Lratio = Lmin / L12;
px = px * Lratio;
py = py * Lratio;
pz = pz * Lratio;
L12 = Lmin;
kineAngle[2] = maxAngle[2];
}
else
{
double argument1 = (Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - Math.Pow(L12, 2)) / (2 * LengthUpperArm * LengthForearm);
kineAngle[2] = Math.PI - Math.Acos(argument1);
}
double argument2 = py / (LengthUpperArm + LengthForearm * Math.Cos(kineAngle[2]));
if (argument2 > 1)
{
argument2 = 1;
angleLimited = true;
}
kineAngle[1] = Math.Atan2(argument2, Math.Sqrt(1 - Math.Pow(argument2, 2)));
double argument3 = LengthUpperArm * Math.Cos(kineAngle[1]) + LengthForearm * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]);
kineAngle[0] = -Math.Atan2(pz, px) + Math.Atan2(argument3, Math.Sqrt(Math.Pow(px, 2) + Math.Pow(pz, 2) - Math.Pow(argument3, 2)));
if (px < 0 && pz < 0)
{
kineAngle[0] = kineAngle[0] - 2 * Math.PI;
}
for (int i = 0; i < 3; i++)
{
if (kineAngle[i] < minAngle[i])
{
kineAngle[i] = minAngle[i];
angleLimited = true;
}
else if (kineAngle[i] > maxAngle[i])
{
kineAngle[i] = maxAngle[i];
angleLimited = true;
}
}
radianAngle[0] = kineAngle[0] + kineAngle[1];
radianAngle[1] = kineAngle[0] - kineAngle[1];
radianAngle[2] = kineAngle[2];
//radianAngle[0] = kineAngle[0];
//radianAngle[1] = kineAngle[1];
//radianAngle[2] = kineAngle[2];
angles[0] = radianAngle[0] * 180 / Math.PI;
angles[1] = radianAngle[1] * 180 / Math.PI;
angles[2] = radianAngle[2] * 180 / Math.PI;
// calculate forward kinematics and haptic forces
double kineZ = LengthUpperArm * Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) - LengthForearm * (Math.Sin(kineAngle[0]) * Math.Sin(kineAngle[2]) - Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
double kineY = LengthUpperArm * Math.Sin(kineAngle[1]) + LengthForearm * Math.Sin(kineAngle[1]) * Math.Cos(kineAngle[2]);
double kineX = LengthUpperArm * Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) + LengthForearm * (Math.Cos(kineAngle[0]) * Math.Sin(kineAngle[2]) + Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
if (!angleLimited)
{
oldPoint.X = kineX;
oldPoint.Y = kineY;
oldPoint.Z = kineZ;
}
double barrierSpring = 0.5;
angles[3] = (oldPoint.X - Position.X) * barrierSpring;
angles[4] = (oldPoint.Y - Position.Y) * -barrierSpring;
angles[5] = (oldPoint.Z - Position.Z) * -barrierSpring;
for (int i = 3; i < 6; i++)
{
if (angles[i] > 4)
{
angles[i] = 4;
}
else if (angles[i] < -4)
{
angles[i] = -4;
}
}
for (int i = 0; i < 6; i++)
{
if (double.IsNaN(angles[i]))
{
angles[i] = oldAngle[i];
}
oldAngle[i] = angles[i];
}
return(angles);
}
public void UpdateOutput()
{
// Only update the output if we've set our kinematic model
if (model == null)
return;
Point3D pointL = new Point3D();
pointL.X = InvertXL ? -xL : xL;
pointL.Y = InvertYL ? -yL : yL;
pointL.Z = InvertZL ? -zL : zL;
Point3D pointR = new Point3D();
pointR.X = InvertXR ? -xR : xR;
pointR.Y = InvertYR ? -yR : yR;
pointR.Z = InvertZR ? -zR : zR;
double[] angles = model.GetJointAngles(pointL, pointR);
for (int i = 0; i < angles.Length; i++)
{
Outputs[model.OutputNames[i]].Value = angles[i];
}
}
/// <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 PositionL, Point3D PositionR)
{
double pxL = PositionL.X;
double pyL = PositionL.Y;
double pzL = PositionL.Z;
double pxR = PositionR.X;
double pyR = PositionR.Y;
double pzR = PositionR.Z;
double[] angles = new double[12];
double[] kineAngle = new double[3];
double[] radianAngle = new double[3];
double[] minAngle = new double[3];
double[] maxAngle = new double[3];
double[] dummyAngle = { 0, 0, 0 };
bool angleLimited = false;
minAngle[0] = Theta1Min * Math.PI / 180;
minAngle[1] = Theta2Min * Math.PI / 180;
minAngle[2] = Theta3Min * Math.PI / 180;
maxAngle[0] = Theta1Max * Math.PI / 180;
maxAngle[1] = Theta2Max * Math.PI / 180;
maxAngle[2] = Theta3Max * Math.PI / 180;
double Lmax = LengthUpperArm + LengthForearm;
double barrierSpring = 0.5;
// Left Arm Kinematics
double L12 = Math.Sqrt(Math.Pow(pxL, 2) + Math.Pow(pyL, 2) + Math.Pow(pzL, 2));
double Lratio = Lmax / L12;
if(L12 > Lmax)
{
pxL = pxL * Lratio;
pyL = pyL * Lratio;
pzL = pzL * Lratio;
L12 = Lmax;
kineAngle[2] = 0;
}
else
{
double argument1 = (Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - Math.Pow(L12, 2)) / (2 * LengthUpperArm * LengthForearm);
kineAngle[2] = Math.PI - Math.Acos(argument1);
}
double argument2 = pyL / (LengthUpperArm + LengthForearm * Math.Cos(kineAngle[2]));
if (argument2 > 1)
{
argument2 = 1;
angleLimited = true;
}
kineAngle[1] = Math.Atan2(argument2, Math.Sqrt(1 - Math.Pow(argument2, 2)));
double argument3 = LengthUpperArm * Math.Cos(kineAngle[1]) + LengthForearm * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]);
kineAngle[0] = -Math.Atan2(pzL, pxL) + Math.Atan2(argument3, Math.Sqrt(Math.Pow(pxL, 2) + Math.Pow(pzL, 2) - Math.Pow(argument3, 2)));
if(pxL < 0 && pzL < 0)
{
kineAngle[0] = kineAngle[0] - 2 * Math.PI;
}
for (int i = 0; i < 3; i++ )
{
if (kineAngle[i] < minAngle[i])
{
kineAngle[i] = minAngle[i];
angleLimited = true;
}
else if (kineAngle[i] > maxAngle[i])
{
kineAngle[i] = maxAngle[i];
angleLimited = true;
}
}
radianAngle[0] = kineAngle[0] + kineAngle[1];
radianAngle[1] = kineAngle[0] - kineAngle[1];
radianAngle[2] = kineAngle[2];
angles[0] = radianAngle[0] * 180 / Math.PI;
angles[1] = radianAngle[1] * 180 / Math.PI;
angles[2] = radianAngle[2] * 180 / Math.PI;
// calculate inverse kinematics and haptic forces
double kineZ = LengthUpperArm * Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) - LengthForearm * (Math.Sin(kineAngle[0]) * Math.Sin(kineAngle[2]) - Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
double kineY = LengthUpperArm * Math.Sin(kineAngle[1]) + LengthForearm * Math.Sin(kineAngle[1]) * Math.Cos(kineAngle[2]);
double kineX = LengthUpperArm * Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) + LengthForearm * (Math.Cos(kineAngle[0]) * Math.Sin(kineAngle[2]) + Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
if(!angleLimited)
{
oldPoint.X = kineX;
oldPoint.Y = kineY;
oldPoint.Z = kineZ;
}
angles[3] = (oldPoint.X - PositionL.X) * barrierSpring;
angles[4] = (oldPoint.Y - PositionL.Y) * -barrierSpring;
angles[5] = (oldPoint.Z - PositionL.Z) * -barrierSpring;
for (int i = 3; i < 6; i++)
{
if (angles[i] > 4)
angles[i] = 4;
else if (angles[i] < -4)
angles[i] = -4;
}
// Right arm kinematics
L12 = Math.Sqrt(Math.Pow(pxR, 2) + Math.Pow(pyR, 2) + Math.Pow(pzR, 2));
Lratio = Lmax / L12;
if (L12 > Lmax)
{
pxR = pxR * Lratio;
pyR = pyR * Lratio;
pzR = pzR * Lratio;
L12 = Lmax;
kineAngle[2] = 0;
}
else
{
double argument1 = (Math.Pow(LengthUpperArm, 2) + Math.Pow(LengthForearm, 2) - Math.Pow(L12, 2)) / (2 * LengthUpperArm * LengthForearm);
kineAngle[2] = Math.PI - Math.Acos(argument1);
}
argument2 = pyR / (LengthUpperArm + LengthForearm * Math.Cos(kineAngle[2]));
if (argument2 > 1)
{
argument2 = 1;
angleLimited = true;
}
kineAngle[1] = Math.Atan2(argument2, Math.Sqrt(1 - Math.Pow(argument2, 2)));
argument3 = LengthUpperArm * Math.Cos(kineAngle[1]) + LengthForearm * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]);
kineAngle[0] = -Math.Atan2(pzR, pxR) + Math.Atan2(argument3, Math.Sqrt(Math.Pow(pxR, 2) + Math.Pow(pzR, 2) - Math.Pow(argument3, 2)));
if (pxL < 0 && pzL < 0)
{
kineAngle[0] = kineAngle[0] - 2 * Math.PI;
}
for (int i = 0; i < 3; i++)
{
if (kineAngle[i] < minAngle[i])
{
kineAngle[i] = minAngle[i];
angleLimited = true;
}
else if (kineAngle[i] > maxAngle[i])
{
kineAngle[i] = maxAngle[i];
angleLimited = true;
}
}
radianAngle[0] = kineAngle[0] + kineAngle[1];
radianAngle[1] = kineAngle[0] - kineAngle[1];
radianAngle[2] = kineAngle[2];
angles[6] = radianAngle[0] * 180 / Math.PI;
angles[7] = radianAngle[1] * 180 / Math.PI;
angles[8] = radianAngle[2] * 180 / Math.PI;
// calculate inverse kinematics and haptic forces
kineZ = LengthUpperArm * Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) - LengthForearm * (Math.Sin(kineAngle[0]) * Math.Sin(kineAngle[2]) - Math.Cos(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
kineY = LengthUpperArm * Math.Sin(kineAngle[1]) + LengthForearm * Math.Sin(kineAngle[1]) * Math.Cos(kineAngle[2]);
kineX = LengthUpperArm * Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) + LengthForearm * (Math.Cos(kineAngle[0]) * Math.Sin(kineAngle[2]) + Math.Sin(kineAngle[0]) * Math.Cos(kineAngle[1]) * Math.Cos(kineAngle[2]));
if (!angleLimited)
{
oldPoint.X = kineX;
oldPoint.Y = kineY;
oldPoint.Z = kineZ;
}
angles[9] = (oldPoint.X - PositionR.X) * -barrierSpring;
angles[10] = (oldPoint.Y - PositionR.Y) * -barrierSpring;
angles[11] = (oldPoint.Z - PositionR.Z) * -barrierSpring;
for (int i = 3; i < 6; i++)
{
if (angles[i] > 4)
angles[i] = 4;
else if (angles[i] < -4)
angles[i] = -4;
}
for (int i = 0; i < 6; i++)
{
if (double.IsNaN(angles[i]))
{
angles[i] = oldAngle[i];
}
oldAngle[i] = angles[i];
}
return angles;
}
protected virtual double[] getJointAngles(Point3D PositionL, Point3D PositionR)
{
return new double[0];
}
public double[] GetJointAngles(Point3D PositionL, Point3D PositionR)
{
return (getJointAngles(PositionL, PositionR));
}
public void UpdateOutput()
{
// Only update the output if we've set our kinematic model
if (model == null)
return;
Point3D point = new Point3D();
point.X = InvertX ? -ix : ix;
point.Y = InvertY ? -iy : iy;
point.Z = InvertZ ? -iz : iz;
Point3D orient = new Point3D();
orient.X = iroll;
orient.Y = ipitch;
orient.Z = iyaw;
double[] angles = model.GetJointAngles(point, orient);
for (int i = 0; i < angles.Length; i++)
{
Outputs[model.OutputNames[i]].Value = angles[i];
}
}
