/// <summary>
/// Get the external axis values from a defined joint target
/// </summary>
/// <param name="jointTarget"> The joint target to get the external axis values from. </param>
/// <returns></returns>
private List <double> GetExternalAxisValuesAsList(ABB.Robotics.Controllers.RapidDomain.JointTarget jointTarget)
{
// Initiate the list with external axis values
List <double> result = new List <double>()
{
};
// Get the axis values from the joint target
result.Add(jointTarget.ExtAx.Eax_a);
result.Add(jointTarget.ExtAx.Eax_b);
result.Add(jointTarget.ExtAx.Eax_c);
result.Add(jointTarget.ExtAx.Eax_d);
result.Add(jointTarget.ExtAx.Eax_e);
result.Add(jointTarget.ExtAx.Eax_f);
// Replace large numbers (the not connected axes)
for (int i = 0; i < result.Count; i++)
{
if (result[i] > 9.0e+8)
{
result[i] = 9e9;
}
}
// Return the list with axis values
return(result);
}
// Additional methods
#region additional methods
/// <summary>
/// Get the internal axis values from a defined joint target
/// </summary>
/// <param name="jointTarget"> The joint target to get the internal axis values from. </param>
/// <returns></returns>
private List <double> GetInternalAxisValuesAsList(ABB.Robotics.Controllers.RapidDomain.JointTarget jointTarget)
{
// Initiate the list with internal axis values
List <double> result = new List <double>()
{
};
// Get the axis values from the joint target
result.Add(jointTarget.RobAx.Rax_1);
result.Add(jointTarget.RobAx.Rax_2);
result.Add(jointTarget.RobAx.Rax_3);
result.Add(jointTarget.RobAx.Rax_4);
result.Add(jointTarget.RobAx.Rax_5);
result.Add(jointTarget.RobAx.Rax_6);
// Replace large numbers (the not connected axes) with an axis value equal to zero
for (int i = 0; i < result.Count; i++)
{
if (result[i] > 9.0e+8)
{
result[i] = 0;
}
}
// Return the list with axis values
return(result);
}
/// <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
GH_Controller controllerGoo = null;
// Catch input data
if (!DA.GetData(0, ref controllerGoo))
{
return;
}
// Clear output variables
_robotJointPositions.Clear();
_externalAxisValues.Clear();
// Data needed for making the datatree with axis values
MechanicalUnitCollection mechanicalUnits = controllerGoo.Value.MotionSystem.MechanicalUnits;
int externalAxisValuesPath = 0;
List <double> values;
GH_Path path;
// Make the output datatree with names with a branch for each mechanical unit
for (int i = 0; i < mechanicalUnits.Count; i++)
{
// Get the ABB joint target of the mechanical unit
MechanicalUnit mechanicalUnit = mechanicalUnits[i];
ABB.Robotics.Controllers.RapidDomain.JointTarget jointTarget = mechanicalUnit.GetPosition();
// For internal axis values
if (mechanicalUnit.Type == MechanicalUnitType.TcpRobot)
{
values = GetInternalAxisValuesAsList(jointTarget);
_robotJointPositions.Add(new RobotJointPosition(values));
}
// For external axis values
else
{
values = GetExternalAxisValuesAsList(jointTarget);
path = new GH_Path(externalAxisValuesPath);
_externalAxisValues.AppendRange(values.GetRange(0, mechanicalUnit.NumberOfAxes).ConvertAll(val => new GH_Number(val)), path);
externalAxisValuesPath += 1;
}
}
// Output
DA.SetDataList(0, _robotJointPositions);
DA.SetDataTree(1, _externalAxisValues);
}
