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); }