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);
}
private static RobotSystem Create(XElement element, Plane basePlane)
{
var type = element.Name.LocalName;
var name = element.Attribute(XName.Get("name")).Value;
var manufacturer = (Manufacturers)Enum.Parse(typeof(Manufacturers), element.Attribute(XName.Get("manufacturer")).Value);
var mechanisms = new List <Mechanism>();
var mechanicalGroups = new List <MechanicalGroup>();
foreach (var mechanicalGroup in element.Elements(XName.Get("Mechanisms")))
{
mechanicalGroups.Add(MechanicalGroup.Create(mechanicalGroup));
}
IO io = null;
XElement ioElement = element.Element(XName.Get("IO"));
if (ioElement != null)
{
string[] doNames = null, diNames = null, aoNames = null, aiNames = null;
var doElement = ioElement.Element(XName.Get("DO"));
var diElement = ioElement.Element(XName.Get("DI"));
var aoElement = ioElement.Element(XName.Get("AO"));
var aiElement = ioElement.Element(XName.Get("AI"));
if (doElement != null)
{
doNames = doElement.Attribute(XName.Get("names")).Value.Split(',');
}
if (diElement != null)
{
diNames = diElement.Attribute(XName.Get("names")).Value.Split(',');
}
if (aoElement != null)
{
aoNames = aoElement.Attribute(XName.Get("names")).Value.Split(',');
}
if (aiElement != null)
{
aiNames = aiElement.Attribute(XName.Get("names")).Value.Split(',');
}
io = new IO()
{
DO = doNames, DI = diNames, AO = aoNames, AI = aiNames
};
}
Mesh environment = null;
if (type == "RobotCell")
{
switch (manufacturer)
{
case (Manufacturers.ABB):
return(new RobotCellAbb(name, mechanicalGroups, io, basePlane, environment));
case (Manufacturers.KUKA):
return(new RobotCellKuka(name, mechanicalGroups, io, basePlane, environment));
case (Manufacturers.UR):
return(new RobotCellUR(name, mechanicalGroups[0].Robot, io, basePlane, environment));
case (Manufacturers.FANUC):
return(new RobotCellFanuc(name, mechanicalGroups, io, basePlane, environment));
case (Manufacturers.Staubli):
return(new RobotCellStaubli(name, mechanicalGroups, io, basePlane, environment));
}
}
return(null);
}
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 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);
}
}
