/// <summary>
/// Returns the base position of the robot in world coordinate space if it is moved by an external axis.
/// </summary>
/// <returns> The position of the robot as a plane. </returns>
private Plane GetPositionPlane()
{
// NOTE: Only works for a robot info with an maximum of one external linear axis
// Deep copy the current position / base plane
Plane plane = new Plane(_robot.BasePlane);
// Check if an external axis is attached to the robot
for (int i = 0; i < _robot.ExternalAxes.Count; i++)
{
ExternalAxis externalAxis = _robot.ExternalAxes[i];
int logic = externalAxis.AxisNumber;
// Moves Robot?
if (externalAxis.MovesRobot == true)
{
// An External Linear Axis that moves the robot
if (externalAxis is ExternalLinearAxis externalLinearAxis)
{
if (_target.ExternalJointPosition[logic] == 9e9)
{
externalLinearAxis.AxisCurve.ClosestPoint(_targetPlane.Origin, out double param);
plane.Origin = externalLinearAxis.AxisCurve.PointAt(param);
}
else
{
plane = externalLinearAxis.CalculatePosition(_target.ExternalJointPosition, out _);
}
}
// An External Rotational Axis that moves the robot
else if (externalAxis is ExternalRotationalAxis externalRotationalAxis)
{
plane = externalRotationalAxis.CalculatePosition(_target.ExternalJointPosition, out _);
}
// Other External Axis types that move the robot
else
{
plane = externalAxis.CalculatePosition(_target.ExternalJointPosition, out _);
}
// Break the loop since one axternal axis can move the robot.
break;
}
}
// Returns the position plane of the robot
return(plane);
}
/// <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 forward kinematics solution with the Joint Positions stored inside this Forward Kinematics instance.
/// </summary>
public void Calculate()
{
// Clear current solution
Clear();
// Check axis limits
CheckInternalAxisLimits();
CheckExternalAxisLimits();
// Deep copy the mehses if the pose should be calculated
if (_hideMesh == false)
{
_posedInternalAxisMeshes = _robot.Meshes.ConvertAll(mesh => mesh.DuplicateMesh());
_posedExternalAxisMeshes = new List <List <Mesh> >();
}
// Get initial position plane
_positionPlane = new Plane(_robot.BasePlane);
// Count the number of external linear axes that is used: it is now limited to one
double count = 0;
// Calculates external axes positions
_posedExternalAxisPlanes = new Plane[_robot.ExternalAxes.Count];
for (int i = 0; i < _robot.ExternalAxes.Count; i++)
{
// Get the external axis
ExternalAxis externalAxis = _robot.ExternalAxes[i];
int logic = _robot.ExternalAxes[i].AxisNumber;
// Get external axis plane
_posedExternalAxisPlanes[i] = externalAxis.CalculatePositionSave(_externalJointPosition);
// Check if an external axis moves the robot and calculate the position plane.
if (externalAxis.MovesRobot == true && count == 0)
{
_positionPlane = externalAxis.CalculatePosition(_externalJointPosition, out bool inLimits);
count += 1;
}
}
// Move relative to base
Transform transNow;
transNow = Transform.PlaneToPlane(_robot.BasePlane, _positionPlane);
// Calculates internal axes
// First caculate all tansformations (rotations)
// Axis 1
Transform rot1;
Plane planeAxis1 = new Plane(_robot.InternalAxisPlanes[0]);
rot1 = Transform.Rotation(_robotJointPosition[0] * Math.PI / 180, planeAxis1.ZAxis, planeAxis1.Origin);
// Axis 2
Transform rot2;
Plane planeAxis2 = new Plane(_robot.InternalAxisPlanes[1]);
planeAxis2.Transform(rot1);
rot2 = Transform.Rotation(_robotJointPosition[1] * Math.PI / 180, planeAxis2.ZAxis, planeAxis2.Origin);
// Axis 3
Transform rot3;
Plane planeAxis3 = new Plane(_robot.InternalAxisPlanes[2]);
planeAxis3.Transform(rot2 * rot1);
rot3 = Transform.Rotation(_robotJointPosition[2] * Math.PI / 180, planeAxis3.ZAxis, planeAxis3.Origin);
// Axis 4
Transform rot4;
Plane planeAxis4 = new Plane(_robot.InternalAxisPlanes[3]);
planeAxis4.Transform(rot3 * rot2 * rot1);
rot4 = Transform.Rotation(_robotJointPosition[3] * Math.PI / 180, planeAxis4.ZAxis, planeAxis4.Origin);
// Axis 5
Transform rot5;
Plane planeAxis5 = new Plane(_robot.InternalAxisPlanes[4]);
planeAxis5.Transform(rot4 * rot3 * rot2 * rot1);
rot5 = Transform.Rotation(_robotJointPosition[4] * Math.PI / 180, planeAxis5.ZAxis, planeAxis5.Origin);
// Axis 6
Transform rot6;
Plane planeAxis6 = new Plane(_robot.InternalAxisPlanes[5]);
planeAxis6.Transform(rot5 * rot4 * rot3 * rot2 * rot1);
rot6 = Transform.Rotation(_robotJointPosition[5] * Math.PI / 180, planeAxis6.ZAxis, planeAxis6.Origin);
// Apply transformations on tcp plane
_tcpPlane = new Plane(_robot.ToolPlane);
_tcpPlane.Transform(transNow * rot6 * rot5 * rot4 * rot3 * rot2 * rot1);
// Get posed meshes
if (_hideMesh == false)
{
// Base link transform
_posedInternalAxisMeshes[0].Transform(transNow);
// Link_1 tranform
_posedInternalAxisMeshes[1].Transform(transNow * rot1);
// Link_2 tranform
_posedInternalAxisMeshes[2].Transform(transNow * rot2 * rot1);
// Link_3 tranform
_posedInternalAxisMeshes[3].Transform(transNow * rot3 * rot2 * rot1);
// Link_4 tranform
_posedInternalAxisMeshes[4].Transform(transNow * rot4 * rot3 * rot2 * rot1);
// Link_5 tranform
_posedInternalAxisMeshes[5].Transform(transNow * rot5 * rot4 * rot3 * rot2 * rot1);
// Link_6 tranform
_posedInternalAxisMeshes[6].Transform(transNow * rot6 * rot5 * rot4 * rot3 * rot2 * rot1);
// End-effector transform
_posedInternalAxisMeshes[7].Transform(transNow * rot6 * rot5 * rot4 * rot3 * rot2 * rot1);
// External axis meshes
for (int i = 0; i < _robot.ExternalAxes.Count; i++)
{
_robot.ExternalAxes[i].PoseMeshes(_externalJointPosition);
_posedExternalAxisMeshes.Add(_robot.ExternalAxes[i].PosedMeshes.ConvertAll(mesh => mesh.DuplicateMesh()));
}
}
}
/// <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)
{
// Clears Work Objects List
_workObjects.Clear();
// Input variables
List <string> names = new List <string>();
List <Plane> planes = new List <Plane>();
List <ExternalAxis> externalAxes = new List <ExternalAxis>();
// Catch the input data
if (!DA.GetDataList(0, names))
{
return;
}
if (!DA.GetDataList(1, planes))
{
return;
}
if (!DA.GetDataList(2, externalAxes))
{
externalAxes = new List <ExternalAxis>()
{
null
};
}
// Replace spaces
names = HelperMethods.ReplaceSpacesAndRemoveNewLines(names);
// Get longest Input List
int[] sizeValues = new int[3];
sizeValues[0] = names.Count;
sizeValues[1] = planes.Count;
sizeValues[2] = externalAxes.Count;
int biggestSize = HelperMethods.GetBiggestValue(sizeValues);
// Keeps track of used indicies
int nameCounter = -1;
int planesCounter = -1;
int axisCounter = -1;
// Creates work objects
WorkObject workObject;
List <WorkObject> workObjects = new List <WorkObject>();
for (int i = 0; i < biggestSize; i++)
{
string name = "";
Plane plane = new Plane();
ExternalAxis externalAxis = null;
// Names counter
if (i < sizeValues[0])
{
name = names[i];
nameCounter++;
}
else
{
name = names[nameCounter] + "_" + (i - nameCounter);
}
// Planes counter
if (i < sizeValues[1])
{
plane = planes[i];
planesCounter++;
}
else
{
plane = planes[planesCounter];
}
// Axis counter
if (i < sizeValues[2])
{
externalAxis = externalAxes[i];
axisCounter++;
}
else
{
externalAxis = externalAxes[axisCounter];
}
// Make work object
workObject = new WorkObject(name, plane, externalAxis);
workObjects.Add(workObject);
}
// Output
_workObjects = workObjects;
DA.SetDataList(0, workObjects);
#region Object manager
// Gets ObjectManager of this document
_objectManager = DocumentManager.GetDocumentObjectManager(this.OnPingDocument());
// Clears Work Object Name
for (int i = 0; i < _woNames.Count; i++)
{
_objectManager.WorkObjectNames.Remove(_woNames[i]);
}
_woNames.Clear();
// Removes lastName from WorkObjectNameList
if (_objectManager.WorkObjectNames.Contains(_lastName))
{
_objectManager.WorkObjectNames.Remove(_lastName);
}
// Adds Component to WorkObjectsByGuid Dictionary
if (!_objectManager.WorkObjectsByGuid.ContainsKey(this.InstanceGuid))
{
_objectManager.WorkObjectsByGuid.Add(this.InstanceGuid, this);
}
// Checks if the work object 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.WorkObjectNames.Contains(names[i]))
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Work Object Name already in use.");
_namesUnique = false;
_lastName = "";
break;
}
else
{
// Adds Work Object Name to list
_woNames.Add(names[i]);
_objectManager.WorkObjectNames.Add(names[i]);
// Run SolveInstance on other Work Objects with no unique Name to check if their name is now available
_objectManager.UpdateWorkObjects();
_lastName = names[i];
}
// Checks if variable name exceeds max character limit for RAPID Code
if (HelperMethods.VariableExeedsCharacterLimit32(names[i]))
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Work Object 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, "Work Object 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 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();
}