/// <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> /// 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> /// 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 is used to retrieve from inputs and store in outputs.</param> protected override void SolveInstance(IGH_DataAccess DA) { // Input variables RobotJointPosition robJointPosition = null; // Catch the input data if (!DA.GetData(0, ref robJointPosition)) { return; } // Check if the object is valid if (!robJointPosition.IsValid) { AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "The Robot Joint Position is not valid"); } // Output DA.SetData(0, robJointPosition[0]); DA.SetData(1, robJointPosition[1]); DA.SetData(2, robJointPosition[2]); DA.SetData(3, robJointPosition[3]); DA.SetData(4, robJointPosition[4]); DA.SetData(5, robJointPosition[5]); }
/// <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; }
private ExternalJointPosition _externalJointPosition = new ExternalJointPosition(); // Contains the final solution #endregion #region constructors /// <summary> /// Initializes an empty instance of the Inverse Kinematics class. /// </summary> public InverseKinematics() { for (int i = 0; i < 8; i++) { _robotJointPositions[i] = new RobotJointPosition(); } }
/// <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> /// Initializes a new instance of the Inverse Kinematics class from a Movement. /// </summary> /// <param name="movement"> The Movement. </param> /// <param name="robot"> The Robot. </param> public InverseKinematics(Movement movement, Robot robot) { _robot = robot; _movement = movement; for (int i = 0; i < 8; i++) { _robotJointPositions[i] = new RobotJointPosition(); } Initialize(); }
/// <summary> /// Initializes a new instance of the Inverse Kinematics class from a Target. /// The target will be casted to robot movement with a default work object (wobj0). /// </summary> /// <param name="target"> The Target </param> /// <param name="robot"> The Robot. </param> public InverseKinematics(ITarget target, Robot robot) { _robot = robot; _movement = new Movement(target); for (int i = 0; i < 8; i++) { _robotJointPositions[i] = new RobotJointPosition(); } Initialize(); }
/// <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> /// 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> /// 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> /// 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 double internalAxisValue1 = 0.0; double internalAxisValue2 = 0.0; double internalAxisValue3 = 0.0; double internalAxisValue4 = 0.0; double internalAxisValue5 = 0.0; double internalAxisValue6 = 0.0; // Catch input data if (!DA.GetData(0, ref internalAxisValue1)) { return; } if (!DA.GetData(1, ref internalAxisValue2)) { return; } if (!DA.GetData(2, ref internalAxisValue3)) { return; } if (!DA.GetData(3, ref internalAxisValue4)) { return; } if (!DA.GetData(4, ref internalAxisValue5)) { return; } if (!DA.GetData(5, ref internalAxisValue6)) { return; } // Create external joint position RobotJointPosition robJointPosition = new RobotJointPosition(internalAxisValue1, internalAxisValue2, internalAxisValue3, internalAxisValue4, internalAxisValue5, internalAxisValue6); // Sets Output DA.SetData(0, robJointPosition); }
/// <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> /// 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> /// 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> /// 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(); }
/// <summary> /// Calculates the Robot Joint Position of the Inverse Kinematics solution. /// This method does not check the internal axis limits. /// </summary> public void CalculateRobotJointPosition() { // Clear the current solutions before calculating a new ones. ResetRobotJointPositions(); // Tracks the index of the axis configuration int index1 = 0; int index2 = 0; if (_target is RobotTarget robotTarget) { #region wrist center relative to axis 1 // Note that this is reversed because the clockwise direction when looking // down at the XY plane is typically taken as the positive direction for robot axis1 // Caculate the position of robot axis 1: Wrist center relative to axis 1 in front of robot (configuration 0, 1, 2, 3) double internalAxisValue1 = -1 * Math.Atan2(_wrist.Y, _wrist.X); if (internalAxisValue1 > Math.PI) { internalAxisValue1 -= 2 * Math.PI; } _robotJointPositions[0][0] = internalAxisValue1; _robotJointPositions[1][0] = internalAxisValue1; _robotJointPositions[2][0] = internalAxisValue1; _robotJointPositions[3][0] = internalAxisValue1; // Rotate the joint position 180 degrees (pi radians): Wrist center relative to axis 1 behind robot (configuration 4, 5, 6, 7) internalAxisValue1 += Math.PI; if (internalAxisValue1 > Math.PI) { internalAxisValue1 -= 2 * Math.PI; } _robotJointPositions[4][0] = internalAxisValue1; _robotJointPositions[5][0] = internalAxisValue1; _robotJointPositions[6][0] = internalAxisValue1; _robotJointPositions[7][0] = internalAxisValue1; #endregion // Generates 4 sets of values for each option of axis 1 // i = 0: Wrist center relative to axis 1 in front of robot (configuration 0, 1, 2, 3) // i = 1: Wrist center relative to axis 1 behind robot (configuration 4, 5, 6, 7) for (int i = 0; i < 2; i++) { // Get the position of robot axis 1 internalAxisValue1 = _robotJointPositions[i * 4][0]; // Get the elbow points Point3d internalAxisPoint1 = new Point3d(_axisPlanes[1].Origin); Point3d internalAxisPoint2 = new Point3d(_axisPlanes[2].Origin); Point3d internalAxisPoint4 = new Point3d(_axisPlanes[4].Origin); // Rotate the points to the correction position Transform rot1 = Transform.Rotation(-1 * internalAxisValue1, Point3d.Origin); internalAxisPoint1.Transform(rot1); internalAxisPoint2.Transform(rot1); internalAxisPoint4.Transform(rot1); // Create the elbow projection plane Vector3d elbowDir = new Vector3d(1, 0, 0); elbowDir.Transform(rot1); Plane elbowPlane = new Plane(internalAxisPoint1, elbowDir, Vector3d.ZAxis); Sphere sphere1 = new Sphere(internalAxisPoint1, _lowerArmLength); Sphere sphere2 = new Sphere(_wrist, _upperArmLength); Intersection.SphereSphere(sphere1, sphere2, out Circle circ); Intersection.PlaneCircle(elbowPlane, circ, out double par1, out double par2); Point3d intersectPt1 = circ.PointAt(par1); Point3d intersectPt2 = circ.PointAt(par2); // Calculates the position of robot axis 2 and 3 for (int j = 0; j < 2; j++) { // Calculate elbow and wrist variables Point3d elbowPoint; if (j == 1) { elbowPoint = intersectPt1; } else { elbowPoint = intersectPt2; } elbowPlane.ClosestParameter(elbowPoint, out double elbowX, out double elbowY); elbowPlane.ClosestParameter(_wrist, out double wristX, out double wristY); // Calculate the position of robot axis 2 double internalAxisValue2 = Math.Atan2(elbowY, elbowX); double internalAxisValue3 = Math.PI - internalAxisValue2 + Math.Atan2(wristY - elbowY, wristX - elbowX) - _axis4offsetAngle; for (int k = 0; k < 2; k++) { // Adds the position of robot axis 2 _robotJointPositions[index1][1] = -internalAxisValue2; // Calculate the position of robot axis 3 double internalAxisValue3Wrapped = -internalAxisValue3 + Math.PI; while (internalAxisValue3Wrapped >= Math.PI) { internalAxisValue3Wrapped -= 2 * Math.PI; } while (internalAxisValue3Wrapped < -Math.PI) { internalAxisValue3Wrapped += 2 * Math.PI; } _robotJointPositions[index1][2] = internalAxisValue3Wrapped; // Update in index tracker index1++; } for (int k = 0; k < 2; k++) { // Calculate the position of robot axis 4 Vector3d axis4 = new Vector3d(_wrist - elbowPoint); axis4.Rotate(-_axis4offsetAngle, elbowPlane.ZAxis); Plane tempPlane = new Plane(elbowPlane); tempPlane.Rotate(internalAxisValue2 + internalAxisValue3, tempPlane.ZAxis); Plane internalAxisPlane4 = new Plane(_wrist, tempPlane.ZAxis, -1.0 * tempPlane.YAxis); internalAxisPlane4.ClosestParameter(_endPlane.Origin, out double axis6X, out double axis6Y); double internalAxisValue4 = Math.Atan2(axis6Y, axis6X); if (k == 1) { internalAxisValue4 += Math.PI; if (internalAxisValue4 > Math.PI) { internalAxisValue4 -= 2 * Math.PI; } } double internalAxisValue4Wrapped = internalAxisValue4 + Math.PI / 2; while (internalAxisValue4Wrapped >= Math.PI) { internalAxisValue4Wrapped -= 2 * Math.PI; } while (internalAxisValue4Wrapped < -Math.PI) { internalAxisValue4Wrapped += 2 * Math.PI; } _robotJointPositions[index2][3] = internalAxisValue4Wrapped; // Calculate the position of robot axis 5 Plane internalAxisPlane5 = new Plane(internalAxisPlane4); internalAxisPlane5.Rotate(internalAxisValue4, internalAxisPlane4.ZAxis); internalAxisPlane5 = new Plane(_wrist, -internalAxisPlane5.ZAxis, internalAxisPlane5.XAxis); internalAxisPlane5.ClosestParameter(_endPlane.Origin, out axis6X, out axis6Y); double internalAxisValue5 = Math.Atan2(axis6Y, axis6X); _robotJointPositions[index2][4] = internalAxisValue5; // Calculate the position of robot axis 6 Plane internalAxisPlane6 = new Plane(internalAxisPlane5); internalAxisPlane6.Rotate(internalAxisValue5, internalAxisPlane5.ZAxis); internalAxisPlane6 = new Plane(_wrist, -internalAxisPlane6.YAxis, internalAxisPlane6.ZAxis); internalAxisPlane6.ClosestParameter(_endPlane.PointAt(0, -1), out double endX, out double endY); double internalAxisValue6 = Math.Atan2(endY, endX); _robotJointPositions[index2][5] = internalAxisValue6; // Update index tracker index2++; } } } // Corrections for (int i = 0; i < 8; i++) { // From radians to degrees _robotJointPositions[i] *= (180 / Math.PI); // Other corrections _robotJointPositions[i][0] *= -1; _robotJointPositions[i][1] += 90; _robotJointPositions[i][2] -= 90; _robotJointPositions[i][3] *= -1; if (_robotJointPositions[i][5] <= 0) { _robotJointPositions[i][5] = -180 - _robotJointPositions[i][5]; } else { _robotJointPositions[i][5] = 180 - _robotJointPositions[i][5]; } } // Select solution _robotJointPosition = _robotJointPositions[robotTarget.AxisConfig]; } else if (_target is JointTarget jointTarget) { _robotJointPosition = jointTarget.RobotJointPosition; } else { _robotJointPosition = new RobotJointPosition(); } }
/// <summary> /// Calculates and returns the closest Robot Joint Position to a given previous Robot Joint Position. /// This methods sets and returns the closest Robot Joint Poistion insides this Inverse Kinematics object. /// You first have to calculate the Inverse Kinematics solution before you call this method. /// This method is typically used for using the Auto Axis Config inside the Path Generator. /// </summary> /// <param name="prevJointPosition"> The previous Robot Joint Position. </param> /// <returns> The closest Robot Joint Position. </returns> public RobotJointPosition CalculateClosestRobotJointPosition(RobotJointPosition prevJointPosition) { RobotJointPosition diff; double sum; // First, check the selected axis configuration diff = _robotJointPosition - prevJointPosition; for (int i = 0; i < 6; i++) { diff[i] = Math.Sqrt(diff[i] * diff[i]); } double min = diff.Sum(); _robotJointPosition = _robotJointPosition.Duplicate(); // Check for flipping axis 4 and 6 (if this is within the axis limits) double[] joint4 = new double[3] { -360, 0, 360 }; double[] joint6 = new double[3] { -360, 0, 360 }; for (int i = 0; i < _robotJointPositions.Length; i++) { for (int j = 0; j < joint4.Length; j++) { // Check axis 4 if (_robot.InternalAxisLimits[3].IncludesParameter(_robotJointPositions[i][3] + joint4[j], false) == true) { // Add value to axis 4 _robotJointPositions[i][3] += joint4[j]; for (int k = 0; k < joint6.Length; k++) { // Check axis 6 if (_robot.InternalAxisLimits[5].IncludesParameter(_robotJointPositions[i][5] + joint6[k], false) == true) { // Add value to axis 6 _robotJointPositions[i][5] += joint6[k]; // Check the configuration (min. rotation) diff = _robotJointPositions[i] - prevJointPosition; for (int l = 0; l < 6; l++) { diff[l] = Math.Sqrt(diff[l] * diff[l]); } sum = diff.Sum(); if (sum < min) { _robotJointPosition = _robotJointPositions[i].Duplicate(); min = sum; } // Reset axis 6 (substract value from axis 6) _robotJointPositions[i][5] -= joint6[k]; } } // Reset axis 4 (substract value from axis 4) _robotJointPositions[i][3] -= joint4[j]; } } } // Check axis limits _errorText.Clear(); CheckInternalAxisLimits(); CheckExternalAxisLimits(); return(_robotJointPosition); }