예제 #1
0
        /// <summary>
        /// Apply Rotation Action.
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionRotation action)
        {
            Rotation newRot;

            // @TODO: implement some kind of security check here...
            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 rotation values first before applying relative ones... ");
                    return(false);
                }

                prevRotation = rotation;
                if (referenceCS == ReferenceCS.World)
                {
                    //rotation.RotateGlobal(action.rotation);
                    newRot = Rotation.Global(rotation, action.rotation);  // @TODO: TEST THIS
                }
                else
                {
                    //rotation.RotateLocal(action.rotation);
                    newRot = Rotation.Local(rotation, action.rotation);  // @TODO: TEST THIS
                }
            }
            else
            {
                // Fail if issued abs rotation without prior position info (this limitation is due to current lack of FK/IK solvers).
                if (position == null)
                {
                    logger.Warning($"Cannot apply \"{action}\", currently missing TCP position to work with... ");
                    return(false);
                }

                newRot = new Rotation(action.rotation);
            }

            prevRotation = rotation;
            rotation     = newRot;

            prevPosition = position;  // to flag same-position change

            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 orientation at " + new Orientation(this.rotation));
            }

            return(true);
        }
예제 #2
0
        /// <summary>
        /// Apply Rotation Action.
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionRotation action)
        {
            Rotation newRot;

            // @TODO: implement some kind of security check here...
            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)
                {
                    Console.WriteLine("Sorry, must provide absolute rotation values first before applying relative ones... " + this);
                    return(false);
                }

                prevRotation = rotation;
                if (referenceCS == ReferenceCS.World)
                {
                    //rotation.RotateGlobal(action.rotation);
                    newRot = Rotation.Global(rotation, action.rotation);  // @TODO: TEST THIS
                }
                else
                {
                    //rotation.RotateLocal(action.rotation);
                    newRot = Rotation.Local(rotation, action.rotation);  // @TODO: TEST THIS
                }
            }
            else
            {
                // Fail if issued abs rotation without prior position info (this limitation is due to current lack of FK/IK solvers).
                if (position == null)
                {
                    Console.WriteLine("Sorry, currently missing TCP position to work with... " + this);
                    return(false);
                }

                newRot = new Rotation(action.rotation);
            }

            prevRotation = rotation;
            rotation     = newRot;

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

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

            return(true);
        }