Пример #1
0
        /// <summary>
        /// Apply Detach Tool action
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionDetach action)
        {
            if (this.tool == null)
            {
                Console.WriteLine("Robot had no tool attached");
                return(false);
            }

            // 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 detaching a tool... " + this);
                return(false);
            }

            // Now undo the tool's transforms
            // TODO: at some point in the future, check for translationFirst here
            Rotation newRot      = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation)); // postmultiplication by the inverse rotation
            Vector   worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
            Vector   newPos      = this.position - worldVector;

            this.prevPosition = this.position;
            this.position     = newPos;
            this.prevRotation = this.rotation;
            this.rotation     = newRot;
            this.prevJoints   = this.joints;
            this.joints       = null;

            // Detach the tool
            this.tool = null;

            return(true);
        }
Пример #2
0
        /// <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);
        }
Пример #3
0
        /// <summary>
        /// Apply Detach Tool action
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionDetachTool action)
        {
            if (this.tool == null)
            {
                logger.Verbose("Robot had no tool attached");
                return(false);
            }

            // Shim for lack of IK
            // If coming from axes motion, no need to undo the tool's transform on the TCP
            if (this.position == null || this.rotation == null)
            {
                // Really nothing to do here right?
            }
            // Otherwise undo the tool's transforms
            else
            {
                // TODO: at some point in the future, check for translationFirst here
                Rotation newRot      = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation)); // postmultiplication by the inverse rotation
                Vector   worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
                Vector   newPos      = this.position - worldVector;

                this.prevPosition = this.position;
                this.position     = newPos;
                this.prevRotation = this.rotation;
                this.rotation     = newRot;
                //this.prevAxes = this.axes;
                //this.axes = null;  // axes were null anyway...?
            }

            // "Detach" the tool
            this.tool = null;

            // -> This code was not properly detaching the tool when coming from axis motion
            //// 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)
            //{
            //    logger.Warning($"Cannot apply \"{action}\"; please provide absolute transform values before detaching a tool and try again.");
            //    return false;
            //}

            //// Now undo the tool's transforms
            //// TODO: at some point in the future, check for translationFirst here
            //Rotation newRot = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation));  // postmultiplication by the inverse rotation
            //Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
            //Vector newPos = this.position - worldVector;

            //this.prevPosition = this.position;
            //this.position = newPos;
            //this.prevRotation = this.rotation;
            //this.rotation = newRot;
            //this.prevAxes = this.axes;
            //this.axes = null;

            //// Detach the tool
            //this.tool = null;

            return(true);
        }
Пример #4
0
        /// <summary>
        /// Modify a cursor's TCP transform according to a tool. Useful for Attach operations.
        /// </summary>
        /// <param name="tool"></param>
        internal void ApplyToolTransformToCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
        {
            // Now transform the cursor position to the tool's transformation params:
            Vector   worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
            Vector   newPos      = cursor.position + worldVector;
            Rotation newRot      = Rotation.Combine(cursor.rotation, tool.TCPOrientation); // postmultiplication

            cursor.prevPosition = cursor.position;
            cursor.position     = newPos;
            cursor.prevRotation = cursor.rotation;
            cursor.rotation     = newRot;
            //cursor.prevAxes = cursor.axes;  // why was this here? joints don't change on tool attachment...

            if (log)
            {
                logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool attachment");
            }
        }
Пример #5
0
        /// <summary>
        /// Undo tool-based TCP transformations on a cursor. Useful for Detach operations.
        /// </summary>
        /// <param name="tool"></param>
        internal void UndoToolTransformOnCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
        {
            // TODO: at some point in the future, check for translationFirst here
            Rotation newRot      = Rotation.Combine(cursor.rotation, Rotation.Inverse(tool.TCPOrientation)); // postmultiplication by the inverse rotation
            Vector   worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
            Vector   newPos      = cursor.position - worldVector;

            cursor.prevPosition = cursor.position;
            cursor.position     = newPos;
            cursor.prevRotation = cursor.rotation;
            cursor.rotation     = newRot;
            //this.prevAxes = this.axes;
            //this.axes = null;  // axes were null anyway...?

            if (log)
            {
                logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool removal");
            }
        }
Пример #6
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);
        }
Пример #7
0
        /// <summary>
        /// Apply Transformation Action.
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionTransformation action)
        {
            Vector   newPos;
            Rotation newRot;

            // Relative transform
            if (action.relative)
            {
                // 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 (position == null || rotation == null)
                {
                    logger.Warning($"Cannot apply \"{action}\", must provide absolute transform values first before applying relative ones...");
                    return(false);
                }

                // This is Translate + Rotate
                if (action.translationFirst)
                {
                    if (referenceCS == ReferenceCS.World)
                    {
                        newPos = position + action.translation;
                        newRot = Rotation.Combine(action.rotation, rotation);  // premultiplication
                    }
                    else
                    {
                        //Vector worldVector = Vector.Rotation(action.translation, Rotation.Conjugate(this.rotation));
                        Vector worldVector = Vector.Rotation(action.translation, this.rotation);
                        newPos = position + worldVector;
                        newRot = Rotation.Combine(rotation, action.rotation);  // postmultiplication
                    }
                }

                // or Rotate + Translate
                else
                {
                    if (referenceCS == ReferenceCS.World)
                    {
                        newPos = position + action.translation;
                        newRot = Rotation.Combine(action.rotation, rotation);  // premultiplication
                    }

                    else
                    {
                        // @TOCHECK: is this correct?
                        newRot = Rotation.Combine(rotation, action.rotation);  // postmultiplication
                        //Vector worldVector = Vector.Rotation(action.translation, Rotation.Conjugate(newRot));
                        Vector worldVector = Vector.Rotation(action.translation, newRot);
                        newPos = position + worldVector;
                    }
                }
            }

            // Absolute transform
            else
            {
                newPos = new Vector(action.translation);
                newRot = new Rotation(action.rotation);
            }

            //// @TODO: this must be more programmatically implemented
            //if (Control.SAFETY_CHECK_TABLE_COLLISION)
            //{
            //    if (Control.IsBelowTable(newPos.Z))
            //    {
            //        if (Control.SAFETY_STOP_ON_TABLE_COLLISION)
            //        {
            //            Console.WriteLine("Cannot perform action: too close to base XY plane --> TCP.z = {0}", newPos.Z);
            //            return false;
            //        }
            //        else
            //        {
            //            Console.WriteLine("WARNING: too close to base XY plane, USE CAUTION! --> TCP.z = {0}", newPos.Z);
            //        }
            //    }
            //}

            prevPosition = position;
            position     = newPos;
            prevRotation = rotation;
            rotation     = newRot;

            prevAxes = axes;
            axes     = null; // flag joints as null to avoid Joint instructions using obsolete data

            if (isExtruding)
            {
                this.ComputeExtrudedLength();
            }

            if (_logRelativeActions && action.relative)
            {
                logger.Verbose("TCP transform at " + this.position + " " + new Orientation(this.rotation));
            }

            return(true);
        }