static List <Mesh> PoseMeshes(MechanicalGroup group, IList <Plane> planes, Mesh tool)
{
planes = planes.ToList();
var count = planes.Count - 1;
planes.RemoveAt(count);
planes.Add(planes[count - 1]);
var outMeshes = group.DefaultMeshes.Select(m => m.DuplicateMesh()).Append(tool.DuplicateMesh()).ToList();
for (int i = 0; i < group.DefaultPlanes.Count; i++)
{
var s = Transform.PlaneToPlane(group.DefaultPlanes[i], planes[i]);
outMeshes[i].Transform(s);
}
return(outMeshes);
}
internal MechanicalGroupKinematics(MechanicalGroup group, Target target, double[]?prevJoints, Plane?coupledPlane, Plane?basePlane)
{
var jointCount = group.Joints.Count;
if (prevJoints is not null && prevJoints.Length != jointCount)
{
Errors.Add($"Previous joints set but contain {prevJoints.Length} value(s), should contain {jointCount} values.");
prevJoints = null;
}
Joints = new double[jointCount];
var planes = new List <Plane>(jointCount + 2);
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?.Subset(external.Joints.Map(x => x.Number));
var externalKinematics = external.Kinematics(target, externalPrevJoints, basePlane);
for (int i = 0; i < external.Joints.Length; i++)
{
Joints[external.Joints[i].Number] = externalKinematics.Joints[i];
}
planes.AddRange(externalKinematics.Planes);
Errors.AddRange(externalKinematics.Errors);
if (external == coupledMech)
{
coupledPlane = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
}
if (external.MovesRobot)
{
Plane externalPlane = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
robotBase = externalPlane;
}
}
// Coupling
if (coupledPlane is not null)
{
var coupledFrame = target.Frame.ShallowClone();
var plane = coupledFrame.Plane;
var cPlane = (Plane)coupledPlane;
plane.Orient(ref cPlane);
coupledFrame.Plane = plane;
target.Frame = coupledFrame;
}
// Robot
var robot = group.Robot;
if (robot is not null)
{
var robotPrevJoints = prevJoints?.Subset(robot.Joints.Map(x => x.Number));
var robotKinematics = robot.Kinematics(target, robotPrevJoints, robotBase);
for (int j = 0; j < robot.Joints.Length; j++)
{
Joints[robot.Joints[j].Number] = robotKinematics.Joints[j];
}
planes.AddRange(robotKinematics.Planes);
Configuration = robotKinematics.Configuration;
Errors.AddRange(robotKinematics.Errors);
}
// Tool
Plane toolPlane = target.Tool.Tcp;
var lastPlane = planes[planes.Count - 1];
toolPlane.Orient(ref lastPlane);
planes.Add(toolPlane);
Planes = planes.ToArray();
}
