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; }