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);
}
}
internal static MechanicalGroup Create(XElement element)
{
int index = 0;
var groupAttribute = element.Attribute(XName.Get("group"));
if (groupAttribute != null)
{
index = XmlConvert.ToInt32(groupAttribute.Value);
}
var mechanisms = new List <Mechanism>();
foreach (var mechanismElement in element.Elements())
{
mechanisms.Add(Mechanism.Create(mechanismElement));
}
return(new MechanicalGroup(index, mechanisms));
}
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?.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)
{
Plane plane = (robotBase != null) ? robotBase.Value : Plane.WorldXY;
Plane externalPlane = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
plane.Transform(externalPlane.ToTransform());
robotBase = plane;
}
}
// 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?.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);
}
}
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);
}
