public override String to_script(bRobotBrand type, bool cont)
{
string cmd;
switch (type)
{
case (bRobotBrand.UNKNOWN):
return "Unknown robot model";
// URSCRIPT
case (bRobotBrand.UR):
Types.UR.AxisAngle aa = new Types.UR.AxisAngle(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
if (this.axis)
{
cmd = String.Format("movep([{0:0.000}, {1:0.000}, {2:0.000}, {3:0.000}, {4:0.000}, {5:0.000}], a={6:0.000}, v={7:0.000}",
this.q[0], this.q[1], this.q[2],
this.q[3], this.q[4], this.q[5],
this.qdd, this.qd);
}
else
{
cmd = String.Format("movep(p[{0:0.000}, {1:0.000}, {2:0.000}, {3:0.000}, {4:0.000}, {5:0.000}], a={6:0.000}, v={7:0.000}",
this.waypoint.Origin.X * 0.001, this.waypoint.Origin.Y * 0.001, this.waypoint.Origin.Z * 0.001,
aa.x * aa.angle, aa.y * aa.angle, aa.z * aa.angle,
this.qdd, this.qd);
}
if (r > 0.0001)
cmd += String.Format(", r={0:0.0000}", r * 0.001);
cmd += ")\n";
return cmd;
// KUKA KRL
case (bRobotBrand.KUKA):
Types.KUKA.kukaABC k1 = new Types.KUKA.kukaABC(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
cmd = "SPL {" + String.Format("X {0:0.0000}, Y {1:0.0000}, Z {2:0.0000}, A {3:0.0000}, B {4:0.0000}, C {5:0.0000},",
k1.x, k1.y, k1.z, k1.a, k1.b, k1.c)
+ "E1 0, E2 0, E3 0, E4 0, E5 0, E6 0}";
if (cont)
cmd += "\n";
//cmd += " C_VEL\n";
else
cmd += "\n";
return cmd;
// ABB RAPID
case (bRobotBrand.ABB):
Quaternion quat;
break;
}
return "circular move";
}
public override String to_script(bRobotBrand type, bool cont)
{
string cmd;
switch (type)
{
case (bRobotBrand.UNKNOWN):
return "Unknown robot model";
case (bRobotBrand.UR):
Types.UR.AxisAngle aa = new Types.UR.AxisAngle(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
if (this.axis)
{
cmd = String.Format("movej([{0:0.0000}, {1:0.0000}, {2:0.0000}, {3:0.0000}, {4:0.0000}, {5:0.0000}], a={6:0.0000}, v={7:0.0000}",
this.q[0], this.q[1], this.q[2],
this.q[3], this.q[4], this.q[5],
this.qdd, this.qd);
}
else
{
cmd = String.Format("movej(p[{0:0.0000}, {1:0.0000}, {2:0.0000}, {3:0.0000}, {4:0.0000}, {5:0.0000}], a={6:0.0000}, v={7:0.0000}",
this.waypoint.Origin.X * 0.001, this.waypoint.Origin.Y * 0.001, this.waypoint.Origin.Z * 0.001,
aa.x * aa.angle, aa.y * aa.angle, aa.z * aa.angle,
this.qdd, this.qd);
}
if (r > 0.0001)
cmd += String.Format(", r = {0:0.0000}", r * 0.001);
cmd += ")\n";
return cmd;
case (bRobotBrand.KUKA):
Types.KUKA.kukaABC k = new Types.KUKA.kukaABC(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
//cmd = "PTP {" + String.Format("X {0:0.0000}, Y {1:0.0000}, Z {2:0.0000}, A {3:0.0000}, B {4:0.0000}, C {5:0.0000},",
//k.x, k.y, k.z, k.a, k.b, k.c)
//+ String.Format("E1 0, E2 0, E3 0, E4 0, E5 0, E6 0, S {0}, T {1}", k.s, k.t) + "}";
cmd = "PTP {" + String.Format("X {0:0.0000}, Y {1:0.0000}, Z {2:0.0000}, A {3:0.0000}, B {4:0.0000}, C {5:0.0000},",
k.x, k.y, k.z, k.a, k.b, k.c)
+
"E1 0, E2 0, E3 0, E4 0, E5 0, E6 0 }";
if (cont)
cmd += "\n";
//cmd += " C_PTP\n";
else
cmd += "\n";
return cmd;
case (bRobotBrand.ABB):
break;
}
return "joint move";
}
public override String to_script(bRobotBrand type, bool cont)
{
switch (type)
{
case (bRobotBrand.UNKNOWN):
return "Unknown robot model";
case (bRobotBrand.UR):
Types.UR.AxisAngle q1 = new Types.UR.AxisAngle(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
Types.UR.AxisAngle q2 = new Types.UR.AxisAngle(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint2));
return String.Format("movec(p[{0:0.0000}, {1:0.0000}, {2:0.0000}, {3:0.0000}, {4:0.0000}, {5:0.0000}], p[{6:0.0000}, {7:0.0000}, {8:0.0000}, {9:0.0000}, {10:0.0000}, {11:0.0000}], a={12:0.0000}, v={13:0.0000})",
this.waypoint.Origin.X * 0.001, this.waypoint.Origin.Y * 0.001, this.waypoint.Origin.Z * 0.001,
q1.x, q1.y, q1.z,
this.waypoint2.Origin.X * 0.001, this.waypoint2.Origin.Y * 0.001, this.waypoint2.Origin.Z * 0.001,
q2.x, q2.y, q2.z,
this.qdd, this.qd);
case (bRobotBrand.KUKA):
Types.KUKA.kukaABC k1 = new Types.KUKA.kukaABC(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint));
Types.KUKA.kukaABC k2 = new Types.KUKA.kukaABC(Rhino.Geometry.Transform.PlaneToPlane(Plane.WorldXY, this.waypoint2));
string cmd = String.Format("CIRC {X {0:0.000}, Y {1:0.000}, Z {2:0.000}, A {3:0.000}, B {4:0.000}, C {5:0.000},",
k1.x, k1.y, k1.z, k1.a, k1.b, k1.c)
+ String.Format("X {0:0.000}, Y {1:0.000}, Z {2:0.000}, A {3:0.000}, B {4:0.000}, C {5:0.000},",
k2.x, k2.y, k2.z, k2.a, k2.b, k2.c)
+ "E1 0, E2 0, E3 0, E4 0, E5 0, E6 0}";
if (cont)
cmd += " C_VEL\n";
else
cmd += "\n";
return cmd;
case (bRobotBrand.ABB):
break;
}
return "circular move";
}
public override int Write()
{
if (this.Robot.Type() != bRobotBrand.KUKA)
{
System.ArgumentException argEx = new System.ArgumentException("Invalid robot type for program. Is it a KUKA robot?");
throw argEx;
}
// GET INIT DATA
this.program = "";
this.acceleration = 100;
this.velocity = 100;
// Compile robot program
/* HEADER */
//this.program = "&ACCESS RVO\n";
this.program = "DEF " + this.name + "()\n";
this.program += "; Program generated by Brick v0.1\n; Brick is written by Tom Svilans\n; tomsvilans.com\n\n";
/* INITIALIZATION */
/* replace with proper joint max velocities and speeds */
this.program += String.Format("INT I\nBAS (#INITMOV,0 )\nFOR I = 1 TO 6\n$VEL_AXIS[I] = {0}\n$ACC_AXIS[I] = {1}\n$VEL_EXTAX[I] = {0}\n$ACC_EXTAX[I] = {1}\nENDFOR\n\n", this.velocity, this.acceleration);
/* set mode to 'tool on robot' */
this.program += "$IPO_MODE = #BASE\n";
/* replace with radius */
this.program += String.Format("$APO.CDIS = {0}\n", 100); // approximate distance
this.program += String.Format("$APO.CVEL = {0}\n", 100); // approximate velocity
/* set advance pointer */
this.program += String.Format("$ADVANCE = {0}\n", 3);
/* set tool data */
//this.program += String.Format("TOOL_DATA[{0}] = ", 14) + "{" + String.Format("X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}", 0, 0, 0, 0, 0, 0) + "}\n"; // set tool number and offset frame
//this.program += String.Format("$TOOL = TOOL_DATA[{0}]\n", 1); // tool number
/* get tool parameters */
Types.KUKA.kukaABC ttr = new Types.KUKA.kukaABC(robot.Tool.RootToTCP);
this.program += "$TOOL = {" + String.Format("X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}", ttr.x, ttr.y, ttr.z, ttr.a, ttr.b, ttr.c) + "}\n";
//this.program += "$TOOL = {" + String.Format("X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}", 0, 0, 0, 30, 0, 0) + "}\n"; // tool number
/* set base data */
this.program += String.Format("BASE_DATA[{0}]", 17) + " = {" + String.Format("X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}", 0, 0, 0, 0, 0, 0) + "}\n";
/* activate base */
this.program += String.Format("$BASE = BASE_DATA[{0}]\n", 17); // use base 17 as 0 base
/* active base with external axis */
//this.program += String.Format("$BASE = EK(MACHINE_DEF[2].ROOT, MACHINE_DEF[2].MECH_TYPE, BASE_DATA[{0}]: ", 17) + "{x 0, y 0, z 0, a 0, b 0, c 0})\n"; // use base 17 as 0 base
this.program += "\n"; // breathing room...
/* replace with initial acceleration */
this.program += String.Format("$ACC.CP = {0}\n", this.acceleration * 0.006);
/* replace with initial velocity */
this.program += String.Format("$VEL.CP = {0}\n", this.velocity * 0.002);
this.program += String.Format("$VEL.ORI1 = {0}\n", (int)(this.velocity / 3));
this.program += String.Format("$ACC.ORI1 = {0}\n", (int)(this.acceleration / 2));
this.program += String.Format("$VEL.ORI2 = {0}\n", (int)(this.velocity / 3));
this.program += String.Format("$ACC.ORI2 = {0}\n", (int)(this.acceleration / 2));
//this.program += "$H_POS = {" + String.Format("A1 {0}, A2 {1}, A3 {2}, A4 {3}, A5 {4}, A6 {5},",
// 0, -80, 120, 0, -40, 0) +
// String.Format(" E1 0, E2 0, E3 0, E4 0, E5 0, E6 0, T '{0}', S '{1}'", 0, 0) + "}\n";
double r2d = 180.0 / Math.PI;
this.program += "$H_POS = {" + String.Format("A1 {0}, A2 {1}, A3 {2}, A4 {3}, A5 {4}, A6 {5},",
qHome[0] * r2d, qHome[1] * r2d, qHome[2] * r2d, qHome[3] * r2d, qHome[4] * r2d, qHome[5] * r2d) +
" E1 0, E2 0, E3 0, E4 0, E5 0, E6 0}\n";
this.program += "PTP $H_POS\n\n"; // go to home position
/* WRITE COMMANDS */
/* set current velocity and acceleration values, if they differ, write speed command too */
int last = this.commands.Count - 1;
for (int i = 0; i < this.commands.Count; ++i)
{
switch (this.commands[i].Type())
{
case (CommandType.MoveCommand):
/* cast command as MoveCommand */
bMoveBase movcmd = (bMoveBase)this.commands[i];
/* handle continuous movements */
bool cont = true;
if (i < last)
{
if (this.commands[i + 1].Type() == CommandType.MoveCommand)
{
/* check if next move is a stop move (not continuous) */
bMoveBase movnxt = (bMoveBase)this.commands[i + 1];
if (movnxt.MovementType() == bMoveBase.MoveType.Stop)
cont = false;
}
}
/* if it is the last command, not continous */
else
cont = false;
/* transform waypoint to local coordinates */
movcmd.Transform(Transform.PlaneToPlane(robot.Position, Plane.WorldXY));
/* where to put this? needs to go between below if-else block for splines... */
if ((movcmd.MovementType() != bMoveBase.MoveType.Stop) && (Math.Abs(movcmd.Velocity - this.velocity) > 0.0001))
{
/* update current velocity to new one */
this.velocity = movcmd.Velocity;
/* record change in velocity */
this.program += String.Format("$VEL.CP = {0}\n", this.velocity); // record change in velocity and convert from mm/s to m/s
}
/* handle spline moves */
if (movcmd.MovementType() == bMoveBase.MoveType.Process && !spline)
{
/* start spline block */
this.spline = true;
this.program += "SPLINE\n";
}
else if (movcmd.MovementType() != bMoveBase.MoveType.Process && spline)
{
/* end spline block */
this.program += "ENDSPLINE\n";
this.spline = false;
}
/* write command in KRL */
this.program += movcmd.to_script(bRobotBrand.KUKA, cont);
/* put waypoint back into world space */
movcmd.Transform(Transform.PlaneToPlane(Plane.WorldXY, robot.Position));
break;
case (CommandType.IOCommand):
/* cast command as IOCommand */
bIOBase iocmd = (bIOBase)this.commands[i];
/* write command in KRL*/
this.program += iocmd.to_script(bRobotBrand.KUKA);
break;
}
}
if (this.spline)
{
/* if spline block is open, close it */
this.spline = false;
this.program += "ENDSPLINE\n";
}
/* return home and end it */
this.program += "PTP $H_POS\n\n";
this.program += "\nEND\n";
return 1;
}
