예제 #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 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);
        }
예제 #3
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");
            }
        }