Ejemplo n.º 1
0
        /// <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);
        }
Ejemplo n.º 2
0
        /// <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);
        }
Ejemplo n.º 3
0
        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);
        }
Ejemplo n.º 4
0
        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);
        }
Ejemplo n.º 5
0
        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);
        }