/// <summary> /// Defines a Forward Kinematic object from a Joint Target with defined Joint Positions obtained from a Joint Target. /// </summary> /// <param name="robot"> The Robot. </param> /// <param name="jointTarget"> The Joint Target. </param> /// <param name="hideMesh"> Specifies whether the mesh will be supressed. </param> public ForwardKinematics(Robot robot, JointTarget jointTarget, bool hideMesh = false) { _robot = robot; _hideMesh = hideMesh; _robotJointPosition = jointTarget.RobotJointPosition; _externalJointPosition = jointTarget.ExternalJointPosition; }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object can be used to retrieve data from input parameters and to store data in output parameters.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Variables string name = ""; RobotJointPosition robotJointPosition = new RobotJointPosition(); ExternalJointPosition externalJointPosition = new ExternalJointPosition(); // Catch input data if (!DA.GetData(0, ref name)) { return; } if (!DA.GetData(1, ref robotJointPosition)) { robotJointPosition = new RobotJointPosition(); } if (!DA.GetData(2, ref externalJointPosition)) { externalJointPosition = new ExternalJointPosition(); } // Replace spaces name = HelperMethods.ReplaceSpacesAndRemoveNewLines(name); JointTarget jointTarget = new JointTarget(name, robotJointPosition, externalJointPosition); // Sets Output DA.SetData(0, jointTarget); }
/// <summary> /// Calculates the interpolated path of a joint movement from a Robot Target. /// </summary> /// <param name="movement"> The movement with as Target a Robot Target. </param> private void JointMovementFromRobotTarget(Movement movement) { // Set the correct tool for this movement SetRobotTool(movement); // Get the final joint positions of this movement _robot.InverseKinematics.Movement = movement; _robot.InverseKinematics.Calculate(); // Auto Axis Config if (_jointConfigurationControl == false && movement.MovementType != MovementType.MoveAbsJ) { _robot.InverseKinematics.CalculateClosestRobotJointPosition(_lastRobotJointPosition); } // Get the Robot Joint Positions RobotJointPosition towardsRobotJointPosition = _robot.InverseKinematics.RobotJointPosition.Duplicate(); ExternalJointPosition towardsExternalJointPosition = _robot.InverseKinematics.ExternalJointPosition.Duplicate(); // Add error text _errorText.AddRange(_robot.InverseKinematics.ErrorText.ConvertAll(item => string.Copy(item))); // Interpolate InterpolateJointMovement(towardsRobotJointPosition, towardsExternalJointPosition); }
/// <summary> /// Initializes a new instance of the Forward Kinematics class with defined Joint Positions. /// </summary> /// <param name="robot"> The Robot. </param> /// <param name="robotJointPosition"> The Robot Joint Position. </param> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <param name="hideMesh"> Specifies whether the mesh will be supressed. </param> public ForwardKinematics(Robot robot, RobotJointPosition robotJointPosition, ExternalJointPosition externalJointPosition, bool hideMesh = false) { _robot = robot; _hideMesh = hideMesh; _robotJointPosition = robotJointPosition; _externalJointPosition = externalJointPosition; }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Input variables ExternalJointPosition extJointPosition = null; // Catch the input data if (!DA.GetData(0, ref extJointPosition)) { return; } // Check if the object is valid if (!extJointPosition.IsValid) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "The External Joint Position is not valid"); } // Output DA.SetData(0, extJointPosition[0]); DA.SetData(1, extJointPosition[1]); DA.SetData(2, extJointPosition[2]); DA.SetData(3, extJointPosition[3]); DA.SetData(4, extJointPosition[4]); DA.SetData(5, extJointPosition[5]); }
/// <summary> /// Calculates the interpolated path for a joint movement. /// </summary> /// <param name="towardsRobotJointPosition"> The final Robot Joint Position of the joint movement. </param> /// <param name="towardsExternalJointPosition"> The final External Joint Position of the joint movement. </param> private void InterpolateJointMovement(RobotJointPosition towardsRobotJointPosition, ExternalJointPosition towardsExternalJointPosition) { // Calculate the joint position value change per interpolation RobotJointPosition robotJointPositionChange = (towardsRobotJointPosition - _lastRobotJointPosition) / _interpolations; ExternalJointPosition externalJointPositionChange = (towardsExternalJointPosition - _lastExternalJointPosition) / _interpolations; // Points for path List <Point3d> points = new List <Point3d>(); // New joint positions RobotJointPosition newRobotJointPosition = _lastRobotJointPosition; ExternalJointPosition newExternalJointPosition = _lastExternalJointPosition; // Interpolate for (int i = 0; i < _interpolations; i++) { _robotJointPositions.Add(newRobotJointPosition.Duplicate()); _externalJointPositions.Add(newExternalJointPosition.Duplicate()); _robot.ForwardKinematics.Calculate(newRobotJointPosition, newExternalJointPosition); _planes.Add(_robot.ForwardKinematics.TCPPlane); if (i == 0) { points.Add(new Point3d(_robot.ForwardKinematics.TCPPlane.Origin)); } else if (points[points.Count - 1] != _robot.ForwardKinematics.TCPPlane.Origin) { points.Add(new Point3d(_robot.ForwardKinematics.TCPPlane.Origin)); } newRobotJointPosition += robotJointPositionChange; newExternalJointPosition += externalJointPositionChange; } // Add last point _robot.ForwardKinematics.Calculate(towardsRobotJointPosition, towardsExternalJointPosition); if (points[points.Count - 1] != _robot.ForwardKinematics.TCPPlane.Origin) { points.Add(_robot.ForwardKinematics.TCPPlane.Origin); } // Generate path curve if (points.Count > 1) { _paths.Add(Curve.CreateInterpolatedCurve(points, 3)); } else { _paths.Add(null); } // Set last joint positions _lastRobotJointPosition = towardsRobotJointPosition; _lastExternalJointPosition = towardsExternalJointPosition; }
/// <summary> /// Calculates the interpolated path of a joint movement from a Joint Target. /// </summary> /// <param name="movement"> The movement with as Target a Joint Target. </param> private void JointMovementFromJointTarget(Movement movement) { // Set the correct tool for this movement SetRobotTool(movement); // Get the joint target JointTarget jointTarget = movement.Target as JointTarget; // Get the final joint positions of this movement RobotJointPosition towardsRobotJointPosition = jointTarget.RobotJointPosition; ExternalJointPosition towardsExternalJointPosition = jointTarget.ExternalJointPosition; // Add error text _errorText.AddRange(jointTarget.CheckAxisLimits(_robot).ConvertAll(item => string.Copy(item))); // Interpolate InterpolateJointMovement(towardsRobotJointPosition, towardsExternalJointPosition); }
/// <summary> /// Initializes a new instance of the Inverse Kinematics class by duplicating an existing Inverse Kinematics instance. /// </summary> /// <param name="inverseKinematics"> The Inverse Kinematics instance to duplicate. </param> public InverseKinematics(InverseKinematics inverseKinematics) { _robot = inverseKinematics.Robot.Duplicate(); _movement = inverseKinematics.Movement.Duplicate(); for (int i = 0; i < 8; i++) { _robotJointPositions[i] = new RobotJointPosition(); } Initialize(); _robotJointPosition = inverseKinematics.RobotJointPosition.Duplicate(); _externalJointPosition = inverseKinematics.ExternalJointPosition.Duplicate(); _errorText = new List <string>(inverseKinematics.ErrorText); _inLimits = inverseKinematics.InLimits; }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object can be used to retrieve data from input parameters and /// to store data in output parameters.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Input variables Robot robot = null; RobotJointPosition robotJointPosition = new RobotJointPosition(); ExternalJointPosition externalJointPosition = new ExternalJointPosition(); // Catch input data if (!DA.GetData(0, ref robot)) { return; } if (!DA.GetData(1, ref robotJointPosition)) { robotJointPosition = new RobotJointPosition(); } if (!DA.GetData(2, ref externalJointPosition)) { externalJointPosition = new ExternalJointPosition(); } // Calcuate the robot pose _fk = new ForwardKinematics(robot, robotJointPosition, externalJointPosition, _hideMesh); _fk.Calculate(); // Check the values for (int i = 0; i < _fk.ErrorText.Count; i++) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _fk.ErrorText[i]); } // Fill data tree with meshes if (_hideMesh == false) { _meshes = GetPosedMeshesDataTree(_fk); } // Output DA.SetDataTree(0, _meshes); DA.SetData(1, _fk.TCPPlane); DA.SetDataList(2, _fk.PosedExternalAxisPlanes); }
/// <summary> /// Initializes a new instance of the Forward Kinematics class by duplicating an existing Forward Kinematics instance. /// </summary> /// <param name="forwardKinematics"> The Forward Kinematics instance to duplicate. </param> public ForwardKinematics(ForwardKinematics forwardKinematics) { _robot = forwardKinematics.Robot.Duplicate(); _hideMesh = forwardKinematics.HideMesh; _robotJointPosition = forwardKinematics.RobotJointPosition.Duplicate(); _externalJointPosition = forwardKinematics.ExternalJointPosition.Duplicate(); _tcpPlane = new Plane(forwardKinematics.TCPPlane); _errorText = new List <string>(forwardKinematics.ErrorText); _posedInternalAxisMeshes = forwardKinematics.PosedInternalAxisMeshes.ConvertAll(mesh => mesh.DuplicateMesh()); _posedExternalAxisMeshes = new List <List <Mesh> >(forwardKinematics.PosedExternalAxisMeshes); for (int i = 0; i < forwardKinematics.PosedExternalAxisMeshes.Count; i++) { for (int j = 0; j < forwardKinematics.PosedExternalAxisMeshes[i].Count; j++) { forwardKinematics.PosedExternalAxisMeshes[i][j] = forwardKinematics.PosedExternalAxisMeshes[i][j].DuplicateMesh(); } } _posedExternalAxisPlanes = new List <Plane>(forwardKinematics.PosedExternalAxisPlanes).ToArray(); _inLimits = forwardKinematics.InLimits; }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object can be used to retrieve data from input parameters and to store data in output parameters.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Sets inputs and creates target string name = "defaultTar"; Plane plane = Plane.WorldXY; Plane referencePlane = Plane.WorldXY; int axisConfig = 0; ExternalJointPosition externalJointPosition = new ExternalJointPosition(); // Catch inputs if (!DA.GetData(0, ref name)) { return; } if (!DA.GetData(1, ref plane)) { return; } if (!DA.GetData(2, ref referencePlane)) { return; } if (!DA.GetData(3, ref axisConfig)) { return; } if (!DA.GetData(4, ref externalJointPosition)) { return; } // Replace spaces name = HelperMethods.ReplaceSpacesAndRemoveNewLines(name); RobotTarget target = new RobotTarget(name, plane, referencePlane, axisConfig, externalJointPosition); // Sets Output DA.SetData(0, target); }
/// <summary> /// Resets / clears the current solution. /// </summary> private void Reset() { // Clear current solution _robotJointPositions.Clear(); _externalJointPositions.Clear(); _planes.Clear(); _paths.Clear(); _errorText.Clear(); // Reinitiate starting values _currentTool = _initialTool; _linearConfigurationControl = true; _jointConfigurationControl = true; _firstMovementIsMoveAbsJ = false; _lastRobotJointPosition = new RobotJointPosition(); _lastExternalJointPosition = new ExternalJointPosition(); for (int i = 0; i < _robot.ExternalAxes.Count; i++) { _lastExternalJointPosition[_robot.ExternalAxes[i].AxisNumber] = _robot.ExternalAxes[i].AxisLimits.Min; } }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object can be used to retrieve data from input parameters and to store data in output parameters.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Declare variables double externalAxisValueA = 9e9; double externalAxisValueB = 9e9; double externalAxisValueC = 9e9; double externalAxisValueD = 9e9; double externalAxisValueE = 9e9; double externalAxisValueF = 9e9; // External axis A if (Params.Input.Any(x => x.Name == externalAxisParameters[0].Name)) { if (!DA.GetData(externalAxisParameters[0].Name, ref externalAxisValueA)) { externalAxisValueA = 9e9; } } // External axis B if (Params.Input.Any(x => x.Name == externalAxisParameters[1].Name)) { if (!DA.GetData(externalAxisParameters[1].Name, ref externalAxisValueB)) { externalAxisValueB = 9e9; } } // External axis C if (Params.Input.Any(x => x.Name == externalAxisParameters[2].Name)) { if (!DA.GetData(externalAxisParameters[2].Name, ref externalAxisValueC)) { externalAxisValueC = 9e9; } } // External axis D if (Params.Input.Any(x => x.Name == externalAxisParameters[3].Name)) { if (!DA.GetData(externalAxisParameters[3].Name, ref externalAxisValueD)) { externalAxisValueD = 9e9; } } // External axis E if (Params.Input.Any(x => x.Name == externalAxisParameters[4].Name)) { if (!DA.GetData(externalAxisParameters[4].Name, ref externalAxisValueE)) { externalAxisValueE = 9e9; } } // External axis F if (Params.Input.Any(x => x.Name == externalAxisParameters[5].Name)) { if (!DA.GetData(externalAxisParameters[5].Name, ref externalAxisValueF)) { externalAxisValueF = 9e9; } } // Create external joint position ExternalJointPosition extJointPosition = new ExternalJointPosition(externalAxisValueA, externalAxisValueB, externalAxisValueC, externalAxisValueD, externalAxisValueE, externalAxisValueF); // Sets Output DA.SetData(0, extJointPosition); }
/// <summary> /// Calculates the External Joint Position of the Inverse Kinematics solution. /// This method does not check the external axis limits. /// </summary> public void CalculateExternalJointPosition() { // Clear current solution ResetExternalJointPosition(); if (_target is RobotTarget robotTarget) { // NOTE: Only works for a robot with one external axis that moves the robot. double count = 0; // Counts the number of external axes that move the robot. // Calcualtes the position of the external axes for each external axis for (int i = 0; i < _robot.ExternalAxes.Count; i++) { ExternalAxis externalAxis = _robot.ExternalAxes[i]; Interval axisLimits = _robot.ExternalAxes[i].AxisLimits; int logic = externalAxis.AxisNumber; // External Linear axis if (externalAxis is ExternalLinearAxis externalLinearAxis) { // External Linear Axis that moves the robot if (externalLinearAxis.MovesRobot == true && count == 0) { // Checks if the position the external linear axis needs to be negative or positive externalLinearAxis.AxisCurve.ClosestPoint(_robot.BasePlane.Origin, out double robotBasePlaneParam); externalLinearAxis.AxisCurve.ClosestPoint(_positionPlane.Origin, out double basePlaneParam); if (basePlaneParam >= robotBasePlaneParam) { _externalJointPosition[logic] = _positionPlane.Origin.DistanceTo(_robot.BasePlane.Origin); } else { _externalJointPosition[logic] = -_positionPlane.Origin.DistanceTo(_robot.BasePlane.Origin); } count += 1; } // External Linear axis that does not move the robot else { if (robotTarget.ExternalJointPosition[logic] == 9e9) { _externalJointPosition[logic] = Math.Max(0, Math.Min(axisLimits.Min, axisLimits.Max)); } else { _externalJointPosition[logic] = robotTarget.ExternalJointPosition[logic]; } } } // External Rotational Axis else if (externalAxis is ExternalRotationalAxis) { if (robotTarget.ExternalJointPosition[logic] == 9e9) { _externalJointPosition[logic] = Math.Max(0, Math.Min(axisLimits.Min, axisLimits.Max)); } else { _externalJointPosition[logic] = robotTarget.ExternalJointPosition[logic]; } } // Other External Axis types else { if (robotTarget.ExternalJointPosition[logic] == 9e9) { _externalJointPosition[logic] = Math.Max(0, Math.Min(axisLimits.Min, axisLimits.Max)); } else { _externalJointPosition[logic] = robotTarget.ExternalJointPosition[logic]; } } } } // Joint Target else { _externalJointPosition = _target.ExternalJointPosition.Duplicate(); } }
/// <summary> /// Calculates the position of the attachment plane for a given External Joint Position. /// This calculations takes into account the external axis limits. /// If the defined External Joint Posiiton is outside its limits the closest valid external axis value will be used. /// </summary> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <returns> The posed attachement plane. </returns> public abstract Plane CalculatePositionSave(ExternalJointPosition externalJointPosition);
/// <summary> /// Calculates the the transformation matrix for a given External Joint Position. /// This calculation does not take into account the axis limits. /// </summary> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <param name="inLimits"> Specifies whether the External Joint Position is inside its limits. </param> /// <returns> The transformation matrix. </returns> public abstract Transform CalculateTransformationMatrix(ExternalJointPosition externalJointPosition, out bool inLimits);
/// <summary> /// Calculates the position of the attachment plane for a given External Joint Position. /// This calculation does not take into account the axis limits. /// </summary> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <param name="inLimits"> Specifies whether the External Joint Position is inside its limits. </param> /// <returns> The posed attachement plane. </returns> public abstract Plane CalculatePosition(ExternalJointPosition externalJointPosition, out bool inLimits);
/// <summary> /// Calculates the position of the external axis meshes for a given External Joint Position. /// </summary> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <returns> The posed meshes. </returns> public abstract List <Mesh> PoseMeshes(ExternalJointPosition externalJointPosition);
/// <summary> /// Calculates the the transformation matrix for a given External Joint Position. /// This calculations takes into account the external axis limits. /// If the defined External Joint Posiiton is outside its limits the closest valid external axis value will be used. /// </summary> /// <param name="externalJointPosition"> The External Joint Position. </param> /// <returns> The transformation matrix. </returns> public abstract Transform CalculateTransformationMatrixSave(ExternalJointPosition externalJointPosition);
/// <summary> /// Calculates the interpolated path for a linear movement. /// </summary> /// <param name="movement"> The movement as a linear movement type. </param> private void LinearMovementFromRobotTarget(Movement movement) { // Set the correct tool for this movement SetRobotTool(movement); // Points for path List <Point3d> points = new List <Point3d>(); // Get the final joint positions of this movement _robot.InverseKinematics.Movement = movement; _robot.InverseKinematics.CalculateExternalJointPosition(); // Get the External Joint Positions ExternalJointPosition towardsExternalJointPosition = _robot.InverseKinematics.ExternalJointPosition.Duplicate(); // External Joint Position change ExternalJointPosition externalJointPositionChange = (towardsExternalJointPosition - _lastExternalJointPosition) / _interpolations; // TODO: Check with last movement to speed up the process? As in old path generator? // First target plane in WORLD coordinate space _robot.ForwardKinematics.Calculate(_lastRobotJointPosition, _lastExternalJointPosition); Plane plane1 = _robot.ForwardKinematics.TCPPlane; // Second target plane in WORK OBJECT coordinate space RobotTarget robotTarget = movement.Target as RobotTarget; Plane plane2 = robotTarget.Plane; // Correction for rotation of the target plane on a movable work object if (movement.WorkObject.ExternalAxis != null) { ExternalAxis externalAxis = movement.WorkObject.ExternalAxis; Transform trans = externalAxis.CalculateTransformationMatrix(_lastExternalJointPosition * -1, out _); plane1.Transform(trans); } // Re-orient the starting plane to the work object coordinate space of the second target plane Plane globalWorkObjectPlane = new Plane(movement.WorkObject.GlobalWorkObjectPlane); Transform orient = Transform.ChangeBasis(Plane.WorldXY, globalWorkObjectPlane); plane1.Transform(orient); // Target plane position and orientation change per interpolation step Vector3d posChange = (plane2.Origin - plane1.Origin) / _interpolations; Vector3d xAxisChange = (plane2.XAxis - plane1.XAxis) / _interpolations; Vector3d yAxisChange = (plane2.YAxis - plane1.YAxis) / _interpolations; // Correct axis configuration, tool and work object RobotTarget subTarget = new RobotTarget(robotTarget.Name, Plane.WorldXY, robotTarget.AxisConfig); Movement subMovement = new Movement(subTarget); subMovement.RobotTool = movement.RobotTool; subMovement.WorkObject = movement.WorkObject; // New external joint position ExternalJointPosition newExternalJointPosition = _lastExternalJointPosition; // Create the sub target planes, robot joint positions and external joint positions for every interpolation step for (int i = 0; i < _interpolations; i++) { // Create new plane: the local target plane (in work object coordinate space) Plane plane = new Plane(plane1.Origin + posChange * i, plane1.XAxis + xAxisChange * i, plane1.YAxis + yAxisChange * i); // Update the target and movement subTarget.Plane = plane; subTarget.ExternalJointPosition = newExternalJointPosition; subMovement.Target = subTarget; // Calculate joint positions _robot.InverseKinematics.Movement = subMovement; _robot.InverseKinematics.Calculate(); // Auto Axis Config if (_linearConfigurationControl == false) { if (i == 0) { _robot.InverseKinematics.CalculateClosestRobotJointPosition(_lastRobotJointPosition); } else { _robot.InverseKinematics.CalculateClosestRobotJointPosition(_robotJointPositions.Last()); } } // Add te calculated joint positions and plane to the class property _robotJointPositions.Add(_robot.InverseKinematics.RobotJointPosition.Duplicate()); _externalJointPositions.Add(_robot.InverseKinematics.ExternalJointPosition.Duplicate()); // Add error messages (check axis limits) _errorText.AddRange(_robot.InverseKinematics.ErrorText.ConvertAll(item => string.Copy(item))); // Add the plane Plane globalPlane = subMovement.GetPosedGlobalTargetPlane(); _planes.Add(globalPlane); // Always add the first point to list with paths if (i == 0) { points.Add(new Point3d(globalPlane.Origin)); } // Only add the other point if this point is different else if (points[points.Count - 1] != globalPlane.Origin) { points.Add(new Point3d(globalPlane.Origin)); } // Update the external joint position newExternalJointPosition += externalJointPositionChange; } // Add last point Point3d lastPoint = movement.GetPosedGlobalTargetPlane().Origin; if (points[points.Count - 1] != lastPoint) { points.Add(lastPoint); } // Generate path curve if (points.Count > 1) { _paths.Add(Curve.CreateInterpolatedCurve(points, 3)); } else { _paths.Add(null); } // Get the final joint positions of this movement _robot.InverseKinematics.Movement = movement; _robot.InverseKinematics.Calculate(); // Auto Axis Config if (_linearConfigurationControl == false) { _robot.InverseKinematics.CalculateClosestRobotJointPosition(_robotJointPositions.Last()); } // Add error messages (check axis limits) _errorText.AddRange(_robot.InverseKinematics.ErrorText.ConvertAll(item => string.Copy(item))); // Add last Joint Poistions _lastRobotJointPosition = _robot.InverseKinematics.RobotJointPosition.Duplicate(); _lastExternalJointPosition = _robot.InverseKinematics.ExternalJointPosition.Duplicate(); }
/// <summary> /// This is the method that actually does the work. /// </summary> /// <param name="DA">The DA object can be used to retrieve data from input parameters and to store data in output parameters.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Sets inputs and creates target List <string> names = new List <string>(); List <Plane> planes = new List <Plane>(); List <Plane> referencePlanes = new List <Plane>(); List <int> axisConfigs = new List <int>(); List <ExternalJointPosition> externalJointPositions = new List <ExternalJointPosition>(); // Catch name input if (!DA.GetDataList(0, names)) { return; } // Fixed index // Catch target planes input if (!DA.GetDataList(1, planes)) { return; } // Fixed index // Catch reference plane input if (Params.Input.Any(x => x.Name == "Reference Plane")) { if (!DA.GetDataList("Reference Plane", referencePlanes)) { referencePlanes = new List <Plane>() { Plane.WorldXY }; } } // Catch axis configuration input if (Params.Input.Any(x => x.Name == "Axis Configuration")) { if (!DA.GetDataList("Axis Configuration", axisConfigs)) { return; } } // Catch input data for setting the external joint position if (Params.Input.Any(x => x.Name == parameters[1].Name)) { if (!DA.GetDataList(parameters[1].Name, externalJointPositions)) { externalJointPositions = new List <ExternalJointPosition>() { new ExternalJointPosition() }; } } // Make sure variable input has a default value if (referencePlanes.Count == 0) { referencePlanes.Add(Plane.WorldXY); } if (externalJointPositions.Count == 0) { externalJointPositions.Add(new ExternalJointPosition()); } // Replace spaces names = HelperMethods.ReplaceSpacesAndRemoveNewLines(names); // Get longest Input List int[] sizeValues = new int[5]; sizeValues[0] = names.Count; sizeValues[1] = planes.Count; sizeValues[2] = referencePlanes.Count; sizeValues[3] = axisConfigs.Count; sizeValues[4] = externalJointPositions.Count; int biggestSize = HelperMethods.GetBiggestValue(sizeValues); // Keeps track of used indicies int nameCounter = -1; int planesCounter = -1; int referencePlaneCounter = -1; int axisConfigCounter = -1; int externalJointPositionCounter = -1; // Clear list _targets.Clear(); // Creates targets for (int i = 0; i < biggestSize; i++) { string name = ""; Plane plane = new Plane(); Plane referencePlane = new Plane(); int axisConfig = 0; ExternalJointPosition externalJointPosition = new ExternalJointPosition(); // Names counter if (i < sizeValues[0]) { name = names[i]; nameCounter++; } else { name = names[nameCounter] + "_" + (i - nameCounter); } // Target planes counter if (i < sizeValues[1]) { plane = planes[i]; planesCounter++; } else { plane = planes[planesCounter]; } // Reference plane counter if (i < sizeValues[2]) { referencePlane = referencePlanes[i]; referencePlaneCounter++; } else { referencePlane = referencePlanes[referencePlaneCounter]; } // Axis configuration counter if (i < sizeValues[3]) { axisConfig = axisConfigs[i]; axisConfigCounter++; } else { axisConfig = axisConfigs[axisConfigCounter]; } // External Joint Position if (i < sizeValues[4]) { externalJointPosition = externalJointPositions[i]; externalJointPositionCounter++; } else { externalJointPosition = externalJointPositions[externalJointPositionCounter]; } RobotTarget target = new RobotTarget(name, plane, referencePlane, axisConfig, externalJointPosition); _targets.Add(target); } // Sets Output DA.SetDataList(0, _targets); #region Object manager // Gets ObjectManager of this document _objectManager = DocumentManager.GetDocumentObjectManager(this.OnPingDocument()); // Clears targetNames for (int i = 0; i < _targetNames.Count; i++) { _objectManager.TargetNames.Remove(_targetNames[i]); } _targetNames.Clear(); // Removes lastName from targetNameList if (_objectManager.TargetNames.Contains(_lastName)) { _objectManager.TargetNames.Remove(_lastName); } // Adds Component to TargetByGuid Dictionary if (!_objectManager.OldRobotTargetsByGuid.ContainsKey(this.InstanceGuid)) { _objectManager.OldRobotTargetsByGuid.Add(this.InstanceGuid, this); //TODO } // Checks if target name is already in use and counts duplicates #region Check name in object manager _namesUnique = true; for (int i = 0; i < names.Count; i++) { if (_objectManager.TargetNames.Contains(names[i])) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Target Name already in use."); _namesUnique = false; _lastName = ""; break; } else { // Adds Target Name to list _targetNames.Add(names[i]); _objectManager.TargetNames.Add(names[i]); // Run SolveInstance on other Targets with no unique Name to check if their name is now available _objectManager.UpdateTargets(); _lastName = names[i]; } // Checks if variable name exceeds max character limit for RAPID Code if (HelperMethods.VariableExeedsCharacterLimit32(names[i])) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Target Name exceeds character limit of 32 characters."); break; } // Checks if variable name starts with a number if (HelperMethods.VariableStartsWithNumber(names[i])) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Target Name starts with a number which is not allowed in RAPID Code."); break; } } #endregion // Recognizes if Component is Deleted and removes it from Object Managers target and name list GH_Document doc = this.OnPingDocument(); if (doc != null) { doc.ObjectsDeleted += DocumentObjectsDeleted; } #endregion }
/// <summary> /// Calculates the forward kinematics solution with the given Joint Positions. /// </summary> /// <param name="robotJointPosition"> The Robot Joint Position. </param> /// <param name="externalJointPosition"> The External Joint Position. </param> public void Calculate(RobotJointPosition robotJointPosition, ExternalJointPosition externalJointPosition) { _robotJointPosition = robotJointPosition; _externalJointPosition = externalJointPosition; Calculate(); }