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