/// <summary>
/// Divides an External Joint Positin with by an other External Joint Position
/// </summary>
/// <param name="p1"> The first External Joint Position. </param>
/// <param name="p2"> The second External Joint Position. </param>
/// <returns> The first External Joint Position divided by the second External Joint Position. </returns>
public static ExternalJointPosition operator /(ExternalJointPosition p1, ExternalJointPosition p2)
{
if (p2[0] == 0 || p2[1] == 0 || p2[2] == 0 || p2[3] == 0 || p2[4] == 0 || p2[5] == 0)
{
throw new DivideByZeroException();
}
ExternalJointPosition result = new ExternalJointPosition();
for (int i = 0; i < 6; i++)
{
if (p1[i] != _defaultValue && p2[i] != _defaultValue)
{
result[i] = p1[i] / p2[i];
}
else if (p1[i] == _defaultValue && p2[i] == _defaultValue)
{
result[i] = _defaultValue;
}
else
{
throw new InvalidOperationException(String.Format("Mismatch between two External Joint Positions. A definied joint position [on index {0}] is combined with an undefinied joint position.", i));
}
}
return(result);
}
/// <summary>
/// Initializes a new instance of the Joint Target class.
/// </summary>
/// <param name="name"> The target name, must be unique.</param>
/// <param name="robotJointPosition"> The Robot Joint Position</param>
/// <param name="externalJointPosition"> The External Joint Position</param>
public JointTarget(string name, RobotJointPosition robotJointPosition, ExternalJointPosition externalJointPosition)
{
_referenceType = ReferenceType.VAR;
_name = name;
_robotJointPosition = robotJointPosition;
_externalJointPosition = externalJointPosition;
}
/// <summary>
/// Initializes a new instance of the Joint Target class by duplicating an existing Joint Target instance.
/// </summary>
/// <param name="jointTarget"> The Joint Target instance to duplicate. </param>
public JointTarget(JointTarget jointTarget)
{
_referenceType = jointTarget.ReferenceType;
_name = jointTarget.Name;
_robotJointPosition = jointTarget.RobotJointPosition.Duplicate();
_externalJointPosition = jointTarget.ExternalJointPosition.Duplicate();
}
private ExternalJointPosition _externalJointPosition; // the position of the external logical axes
#endregion
#region (de)serialization
/// <summary>
/// Protected constructor needed for deserialization of the object.
/// </summary>
/// <param name="info"> The SerializationInfo to extract the data from. </param>
/// <param name="context"> The context of this deserialization. </param>
protected JointTarget(SerializationInfo info, StreamingContext context)
{
// int version = (int)info.GetValue("Version", typeof(int)); // <-- use this if the (de)serialization changes
_referenceType = (ReferenceType)info.GetValue("Reference Type", typeof(ReferenceType));
_name = (string)info.GetValue("Name", typeof(string));
_robotJointPosition = (RobotJointPosition)info.GetValue("Robot Joint Position", typeof(RobotJointPosition));
_externalJointPosition = (ExternalJointPosition)info.GetValue("External Joint Position", typeof(ExternalJointPosition));
}
/// <summary>
/// Initializes a new instance of the External Joint Position class by duplicating an existing External Joint Position instance.
/// </summary>
/// <param name="externalJointPosition"> The External Joint Position instance to duplicate. </param>
public ExternalJointPosition(ExternalJointPosition externalJointPosition)
{
_val1 = externalJointPosition[0];
_val2 = externalJointPosition[1];
_val3 = externalJointPosition[2];
_val4 = externalJointPosition[3];
_val5 = externalJointPosition[4];
_val6 = externalJointPosition[5];
}
/// <summary>
/// Creates declarations in the RAPID program module inside the RAPID Generator.
/// This method is called inside the RAPID generator.
/// </summary>
/// <param name="RAPIDGenerator"> The RAPID Generator. </param>
public override void ToRAPIDDeclaration(RAPIDGenerator RAPIDGenerator)
{
// Generate the code for the zone and speeddata
_speedData.ToRAPIDDeclaration(RAPIDGenerator);
_zoneData.ToRAPIDDeclaration(RAPIDGenerator);
// Generate code from robot targets
if (_target is RobotTarget robotTarget)
{
// Update the movement of the inverse kinematics
RAPIDGenerator.Robot.InverseKinematics.Movement = this;
// Generates the robot target variable for a MoveL or MoveJ instruction
if (_movementType == MovementType.MoveL || _movementType == MovementType.MoveJ)
{
RAPIDGenerator.Robot.InverseKinematics.CalculateExternalJointPosition();
robotTarget.ToRAPIDDeclaration(RAPIDGenerator);
}
// Generates the joint target variable from a robot target for a MoveAbsJ instruction
else
{
if (!RAPIDGenerator.Targets.ContainsKey(robotTarget.Name + "_jt"))
{
// Calculate the axis values from the robot target
RAPIDGenerator.Robot.InverseKinematics.Calculate();
RAPIDGenerator.ErrorText.AddRange(new List <string>(RAPIDGenerator.Robot.InverseKinematics.ErrorText));
// Create a joint target from the axis values
RobotJointPosition robJointPosition = RAPIDGenerator.Robot.InverseKinematics.RobotJointPosition.Duplicate();
ExternalJointPosition extJointPosition = RAPIDGenerator.Robot.InverseKinematics.ExternalJointPosition.Duplicate();
JointTarget jointTarget = new JointTarget(robotTarget.Name + "_jt", robJointPosition, extJointPosition);
jointTarget.ReferenceType = _target.ReferenceType;
// Create the RAPID code
jointTarget.ToRAPIDDeclaration(RAPIDGenerator);
}
}
}
// Generate code from joint targets
else if (_target is JointTarget jointTarget)
{
// JointTarget with MoveAbsJ
if (_movementType == MovementType.MoveAbsJ)
{
jointTarget.ToRAPIDDeclaration(RAPIDGenerator);
RAPIDGenerator.ErrorText.AddRange(jointTarget.CheckAxisLimits(RAPIDGenerator.Robot));
}
// Joint Target combined with MoveL or MoveJ
else
{
throw new InvalidOperationException("Invalid Move instruction: A Joint Target cannot be combined with a MoveL or MoveJ instruction.");
}
}
}
/// <summary>
/// Initializes a new instance of the Robot Target class with an axis conguration set to zero and an undefined External Joint Position.
/// </summary>
/// <param name="name"> The target name, must be unique. </param>
/// <param name="plane"> The target plane. </param>
public RobotTarget(string name, Plane plane)
{
_referenceType = ReferenceType.VAR;
_name = name;
_plane = plane;
_axisConfig = 0;
_externalJointPosition = new ExternalJointPosition();
_quat = HelperMethods.PlaneToQuaternion(_plane);
}
/// <summary>
/// Initializes a new instance of the Robot Target class by duplicating an existing Robot Target instance.
/// </summary>
/// <param name="target"> The Robot Target instance to duplicate. </param>
public RobotTarget(RobotTarget target)
{
_referenceType = target.ReferenceType;
_name = target.Name;
_plane = new Plane(target.Plane);
_axisConfig = target.AxisConfig;
_externalJointPosition = target.ExternalJointPosition;
_quat = target.Quat;
}
private ExternalJointPosition _externalJointPosition; // the position of the external logical axes
#endregion
#region (de)serialization
/// <summary>
/// Protected constructor needed for deserialization of the object.
/// </summary>
/// <param name="info"> The SerializationInfo to extract the data from. </param>
/// <param name="context"> The context of this deserialization. </param>
protected RobotTarget(SerializationInfo info, StreamingContext context)
{
// int version = (int)info.GetValue("Version", typeof(int)); // <-- use this if the (de)serialization changes
_referenceType = (ReferenceType)info.GetValue("Reference Type", typeof(ReferenceType));
_name = (string)info.GetValue("Name", typeof(string));
_plane = (Plane)info.GetValue("Plane", typeof(Plane));
_axisConfig = (int)info.GetValue("Axis Configuration", typeof(int));
_externalJointPosition = (ExternalJointPosition)info.GetValue("External Joint Position", typeof(ExternalJointPosition));
_quat = HelperMethods.PlaneToQuaternion(_plane);
}
/// <summary>
/// Mutliplies all the values inside the External Joint Position by a number.
/// </summary>
/// <param name="p"> The External Joint Position. </param>
/// <param name="num"> The value to multiply by. </param>
/// <returns> The External Joint Position with multuplied values. </returns>
public static ExternalJointPosition operator *(ExternalJointPosition p, double num)
{
ExternalJointPosition externalJointPosition = new ExternalJointPosition();
for (int i = 0; i < 6; i++)
{
if (p[i] != _defaultValue)
{
externalJointPosition[i] = p[i] * num;
}
}
return(externalJointPosition);
}
/// <summary>
/// Initializes a new instance of the Robot Target class.
/// The target planes will be reoriented from the reference plane to the world xy-plane.
/// </summary>
/// <param name="name"> The target name, must be unique.</param>
/// <param name="plane"> The target plane.</param>
/// <param name="referencePlane"> The Reference plane. </param>
/// <param name="axisConfig"> The axis configuration as a number (0-7).</param>
/// <param name="externalJointPosition"> The External Joint Position. </param>
public RobotTarget(string name, Plane plane, Plane referencePlane, int axisConfig, ExternalJointPosition externalJointPosition)
{
_referenceType = ReferenceType.VAR;
_name = name;
_plane = plane;
_axisConfig = axisConfig;
_externalJointPosition = externalJointPosition;
_quat = HelperMethods.PlaneToQuaternion(referencePlane, _plane);
// Re-orient the plane to the reference plane
Transform orient = Transform.PlaneToPlane(referencePlane, Plane.WorldXY);
_plane.Transform(orient);
}
/// <summary>
/// Initializes a new instance of the Robot Target class with an undefined Extenal Joint Position.
/// The target planes will be reoriented from the reference plane to the world xy-plane.
/// </summary>
/// <param name="name"> The target name, must be unique. </param>
/// <param name="plane"> The target plane. </param>
/// <param name="referencePlane"> The reference plane. </param>
/// <param name="axisConfig"> The axis configuration as a number (0-7). </param>
public RobotTarget(string name, Plane plane, Plane referencePlane, int axisConfig)
{
_referenceType = ReferenceType.VAR;
_name = name;
_plane = plane;
_axisConfig = axisConfig;
_externalJointPosition = new ExternalJointPosition();
_quat = HelperMethods.PlaneToQuaternion(referencePlane, _plane);
// Re-orient the plane from the reference coordinate system to the world coordinate system
Transform orient = Transform.PlaneToPlane(referencePlane, Plane.WorldXY);
_plane.Transform(orient);
}
/// <summary>
/// Multiplies the values inside this Joint Position with the values from another External Joint Position.
/// Value 1 * value 1, value 2 * value 2, value 3 * value 3 etc.
/// </summary>
/// <param name="jointPosition"> The multiplier as an External Joint Position. </param>
public void Multiply(ExternalJointPosition jointPosition)
{
for (int i = 0; i < 6; i++)
{
if (this[i] != _defaultValue && jointPosition[i] != _defaultValue)
{
this[i] *= jointPosition[i];
}
else if (this[i] == _defaultValue && this[i] == _defaultValue)
{
this[i] = _defaultValue;
}
else
{
throw new InvalidOperationException(String.Format("Mismatch between two External Joint Positions. A definied joint position [on index {0}] is combined with an undefinied joint position.", i));
}
}
}
/// <summary>
/// Divides all the values inside the External Joint Position by a number.
/// </summary>
/// <param name="p"> The External Joint Position. </param>
/// <param name="num"> The number to divide by. </param>
/// <returns> The External Joint Position with divide values. </returns>
public static ExternalJointPosition operator /(ExternalJointPosition p, double num)
{
if (num == 0)
{
throw new DivideByZeroException();
}
ExternalJointPosition externalJointPosition = new ExternalJointPosition();
for (int i = 0; i < 6; i++)
{
if (p[i] != _defaultValue)
{
externalJointPosition[i] = p[i] / num;
}
}
return(externalJointPosition);
}