protected override void SetJoints(Target target, double[] prevJoints)
{
int externalNum = mechanism.Joints[0].Number - 6;
if (target.External.Length < externalNum)
Errors.Add($"Track external axis not configured on this target.");
else
Joints[0] = target.External[externalNum];
}
protected override void SetPlanes(Target target)
{
int jointCount = mechanism.Joints.Length;
for (int i = 0; i < jointCount; i++)
{
Planes[i + 1] = mechanism.Joints[i].Plane;
for (int j = i; j >= 0; j--)
Planes[i + 1].Rotate(Joints[j], mechanism.Joints[j].Plane.Normal, mechanism.Joints[j].Plane.Origin);
}
}
protected override void SetJoints(Target target, double[] prevJoints)
{
for (int i = 0; i < mechanism.Joints.Length; i++)
{
int externalNum = mechanism.Joints[i].Number - 6;
if (target.External.Length - 1 < externalNum)
Errors.Add($"Positioner external axis not configured on this target.");
else
Joints[i] = target.External[externalNum];
}
if (prevJoints != null)
Joints = JointTarget.GetAbsoluteJoints(Joints, prevJoints);
}
protected override void SetJoints(Target target, double[] prevJoints)
{
if (target is JointTarget)
{
Joints = (target as JointTarget).Joints;
}
else if (target is CartesianTarget)
{
double[] joints = null;
var cartesianTarget = target as CartesianTarget;
Plane tcp = target.Tool.Tcp;
tcp.Rotate(PI, Vector3d.ZAxis, Point3d.Origin);
Plane targetPlane = cartesianTarget.Plane;
targetPlane.Transform(target.Frame.Plane.ToTransform());
var transform = Transform.PlaneToPlane(Planes[0], Plane.WorldXY) * Transform.PlaneToPlane(tcp, targetPlane);
List<string> errors;
if (cartesianTarget.Configuration != null || prevJoints == null)
{
Configuration = cartesianTarget.Configuration ?? Target.RobotConfigurations.None;
joints = InverseKinematics(transform, Configuration, out errors);
}
else
{
joints = GetClosestSolution(transform, prevJoints, out var configuration, out errors, out var difference);
Configuration = configuration;
}
if (prevJoints != null)
Joints = JointTarget.GetAbsoluteJoints(joints, prevJoints);
else
Joints = joints;
Errors.AddRange(errors);
}
}
protected override void SetPlanes(Target target)
{
var jointTransforms = ForwardKinematics(Joints);
if (target is JointTarget)
{
var closest = GetClosestSolution(jointTransforms[jointTransforms.Length - 1], Joints, out var configuration, out var errors, out var difference);
this.Configuration = difference < AngleTol ? configuration : Target.RobotConfigurations.Undefined;
}
for (int i = 0; i < 6; i++)
{
var plane = jointTransforms[i].ToPlane();
plane.Rotate(PI, plane.ZAxis);
Planes[i + 1] = plane;
}
/*
{
Planes[7] = target.Tool.Tcp;
Planes[7].Transform(Planes[6].ToTransform());
}
*/
}
internal ProgramTarget(Target target, int group)
{
this.Target = target;
this.Group = group;
this.Commands = new Commands.Group();
if (target.Command != null)
{
if (target.Command is Commands.Group)
Commands.AddRange((target.Command as Commands.Group).Flatten());
else
Commands.Add(target.Command);
}
}
public OffsetWristKinematics(RobotArm robot, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane) : base(robot, target, prevJoints, displayMeshes, basePlane) { }
protected RobotKinematics(RobotArm robot, Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null) : base(robot, target, prevJoints, displayMeshes, basePlane) { }
double[] GetClosestSolution(Transform transform, double[] prevJoints, out Target.RobotConfigurations configuration, out List<string> errors, out double difference)
{
var solutions = new double[8][];
var solutionsErrors = new List<List<string>>(8);
for (int i = 0; i < 8; i++)
{
solutions[i] = InverseKinematics(transform, (Target.RobotConfigurations)i, out var solutionErrors);
solutions[i] = JointTarget.GetAbsoluteJoints(solutions[i], prevJoints);
solutionsErrors.Add(solutionErrors);
}
int closestSolutionIndex = 0;
double closestDifference = double.MaxValue;
for (int i = 0; i < 8; i++)
{
double currentDifference = prevJoints.Zip(solutions[i], (x, y) => SquaredDifference(x, y)).Sum();
if (currentDifference < closestDifference)
{
closestSolutionIndex = i;
closestDifference = currentDifference;
}
}
difference = closestDifference;
configuration = (Target.RobotConfigurations)closestSolutionIndex;
errors = solutionsErrors[closestSolutionIndex];
return solutions[closestSolutionIndex];
}
public JointTarget(double[] joints, Target target, IEnumerable<double> external = null) : this(joints, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
/*
public static void WriteMeshes()
{
Rhino.FileIO.File3dm robotsGeometry = Rhino.FileIO.File3dm.Read($@"{ResourcesFolder}\robotsGeometry.3dm");
var jointmeshes = new JointMeshes();
foreach (var layer in robotsGeometry.Layers)
{
if (layer.Name == "Default" || layer.ParentLayerId != Guid.Empty) continue;
jointmeshes.Names.Add(layer.Name);
var meshes = new List<Mesh>();
meshes.Add(robotsGeometry.Objects.First(x => x.Attributes.LayerIndex == layer.LayerIndex).Geometry as Mesh);
int i = 0;
while (true)
{
string name = $"{i++ + 1}";
var jointLayer = robotsGeometry.Layers.FirstOrDefault(x => (x.Name == name) && (x.ParentLayerId == layer.Id));
if (jointLayer == null) break;
meshes.Add(robotsGeometry.Objects.First(x => x.Attributes.LayerIndex == jointLayer.LayerIndex).Geometry as Mesh);
}
jointmeshes.Meshes.Add(meshes);
}
using (var stream = new MemoryStream())
{
var formatter = new BinaryFormatter();
formatter.Serialize(stream, jointmeshes);
File.WriteAllBytes($@"{ResourcesFolder}\Meshes.rob", stream.ToArray());
}
}
*/
public abstract KinematicSolution Kinematics(Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null);
protected override void SetPlanes(Target target)
{
Planes[1] = mechanism.Joints[0].Plane;
Planes[1].Translate(Planes[1].YAxis * Joints[0]);
}
internal TrackKinematics(Track track, Target target, bool displayMeshes, Plane? basePlane) : base(track, target, null, displayMeshes, basePlane) { }
public override KinematicSolution Kinematics(Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null) => new TrackKinematics(this, target, displayMeshes, basePlane);
static Target()
{
Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 }, Tool.Default, Speed.Default, Zone.Default, null, Frame.Default, null);
}
public CartesianTarget(Plane plane, Target target, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, IEnumerable<double> external = null) : this(plane, configuration, motion, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
internal MechanismKinematics(Mechanism mechanism, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane)
{
this.mechanism = mechanism;
int jointCount = mechanism.Joints.Length;
// Init properties
Joints = new double[jointCount];
Planes = new Plane[jointCount + 1];
if (displayMeshes)
Meshes = new Mesh[jointCount + 1];
else
Meshes = new Mesh[0];
// Base plane
Planes[0] = mechanism.BasePlane;
if (basePlane != null)
{
Planes[0].Transform(Transform.PlaneToPlane(Plane.WorldXY,(Plane)basePlane));
}
SetJoints(target, prevJoints);
JointsOutOfRange();
SetPlanes(target);
// Move planes to base
var transform = Planes[0].ToTransform();
for (int i = 1; i < jointCount + 1; i++)
Planes[i].Transform(transform);
// Meshes
if (displayMeshes)
SetMeshes(target.Tool);
}
public override KinematicSolution Kinematics(Target target, double[] prevJoints, bool displayMeshes = true, Plane? basePlane = null) => new OffsetWristKinematics(this, target, prevJoints, displayMeshes, basePlane);
protected abstract void SetJoints(Target target, double[] prevJoints);
/// <summary>
/// Inverse kinematics for a spherical wrist 6 axis robot.
/// Code adapted from https://github.com/whitegreen/KinematikJava
/// </summary>
/// <param name="target">Cartesian target</param>
/// <returns>Returns the 6 rotation values in radians.</returns>
override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors)
{
errors = new List<string>();
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
if (shoulder) elbow = !elbow;
bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);
bool isUnreachable = false;
double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();
Plane flange = Plane.WorldXY;
flange.Transform(transform);
double[] joints = new double[6];
double l2 = Sqrt(a[2] * a[2] + d[3] * d[3]);
double ad2 = Atan2(a[2], d[3]);
Point3d center = flange.Origin - flange.Normal * d[5];
joints[0] = Atan2(center.Y, center.X);
double ll = Sqrt(center.X * center.X + center.Y * center.Y);
Point3d p1 = new Point3d(a[0] * center.X / ll, a[0] * center.Y / ll, d[0]);
if (shoulder)
{
joints[0] += PI;
var rotate = Transform.Rotation(PI, new Point3d(0, 0, 0));
center.Transform(rotate);
}
double l3 = (center - p1).Length;
double l1 = a[1];
double beta = Acos((l1 * l1 + l3 * l3 - l2 * l2) / (2 * l1 * l3));
if (double.IsNaN(beta))
{
beta = 0;
isUnreachable = true;
}
if (elbow)
beta *= -1;
double ttl = new Vector3d(center.X - p1.X, center.Y - p1.Y, 0).Length;
// if (p1.X * (center.X - p1.X) < 0)
if (shoulder)
ttl = -ttl;
double al = Atan2(center.Z - p1.Z, ttl);
joints[1] = beta + al;
double gama = Acos((l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2));
if (double.IsNaN(gama))
{
gama = PI;
isUnreachable = true;
}
if (elbow)
gama *= -1;
joints[2] = gama - ad2 - PI / 2;
double[] c = new double[3];
double[] s = new double[3];
for (int i = 0; i < 3; i++)
{
c[i] = Cos(joints[i]);
s[i] = Sin(joints[i]);
}
var arr = new Transform();
arr[0, 0] = c[0] * (c[1] * c[2] - s[1] * s[2]); arr[0, 1] = s[0]; arr[0, 2] = c[0] * (c[1] * s[2] + s[1] * c[2]); arr[0, 3] = c[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * c[0];
arr[1, 0] = s[0] * (c[1] * c[2] - s[1] * s[2]); arr[1, 1] = -c[0]; arr[1, 2] = s[0] * (c[1] * s[2] + s[1] * c[2]); arr[1, 3] = s[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * s[0];
arr[2, 0] = s[1] * c[2] + c[1] * s[2]; arr[2, 1] = 0; arr[2, 2] = s[1] * s[2] - c[1] * c[2]; arr[2, 3] = a[2] * (s[1] * c[2] + c[1] * s[2]) + a[1] * s[1] + d[0];
arr[3, 0] = 0; arr[3, 1] = 0; arr[3, 2] = 0; arr[3, 3] = 1;
arr.TryGetInverse(out var in123);
var mr = Transform.Multiply(in123, transform);
joints[3] = Atan2(mr[1, 2], mr[0, 2]);
joints[4] = Acos(mr[2, 2]);
joints[5] = Atan2(mr[2, 1], -mr[2, 0]);
if (wrist)
{
joints[3] += PI;
joints[4] *= -1;
joints[5] -= PI;
}
for (int i = 0; i < 6; i++)
{
if (joints[i] > PI) joints[i] -= 2 * PI;
if (joints[i] < -PI) joints[i] += 2 * PI;
}
if (isUnreachable)
errors.Add($"Target out of reach");
if (Abs(1 - mr[2, 2]) < 0.0001)
errors.Add($"Near wrist singularity");
if (new Vector3d(center.X, center.Y, 0).Length < a[0] + SingularityTol)
errors.Add($"Near overhead singularity");
for (int i = 0; i < 6; i++)
{
if (double.IsNaN(joints[i])) joints[i] = 0;
}
return joints;
}
protected abstract void SetPlanes(Target target);
public KinematicSolution Kinematics(Target target, double[] prevJoints = null, Plane? coupledPlane = null, bool displayMeshes = false, Plane? basePlane = null) => new MechanicalGroupKinematics(this, target, prevJoints, coupledPlane, displayMeshes, basePlane);
public double[] RadiansToDegreesExternal(Target target)
{
double[] values = new double[target.External.Length];
foreach (var mechanism in this.Externals)
{
foreach (var joint in mechanism.Joints)
{
values[joint.Number - 6] = mechanism.RadianToDegree(target.External[joint.Number - 6], joint.Index);
}
}
return values;
}
/// <summary>
/// Inverse kinematics for a offset wrist 6 axis robot.
/// Code adapted from https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_kinematics/src/ur_kin.cpp
/// </summary>
/// <param name="target">Cartesian target</param>
/// <returns>Returns the 6 rotation values in radians.</returns>
override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors)
{
errors = new List<string>();
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
if (shoulder) elbow = !elbow;
bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);
if (shoulder) wrist = !wrist;
double[] joints = new double[6];
bool isUnreachable = false;
transform *= Transform.Rotation(PI / 2, Point3d.Origin);
double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();
// shoulder
{
double A = d[5] * transform[1, 2] - transform[1, 3];
double B = d[5] * transform[0, 2] - transform[0, 3];
double R = A * A + B * B;
double arccos = Acos(d[3] / Sqrt(R));
if (double.IsNaN(arccos))
{
errors.Add($"Overhead singularity.");
arccos = 0;
}
double arctan = Atan2(-B, A);
if (!shoulder)
joints[0] = arccos + arctan;
else
joints[0] = -arccos + arctan;
}
// wrist 2
{
double numer = (transform[0, 3] * Sin(joints[0]) - transform[1, 3] * Cos(joints[0]) - d[3]);
double div = numer / d[5];
double arccos = Acos(div);
if (double.IsNaN(arccos))
{
errors.Add($"Overhead singularity 2.");
arccos = PI;
isUnreachable = true;
}
if (!wrist)
joints[4] = arccos;
else
joints[4] = 2.0 * PI - arccos;
}
// rest
{
double c1 = Cos(joints[0]);
double s1 = Sin(joints[0]);
double c5 = Cos(joints[4]);
double s5 = Sin(joints[4]);
joints[5] = Atan2(Sign(s5) * -(transform[0, 1] * s1 - transform[1, 1] * c1), Sign(s5) * (transform[0, 0] * s1 - transform[1, 0] * c1));
double c6 = Cos(joints[5]), s6 = Sin(joints[5]);
double x04x = -s5 * (transform[0, 2] * c1 + transform[1, 2] * s1) - c5 * (s6 * (transform[0, 1] * c1 + transform[1, 1] * s1) - c6 * (transform[0, 0] * c1 + transform[1, 0] * s1));
double x04y = c5 * (transform[2, 0] * c6 - transform[2, 1] * s6) - transform[2, 2] * s5;
double p13x = d[4] * (s6 * (transform[0, 0] * c1 + transform[1, 0] * s1) + c6 * (transform[0, 1] * c1 + transform[1, 1] * s1)) - d[5] * (transform[0, 2] * c1 + transform[1, 2] * s1) + transform[0, 3] * c1 + transform[1, 3] * s1;
double p13y = transform[2, 3] - d[0] - d[5] * transform[2, 2] + d[4] * (transform[2, 1] * c6 + transform[2, 0] * s6);
double c3 = (p13x * p13x + p13y * p13y - a[1] * a[1] - a[2] * a[2]) / (2.0 * a[1] * a[2]);
double arccos = Acos(c3);
if (double.IsNaN(arccos))
{
arccos = 0;
isUnreachable = true;
}
if (!elbow)
joints[2] = arccos;
else
joints[2] = 2.0 * PI - arccos;
double denom = a[1] * a[1] + a[2] * a[2] + 2 * a[1] * a[2] * c3;
double s3 = Sin(arccos);
double A = (a[1] + a[2] * c3);
double B = a[2] * s3;
if (!elbow)
joints[1] = Atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom);
else
joints[1] = Atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom);
double c23_0 = Cos(joints[1] + joints[2]);
double s23_0 = Sin(joints[1] + joints[2]);
joints[3] = Atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0);
}
if (isUnreachable)
errors.Add($"Target out of reach.");
// joints[5] += PI / 2;
for (int i = 0; i < 6; i++)
{
if (joints[i] > PI) joints[i] -= 2.0 * PI;
if (joints[i] < -PI) joints[i] += 2.0 * PI;
}
return joints;
}
internal MechanicalGroupKinematics(MechanicalGroup group, Target target, double[] prevJoints, Plane? coupledPlane, bool displayMeshes, Plane? basePlane)
{
var jointCount = group.Joints.Count;
Joints = new double[jointCount];
var planes = new List<Plane>();
List<Mesh> meshes = (displayMeshes) ? new List<Mesh>() : null;
var errors = new List<string>();
Plane? robotBase = basePlane;
target = target.ShallowClone();
Mechanism coupledMech = null;
if (target.Frame.CoupledMechanism != -1 && target.Frame.CoupledMechanicalGroup == group.Index)
{
coupledMech = group.Externals[target.Frame.CoupledMechanism];
}
// Externals
foreach (var external in group.Externals)
{
var externalPrevJoints = prevJoints == null ? null : prevJoints.Subset(external.Joints.Select(x => x.Number).ToArray());
var externalKinematics = external.Kinematics(target, externalPrevJoints, displayMeshes, basePlane);
for (int i = 0; i < external.Joints.Length; i++)
Joints[external.Joints[i].Number] = externalKinematics.Joints[i];
planes.AddRange(externalKinematics.Planes);
if (displayMeshes) meshes.AddRange(externalKinematics.Meshes);
errors.AddRange(externalKinematics.Errors);
if (external == coupledMech)
coupledPlane = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
if (external.MovesRobot)
robotBase = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
}
// Coupling
if (coupledPlane != null)
{
var coupledFrame = target.Frame.ShallowClone();
var plane = coupledFrame.Plane;
plane.Transform(Transform.PlaneToPlane(Plane.WorldXY, (Plane)coupledPlane));
coupledFrame.Plane = plane;
target.Frame = coupledFrame;
}
// Robot
var robot = group.Robot;
if (robot != null)
{
var robotPrevJoints = prevJoints == null ? null : prevJoints.Subset(robot.Joints.Select(x => x.Number).ToArray());
var robotKinematics = robot.Kinematics(target, robotPrevJoints, displayMeshes, robotBase);
for (int j = 0; j < robot.Joints.Length; j++)
Joints[robot.Joints[j].Number] = robotKinematics.Joints[j];
planes.AddRange(robotKinematics.Planes);
if (displayMeshes) meshes.AddRange(robotKinematics.Meshes);
Configuration = robotKinematics.Configuration;
if (robotKinematics.Errors.Count > 0)
{
errors.AddRange(robotKinematics.Errors);
}
}
// Tool
Plane toolPlane = target.Tool.Tcp;
toolPlane.Transform(planes[planes.Count - 1].ToTransform());
planes.Add(toolPlane);
if (displayMeshes)
{
if (target.Tool.Mesh != null)
{
Mesh toolMesh = target.Tool.Mesh.DuplicateMesh();
toolMesh.Transform(Transform.PlaneToPlane(target.Tool.Tcp, planes[planes.Count - 1]));
meshes.Add(toolMesh);
}
else
meshes.Add(null);
}
Planes = planes.ToArray();
if (displayMeshes) Meshes = meshes.ToArray();
if (errors.Count > 0)
{
Errors.AddRange(errors);
}
}
protected abstract double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors);
internal PositionerKinematics(Positioner positioner, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane) : base(positioner, target, prevJoints, displayMeshes, basePlane) { }