internal RobotCellKinematics(RobotCell cell, IEnumerable <Target> targets, IEnumerable <double[]> prevJoints, bool displayMeshes) { this.Solutions = new List <KinematicSolution>(new KinematicSolution[cell.MechanicalGroups.Count]); if (targets.Count() != cell.MechanicalGroups.Count) { throw new Exception(" Incorrect number of targets."); } if (prevJoints != null && prevJoints.Count() != cell.MechanicalGroups.Count) { throw new Exception(" Incorrect number of previous joint values."); } var groups = cell.MechanicalGroups.ToList(); foreach (var target in targets) { var index = target.Frame.CoupledMechanicalGroup; if (index == -1) { continue; } var group = cell.MechanicalGroups[index]; groups.RemoveAt(index); groups.Insert(0, group); } var targetsArray = targets.ToArray(); var prevJointsArray = prevJoints?.ToArray(); foreach (var group in groups) { int i = group.Index; var target = targetsArray[i]; var prevJoint = prevJointsArray?[i]; Plane?coupledPlane = null; int coupledGroup = target.Frame.CoupledMechanicalGroup; if (coupledGroup != -1 && target.Frame.CoupledMechanism == -1) { if (coupledGroup == i) { throw new Exception(" Can't couple a robot with itself."); } coupledPlane = Solutions[coupledGroup].Planes[Solutions[coupledGroup].Planes.Length - 2] as Plane?; } else { coupledPlane = null; } var kinematics = group.Kinematics(target, prevJoint, coupledPlane, displayMeshes, cell.BasePlane); Solutions[i] = kinematics; } }
internal RobotCellKinematics(RobotCell cell, IEnumerable<Target> targets, IEnumerable<double[]> prevJoints, bool displayMeshes) { this.Solutions = new List<KinematicSolution>(new KinematicSolution[cell.MechanicalGroups.Count]); if (targets.Count() != cell.MechanicalGroups.Count) throw new Exception(" Incorrect number of targets."); if (prevJoints != null && prevJoints.Count() != cell.MechanicalGroups.Count) throw new Exception(" Incorrect number of previous joint values."); var groups = cell.MechanicalGroups.ToList(); foreach (var target in targets) { var index = target.Frame.CoupledMechanicalGroup; if (index == -1) continue; var group = cell.MechanicalGroups[index]; groups.RemoveAt(index); groups.Insert(0, group); } var targetsArray = targets.ToArray(); var prevJointsArray = prevJoints?.ToArray(); foreach (var group in groups) { int i = group.Index; var target = targetsArray[i]; var prevJoint = prevJointsArray?[i]; Plane? coupledPlane = null; int coupledGroup = target.Frame.CoupledMechanicalGroup; if (coupledGroup != -1 && target.Frame.CoupledMechanism == -1) { if (coupledGroup == i) throw new Exception(" Can't couple a robot with itself."); coupledPlane = Solutions[coupledGroup].Planes[Solutions[coupledGroup].Planes.Length - 2] as Plane?; } else { coupledPlane = null; } var kinematics = group.Kinematics(target, prevJoint, coupledPlane, displayMeshes, cell.BasePlane); Solutions[i] = kinematics; } }