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