/// <summary>
/// Apply Attach Tool Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionAttachTool action)
{
// Sanity: this is a fix for pre-0.8.x compatibility where Attach came with the Tool object, not the name.
// Older versions of Machina would yield error searching for `null` key on `availableTools`
if (action.toolName == null)
{
logger.Error($"Obsolete version of AttachTool; please update Machina to latest update.");
return(false);
}
// Sanity
if (!availableTools.ContainsKey(action.toolName))
{
logger.Warning($"No tool named \"{action.toolName}\" defined in this robot; please use \"DefineTool\" first.");
return(false);
}
// This would not work in case the user had defined a new tool with different values but same name (not great practice, but technically possible anyway...)
//if (action.toolName == this.tool.name)
//{
// logger.Verbose($"Attaching the same tool? No changes...");
// return true;
//}
// The cursor has now a tool attached to it
Tool prevTool = this.tool;
this.tool = availableTools[action.toolName];
// Shim for lack of IK
// If coming from axes motion, no need to transform the TCP
if (this.position == null || this.rotation == null)
{
logger.Warning($"Attaching tool without TCP values, inconsistent results may follow...?");
}
// Otherwise transform the TCP
else
{
if (prevTool != null)
{
logger.Debug($"Detaching tool {prevTool.name} before attaching {this.tool.name}.");
UndoToolTransformOnCursor(this, prevTool, logger, _logRelativeActions);
}
ApplyToolTransformToCursor(this, this.tool, logger, _logRelativeActions);
}
return(true);
}
/// <summary>
/// Apply Attach Tool Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionAttachTool action)
{
// Sanity
if (!availableTools.ContainsKey(action.toolName))
{
logger.Warning($"No tool named \"{action.toolName}\" defined in this robot; please use \"DefineTool\" first.");
return(false);
}
// Shim for lack of IK
// If coming from axes motion, no need to transform the TCP
if (this.position == null || this.rotation == null)
{
logger.Warning($"Attaching tool without TCP values, inconsistent results may follow...?");
}
// Otherwise transform the TCP
else
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
Vector newPos = this.position + worldVector;
Rotation newRot = Rotation.Combine(this.rotation, this.tool.TCPOrientation); // postmultiplication
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
//this.prevAxes = this.axes; // why was this here? joints don't change on tool attachment...
}
// The cursor has now a tool attached to it
this.tool = availableTools[action.toolName];
return(true);
}
internal bool GenerateInstructionDeclaration(
Action action,
RobotCursor cursor,
Dictionary <double, string> velNames,
Dictionary <double, string> zoneNames,
Dictionary <Tool, string> toolNames,
out string declaration)
{
string dec = null;
switch (action.Type)
{
case ActionType.Acceleration:
bool zero = cursor.acceleration < Geometry.EPSILON;
dec = string.Format(" WorldAccLim {0};",
zero ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M)).ToString(CultureInfo.InvariantCulture);
break;
//case ActionType.JointSpeed:
//case ActionType.JointAcceleration:
// dec = string.Format(" {0} WARNING: {1}() has no effect in ABB robots.",
// commChar,
// action.Type);
// break;
// @TODO: push/pop management should be done PROGRAMMATICALLY, not this CHAPUZa...
case ActionType.PushPop:
// Find if there was a change in acceleration, and set the corresponsing instruction...
ActionPushPop app = action as ActionPushPop;
if (app.push)
{
break; // only necessary for pops
}
if (Math.Abs(cursor.acceleration - cursor.settingsBuffer.SettingsBeforeLastPop.Acceleration) < Geometry.EPSILON)
{
break; // no change
}
// If here, there was a change, so...
bool zeroAcc = cursor.acceleration < Geometry.EPSILON;
dec = string.Format(" WorldAccLim {0};",
zeroAcc
? "\\Off"
: "\\On := " + Math.Round(0.001 * cursor.acceleration,
Geometry.STRING_ROUND_DECIMALS_M)).ToString(CultureInfo.InvariantCulture);
break;
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
dec = string.Format(" {0} {1}, {2}, {3}, {4}\\{5};",
cursor.motionType == MotionType.Joint ? "MoveJ" : "MoveL",
GetRobTargetValue(cursor),
velNames[cursor.speed],
zoneNames[cursor.precision],
cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
"WObj:=WObj0");
break;
case ActionType.Axes:
dec = string.Format(" MoveAbsJ {0}, {1}, {2}, {3}\\{4};",
GetJointTargetValue(cursor),
velNames[cursor.speed],
zoneNames[cursor.precision],
cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
"WObj:=WObj0");
break;
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" TPWrite \"{0}\";",
am.message.Length <= 80 ?
am.message :
am.message.Substring(0, 80)); // ABB strings can only be 80 chars long
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(CultureInfo.InvariantCulture,
" WaitTime {0};",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
CC,
ac.comment);
break;
case ActionType.DefineTool:
ActionDefineTool adt = action as ActionDefineTool;
dec = string.Format(" {0} Tool \"{1}\" defined", // this action has no actual RAPID instruction, just add a comment
CC,
adt.tool.name);
break;
case ActionType.AttachTool:
ActionAttachTool aa = (ActionAttachTool)action;
dec = string.Format(" {0} Tool \"{1}\" attached", // this action has no actual RAPID instruction, just add a comment
CC,
aa.toolName);
break;
case ActionType.DetachTool:
ActionDetachTool ad = (ActionDetachTool)action;
dec = string.Format(" {0} All tools detached", // this action has no actual RAPID instruction, just add a comment
CC);
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
dec = $" SetDO {aiod.pinName}, {(aiod.on ? "1" : "0")};";
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
dec = $" SetAO {aioa.pinName}, {aioa.value};";
break;
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
if (!acc.isDeclaration)
{
dec = " " + acc.statement;
}
break;
//default:
// dec = string.Format(" ! ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (addActionString && action.Type != ActionType.Comment)
{
dec = string.Format("{0}{1} {2} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
CC,
action.ToString());
}
else if (addActionID)
{
dec = string.Format("{0}{1} {2} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
CC,
action.Id);
}
declaration = dec;
return(dec != null);
}
internal bool GenerateInstructionDeclaration(
Action action, RobotCursor cursor, bool humanComments,
out string declaration)
{
string dec = null;
switch (action.Type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
// Accelerations and velocities have different meaning for movej and movel instructions: they are either angular or linear respectively.
// Use speed and acceleration values as deg/s or mm/s (converted to rad and m) in either case.
if (cursor.motionType == MotionType.Joint)
{
dec = string.Format(CultureInfo.InvariantCulture,
" movej({0}, a={1}, v={2}, r={3})",
GetPoseTargetValue(cursor),
Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
}
else
{
dec = string.Format(CultureInfo.InvariantCulture,
" movep({0}, a={1}, v={2}, r={3})",
GetPoseTargetValue(cursor),
Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
}
break;
//// Accelerations and velocoties have different meaning for moveJ and moveL instructions.
//// Joint motion is essentially the same as Axes motion, just the input is a pose instead of a joints vector.
//if (cursor.motionType == MotionType.Joint)
//{
// dec = string.Format(" movej({0}, a={1}, v={2}, r={3})",
// GetPoseTargetValue(cursor),
// cursor.jointAcceleration > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
// cursor.jointSpeed > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
// Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
//}
//else
//{
// dec = string.Format(" movel({0}, a={1}, v={2}, r={3})",
// GetPoseTargetValue(cursor),
// cursor.acceleration > Geometry.EPSILON ? Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_ACCELERATION,
// cursor.speed > Geometry.EPSILON ? Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_SPEED,
// Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
//}
//break;
case ActionType.Axes:
dec = string.Format(CultureInfo.InvariantCulture,
" movej({0}, a={1}, v={2}, r={3})",
GetJointTargetValue(cursor),
Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
//dec = string.Format(" movej({0}, a={1}, v={2}, r={3})",
// GetJointTargetValue(cursor),
// cursor.jointAcceleration > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
// cursor.jointSpeed > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
// Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
break;
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
am.message);
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(CultureInfo.InvariantCulture,
" sleep({0})",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
CC,
ac.comment);
break;
case ActionType.DefineTool:
ActionDefineTool adt = action as ActionDefineTool;
dec = string.Format(" {0} Tool \"{1}\" defined", // this action has no actual instruction, just add a comment
CC,
adt.tool.name);
break;
case ActionType.AttachTool:
ActionAttachTool aa = (ActionAttachTool)action;
dec = string.Format(" set_tcp({0})", // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
GetToolValue(cursor));
break;
case ActionType.DetachTool:
ActionDetachTool ad = (ActionDetachTool)action;
dec = string.Format(" set_tcp(p[0,0,0,0,0,0])"); // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (!aiod.isDigit)
{
dec = $" {CC} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 0 || aiod.pinNum > 7)
{
dec = $" {CC} ERROR on \"{aiod}\": IO number not available";
}
else
{
dec = $" set_{(aiod.isToolPin ? "tool" : "standard")}_digital_out({aiod.pinNum}, {(aiod.on ? "True" : "False")})";
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (!aioa.isDigit)
{
dec = $" {CC} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 0 || aioa.pinNum > 1)
{
dec = $" {CC} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < 0 || aioa.value > 1)
{
dec = $" {CC} ERROR on \"{aioa}\": value out of range [0.0, 1.0]";
}
else
{
//dec = $" set_{(aioa.isToolPin ? "tool" : "standard")}_analog_out({aioa.pinNum}, {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)})";
dec = string.Format(CultureInfo.InvariantCulture,
" set_{0}_analog_out({1}, {2})",
aioa.isToolPin ? "tool" : "standard",
aioa.pinNum,
Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
}
break;
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
if (!acc.isDeclaration)
{
dec = $" {acc.statement}";
}
break;
//default:
// dec = string.Format(" # ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (humanComments && action.Type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
CC,
action.ToString());
}
//else if (ADD_ACTION_ID)
//{
// dec = string.Format("{0} {1} [{2}]",
// dec,
// CC,
// action.id);
//}
declaration = dec;
return(dec != null);
}
internal bool GenerateInstructionDeclaration(
Action action, RobotCursor cursor,
out string declaration)
{
string dec = null;
switch (action.Type)
{
// KUKA does explicit setting of velocities and approximate positioning, so these actions make sense as instructions
case ActionType.Speed:
dec = string.Format(CultureInfo.InvariantCulture,
//" $VEL = {{CP {0}, ORI1 100, ORI2 100}}", // This was reported to not work
" $VEL.CP = {0}", // @TODO: figure out how to also incorporate ORI1 and ORI2
Math.Round(0.001 * cursor.speed, 3 + Geometry.STRING_ROUND_DECIMALS_MM));
break;
case ActionType.Precision:
dec = string.Format(CultureInfo.InvariantCulture,
" $APO.CDIS = {0}",
cursor.precision);
break;
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
dec = string.Format(" {0} {1} {2}",
cursor.motionType == MotionType.Joint ? "PTP" : "LIN",
GetPositionTargetValue(cursor),
cursor.precision >= 1 ? "C_DIS" : "");
break;
case ActionType.Axes:
dec = string.Format(" {0} {1} {2}",
"PTP",
GetAxisTargetValue(cursor),
cursor.precision >= 1 ? "C_DIS" : ""); // @TODO: figure out how to turn this into C_PTP
break;
// @TODO: apparently, messages in KRL are kind fo tricky, with several manuals just dedicated to it.
// Will figure this out later.
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" {0} MESSAGE: \"{1}\" (messages in KRL currently not supported in Machina)",
CC,
am.message);
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(CultureInfo.InvariantCulture,
" WAIT SEC {0}",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
CC,
ac.comment);
break;
case ActionType.DefineTool:
ActionDefineTool adt = action as ActionDefineTool;
dec = string.Format(" {0} Tool \"{1}\" defined", // this action has no actual instruction, just add a comment
CC,
adt.tool.name);
break;
case ActionType.AttachTool:
ActionAttachTool at = (ActionAttachTool)action;
dec = string.Format(" $TOOL = {0}",
GetToolValue(cursor));
break;
case ActionType.DetachTool:
ActionDetachTool ad = (ActionDetachTool)action;
dec = string.Format(" $TOOL = $NULLFRAME");
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (!aiod.isDigit)
{
dec = $" {CC} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 1 || aiod.pinNum > 32) // KUKA starts counting pins by 1
{
dec = $" {CC} ERROR on \"{aiod}\": IO number not available";
}
else
{
dec = $" $OUT[{aiod.pinNum}] = {(aiod.on ? "TRUE" : "FALSE")}";
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (!aioa.isDigit)
{
dec = $" {CC} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 1 || aioa.pinNum > 16) // KUKA: analog pins [1 to 16]
{
dec = $" {CC} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < -1 || aioa.value > 1)
{
dec = $" {CC} ERROR on \"{aioa}\": value out of range [-1.0, 1.0]";
}
else
{
//dec = $" $ANOUT[{aioa.pinNum}] = {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)}";
dec = string.Format(CultureInfo.InvariantCulture,
" $ANOUT[{0}] = {1}",
aioa.pinNum,
Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
}
break;
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
if (!acc.isDeclaration)
{
dec = $" {acc.statement}";
}
break;
//default:
// dec = string.Format(" ; ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (addActionString && action.Type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
CC,
action.ToString());
}
else if (addActionID)
{
dec = string.Format("{0} {1} [{2}]",
dec,
CC,
action.Id);
}
declaration = dec;
return(dec != null);
}