/// <summary>
/// Apply Attach Tool Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionAttach action)
{
// The cursor has now a tool attached to it
this.tool = action.tool;
// Relative transform
// If user issued a relative action, make sure there are absolute values to work with. (This limitation is due to current lack of FK/IK solvers)
if (this.position == null || this.rotation == null)
{
Console.WriteLine("Sorry, must provide absolute transform values before attaching a tool... " + this);
}
else
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(action.tool.TCPPosition, this.rotation);
Vector newPos = this.position + worldVector;
Rotation newRot = Rotation.Combine(this.rotation, action.tool.TCPOrientation); // postmultiplication
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
this.prevJoints = this.joints;
// this.joints = null; // flag joints as null to avoid Joint instructions using obsolete data --> no need to do this, joints remain the same anyway?
}
return(true);
}
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(" $VEL = {{CP {0}, ORI1 100, ORI2 100}}",
Math.Round(0.001 * cursor.speed, 3 + Geometry.STRING_ROUND_DECIMALS_MM));
break;
case ActionType.Precision:
dec = string.Format(" $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)",
commChar,
am.message);
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(" WAIT SEC {0}",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
commChar,
ac.comment);
break;
case ActionType.Attach:
ActionAttach at = (ActionAttach)action;
dec = string.Format(" $TOOL = {0}",
GetToolValue(cursor));
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)action;
dec = string.Format(" $TOOL = $NULLFRAME");
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (!aiod.isDigit)
{
dec = $" {commChar} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 1 || aiod.pinNum > 32) // KUKA starts counting pins by 1
{
dec = $" {commChar} 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 = $" {commChar} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 1 || aioa.pinNum > 16) // KUKA: analog pins [1 to 16]
{
dec = $" {commChar} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < -1 || aioa.value > 1)
{
dec = $" {commChar} 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)}";
}
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 (ADD_ACTION_STRING && action.type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
commChar,
action.ToString());
}
else if (ADD_ACTION_ID)
{
dec = string.Format("{0} {1} [{2}]",
dec,
commChar,
action.id);
}
declaration = dec;
return(dec != null);
}
internal static 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 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.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
cursor.jointSpeed > Geometry.EPSILON2 ? 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.EPSILON2 ? Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_ACCELERATION,
cursor.speed > Geometry.EPSILON2 ? 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.RotationSpeed:
dec = string.Format(" {0} WARNING: RotationSpeed() has no effect in UR robots, try JointSpeed() or JointAcceleration() instead", COMMENT_CHAR);
break;
case ActionType.Axes:
// HAL generates a "set_tcp(p[0,0,0,0,0,0])" call here which I find confusing...
dec = string.Format(" movej({0}, a={1}, v={2}, r={3})",
GetJointTargetValue(cursor),
cursor.jointAcceleration > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
cursor.jointSpeed > Geometry.EPSILON2 ? 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(" sleep({0})",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
COMMENT_CHAR,
ac.comment);
break;
case ActionType.Attach:
ActionAttach aa = (ActionAttach)action;
dec = string.Format(" set_tcp({0})", // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
GetToolValue(cursor));
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)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 = $" {COMMENT_CHAR} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 0 || aiod.pinNum > 7)
{
dec = $" {COMMENT_CHAR} 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 = $" {COMMENT_CHAR} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 0 || aioa.pinNum > 1)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < 0 || aioa.value > 1)
{
dec = $" {COMMENT_CHAR} 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)})";
}
break;
//default:
// dec = string.Format(" # ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (humanComments && action.type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
COMMENT_CHAR,
action.ToString());
}
//else if (ADD_ACTION_ID)
//{
// dec = string.Format("{0} {1} [{2}]",
// dec,
// COMMENT_CHAR,
// action.id);
//}
declaration = dec;
return(dec != null);
}
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.EPSILON2;
dec = string.Format(" WorldAccLim {0};",
zero ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
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.EPSILON2)
{
break; // no change
}
// If here, there was a change, so...
bool zeroAcc = cursor.acceleration < Geometry.EPSILON2;
dec = string.Format(" WorldAccLim {0};",
zeroAcc ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
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",
GetUNSAFERobTargetValue(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 TPWrite messages can only be 80 chars long
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(" WaitTime {0};",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
commChar,
ac.comment);
break;
case ActionType.Attach:
ActionAttach aa = (ActionAttach)action;
dec = string.Format(" {0} Tool \"{1}\" attached", // this action has no actual RAPID instruction, just add a comment
commChar,
aa.tool.name);
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)action;
dec = string.Format(" {0} Tools detached", // this action has no actual RAPID instruction, just add a comment
commChar);
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (aiod.pin < 0 || aiod.pin >= cursor.digitalOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aiod.ToString());
}
else
{
dec = string.Format(" SetDO {0}, {1};",
cursor.digitalOutputNames[aiod.pin],
aiod.on ? "1" : "0");
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (aioa.pin < 0 || aioa.pin >= cursor.analogOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aioa.ToString());
}
else
{
dec = string.Format(" SetAO {0}, {1};",
cursor.analogOutputNames[aioa.pin],
aioa.value);
}
break;
//default:
// dec = string.Format(" ! ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (ADD_ACTION_STRING && action.type != ActionType.Comment)
{
dec = string.Format("{0}{1} {2} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
commChar,
action.ToString());
}
else if (ADD_ACTION_ID)
{
dec = string.Format("{0}{1} {2} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
commChar,
action.id);
}
declaration = dec;
return(dec != null);
}
