/// <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)
{
List <double> AllAxises = new List <double>();
List <double> RobotData = new List <double>();
List <GeometryBase> RobotModel = new List <GeometryBase>();
Mesh ToolMeshModel = new Mesh();
double SimRatio = 0;
string Path = " ";
List <string> stringwrite = new List <string>();
if (!DA.GetDataList(0, AllAxises))
{
return;
}
if (!DA.GetDataList(1, RobotData))
{
return;
}
if (!DA.GetDataList(2, RobotModel))
{
return;
}
if (!DA.GetData(3, ref ToolMeshModel))
{
return;
}
if (!DA.GetData(4, ref SimRatio))
{
return;
}
if (!DA.GetData(5, ref Path))
{
return;
}
double d01 = RobotData[0];
double d12 = RobotData[1];
double d23 = RobotData[2];
double d34 = RobotData[3];
double d45 = RobotData[4];
double d56 = RobotData[5];
string iniA1 = AllAxises[0].ToString();
string iniA2 = AllAxises[1].ToString();
string iniA3 = AllAxises[2].ToString();
string iniA4 = AllAxises[3].ToString();
string iniA5 = AllAxises[4].ToString();
string iniA6 = AllAxises[5].ToString();
int RobotBase = 1;
int RobotTool = 1;
int RobotVel = 15;
int RobotACC = 100;
int RobotAPO = 50;
int RobotPTP = 3;
double RobotVelCp = 0.25;
double RobotApocdis = 1.5;
double RobotAdvance = 3;
stringwrite.Add(
@"&ACCESS RVP
&REL 1
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF KUKAProg()
;FOLD INI By Cobra(EasyRobot)
;FOLD BASISTECH INI
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS == TRUE DO IR_STOPM()
INTERRUPT ON 3
BAS (#INITMOV,0 )
;ENDFOLD (BASISTECH INI)
;ENDFOLD (INI)");
stringwrite.Add("");
stringwrite.Add(";FOLD STARTPOSITION - BASE IS " + RobotBase + ", TOOL IS " + RobotTool + ", POSITION IS A1 " + iniA1 + ", A2 " + iniA2 + ", A3 " + iniA3 + ", A4 " + iniA4 + ", A5 " + iniA5 + ", A6 " + iniA6);
stringwrite.Add("$BWDSTART = FALSE");
stringwrite.Add("PDAT_ACT = {VEL " + RobotVel + ",ACC " + RobotACC + ",APO_DIST " + RobotAPO + "}");
stringwrite.Add("FDAT_ACT = {TOOL_NO " + RobotTool + ",BASE_NO " + RobotBase + ",IPO_FRAME #BASE}");
stringwrite.Add("BAS (#PTP_PARAMS," + RobotPTP + ")");
stringwrite.Add("PTP {A1 " + iniA1 + ", A2 " + iniA2 + ", A3 " + iniA3 + ", A4 " + iniA4 + ", A5 " + iniA5 + ", A6 " + iniA6 + "}");
stringwrite.Add(";ENDFOLD\n");
stringwrite.Add(";FOLD LIN SPEED IS " + RobotVelCp + " m/sec, INTERPOLATION SETTINGS IN FOLD");
stringwrite.Add("$VEL.CP=" + RobotVelCp);
stringwrite.Add("$APO.CDIS=" + RobotApocdis);
stringwrite.Add("$ADVANCE=" + RobotAdvance);
stringwrite.Add(";ENDFOLD\n");
stringwrite.Add(";FOLD COMMANDS IN FOLD. SELECT EDIT/FOLDS/OPEN ALL FOLDS TO DISPLAY");
stringwrite.Add("BAS(#VEL_PTP," + RobotPTP + ")");
List <double[]> AllAxisesD = new List <double[]>();
for (int i = 0; i < AllAxises.Count / 6; i++)
{
double[] singlePair = new double[6];
for (int j = 0; j < 6; j++)
{
int indexOfSingle = i * 6 + j;
singlePair[j] = AllAxises[indexOfSingle];
}
AllAxisesD.Add(singlePair);
string A1 = singlePair[0].ToString();
string A2 = singlePair[1].ToString();
string A3 = singlePair[2].ToString();
string A4 = singlePair[3].ToString();
string A5 = singlePair[4].ToString();
string A6 = singlePair[5].ToString();
string output = "PTP {E6AXIS: A1 " + A1 + ", A2 " + A2 + ", A3 " + A3 + ", A4 " + A4 + ", A5 " + A5 + ", A6 " + A6 + "}";
stringwrite.Add(output);
}
stringwrite.Add(";ENDFOLD");
stringwrite.Add("END");
int simIndex = (int)((AllAxisesD.Count - 1) * SimRatio);
double[] UseAxises = AllAxisesD[simIndex];
double UseA1 = -UseAxises[0] * Math.PI / 180;
double UseA2 = UseAxises[1] * Math.PI / 180;
double UseA3 = UseAxises[2] * Math.PI / 180;
double UseA4 = -UseAxises[3] * Math.PI / 180;
double UseA5 = UseAxises[4] * Math.PI / 180;
double UseA6 = -UseAxises[5] * Math.PI / 180;
GeometryBase Axis1Model = RobotModel[0];
GeometryBase Axis12Model = RobotModel[1];
GeometryBase Axis23Model = RobotModel[2];
GeometryBase Axis34Model = RobotModel[3];
GeometryBase Axis45Model = RobotModel[4];
GeometryBase Axis56Model = RobotModel[5];
GeometryBase Axis6Model = RobotModel[6];
Point3d a1p1 = new Point3d(0, 0, 0);
Point3d a1p2 = new Point3d(0, 0, d01);
Vector3d a1a2 = new Vector3d(d12, 0, 0);
Vector3d a1v = new Vector3d(0, 0, 1);
a1a2.Rotate(UseA1, a1v);
Axis12Model.Rotate(UseA1, a1v, a1p1);
//---------------------------
Point3d a2p = Point3d.Add(a1p2, a1a2);
Vector3d a2a3 = new Vector3d(d23, 0, 0);
Vector3d a2v = new Vector3d(0, 1, 0);
a2v.Rotate(UseA1, a1v);
a2a3.Rotate(UseA1, a1v);
a2a3.Rotate(UseA2, a2v);
Vector3d a2pv = new Vector3d(a2p);
Axis23Model.Rotate(UseA1, a1v, a1p1);
Axis23Model.Rotate(UseA2, a2v, a1p1);
Axis23Model.Translate(a2pv);
//---------------------------
Point3d a3p = Point3d.Add(a2p, a2a3);
Vector3d a3a4 = new Vector3d(0, 0, 25);
Vector3d a4a5 = new Vector3d(420, 0, 0);
a3a4.Rotate(UseA1, a1v);
a3a4.Rotate(UseA2, a2v);
a3a4.Rotate(UseA3, a2v);
a4a5.Rotate(UseA1, a1v);
a4a5.Rotate(UseA2, a2v);
a4a5.Rotate(UseA3, a2v);
Vector3d a3pv = new Vector3d(a3p);
Axis34Model.Rotate(UseA1, a1v, a1p1);
Axis34Model.Rotate(UseA2, a2v, a1p1);
Axis34Model.Rotate(UseA3, a2v, a1p1);
Axis34Model.Translate(a3pv);
Point3d a4p = Point3d.Add(a3p, a3a4);
Vector3d a4v = new Vector3d(1, 0, 0);
a4v.Rotate(UseA1, a1v);
a4v.Rotate(UseA2, a2v);
a4v.Rotate(UseA3, a2v);
Vector3d a4pv = new Vector3d(a4p);
Axis45Model.Rotate(UseA1, a1v, a1p1);
Axis45Model.Rotate(UseA2, a2v, a1p1);
Axis45Model.Rotate(UseA3, a2v, a1p1);
Axis45Model.Rotate(UseA4, a4v, a1p1);
Axis45Model.Translate(a4pv);
//---------------------------
Point3d a5p = Point3d.Add(a4p, a4a5);
Vector3d a5a6 = new Vector3d(d56, 0, 0);
Vector3d a5v = new Vector3d(0, 1, 0);
a5a6.Rotate(UseA1, a1v);
a5a6.Rotate(UseA2, a2v);
a5a6.Rotate(UseA3, a2v);
a5a6.Rotate(UseA4, a4v);
a5v.Rotate(UseA1, a1v);
a5v.Rotate(UseA4, a4v);
a5a6.Rotate(UseA5, a5v);
Vector3d a5pv = new Vector3d(a5p);
Axis56Model.Rotate(UseA1, a1v, a1p1);
Axis56Model.Rotate(UseA2, a2v, a1p1);
Axis56Model.Rotate(UseA3, a2v, a1p1);
Axis56Model.Rotate(UseA4, a4v, a1p1);
Axis56Model.Rotate(UseA5, a5v, a1p1);
Axis56Model.Translate(a5pv);
Point3d a6p = Point3d.Add(a5p, a5a6);
Vector3d a6v = new Vector3d(1, 0, 0);
Vector3d a6pv = new Vector3d(a6p);
a6v.Rotate(UseA1, a1v);
a6v.Rotate(UseA2, a2v);
a6v.Rotate(UseA3, a2v);
a6v.Rotate(UseA4, a4v);
a6v.Rotate(UseA5, a5v);
Axis6Model.Rotate(UseA1, a1v, a1p1);
Axis6Model.Rotate(UseA2, a2v, a1p1);
Axis6Model.Rotate(UseA3, a2v, a1p1);
Axis6Model.Rotate(UseA4, a4v, a1p1);
Axis6Model.Rotate(UseA5, a5v, a1p1);
Axis6Model.Rotate(UseA6, a6v, a1p1);
Axis6Model.Translate(a5pv);
//---------------------------
ToolMeshModel.Rotate(Math.PI / 2, Vector3d.YAxis, a1p1);
ToolMeshModel.Rotate(UseA1, a1v, a1p1);
ToolMeshModel.Rotate(UseA2, a2v, a1p1);
ToolMeshModel.Rotate(UseA3, a2v, a1p1);
ToolMeshModel.Rotate(UseA4, a4v, a1p1);
ToolMeshModel.Rotate(UseA5, a5v, a1p1);
ToolMeshModel.Rotate(UseA6, a6v, a1p1);
ToolMeshModel.Translate(a6pv);
//---------------------------
List <Point3d> AxisesPos = new List <Point3d>();
AxisesPos.Add(a1p1);
AxisesPos.Add(a1p2);
AxisesPos.Add(a2p);
AxisesPos.Add(a3p);
AxisesPos.Add(a4p);
AxisesPos.Add(a5p);
AxisesPos.Add(a6p);
Polyline RobotArmLine = new Polyline(AxisesPos);
List <GeometryBase> RobotArms = new List <GeometryBase>();
RobotArms.Add(Axis1Model);
RobotArms.Add(Axis12Model);
RobotArms.Add(Axis23Model);
RobotArms.Add(Axis34Model);
RobotArms.Add(Axis45Model);
RobotArms.Add(Axis56Model);
RobotArms.Add(Axis6Model);
RobotArms.Add(ToolMeshModel);
string[] finalExport = stringwrite.ToArray();
System.IO.File.WriteAllLines(Path, finalExport);
DA.SetDataList(0, UseAxises);
DA.SetData(1, RobotArmLine);
DA.SetDataList(2, RobotArms);
}
