public void DetermineLimits()
    {
        MotionLimits  cache  = new MotionLimits();
        DriveSettings driver = wrapped.asmJointOccurrence.DriveSettings;

        driver.DriveType          = DriveTypeEnum.kDriveAngularPositionType;
        driver.CollisionDetection = true;
        driver.OnCollision       += MotionLimits.OnCollisionEvent;
        driver.FrameRate          = 1000;
        float step = 0.05f;

        driver.SetIncrement(IncrementTypeEnum.kAmountOfValueIncrement, step + " rad");

        cache.DoContactSetup(true, wrapped.childGroup, wrapped.parentGroup);

        driver.StartValue = currentAngularPosition + " rad";
        driver.EndValue   = (currentAngularPosition + 6.5) + " rad";

        // Forward
        driver.GoToStart();
        MotionLimits.DID_COLLIDE = false;
        driver.PlayForward();
        if (MotionLimits.DID_COLLIDE)
        {
            angularLimitHigh = (float)wrapped.asmJoint.AngularPosition.Value - step;
            hasAngularLimit  = true;
        }

        // Reverse
        driver.EndValue   = currentAngularPosition + " rad";
        driver.StartValue = (currentAngularPosition - 6.5) + " rad";
        driver.GoToEnd();
        MotionLimits.DID_COLLIDE = false;
        driver.PlayReverse();
        if (MotionLimits.DID_COLLIDE)
        {
            angularLimitLow = (float)wrapped.asmJoint.AngularPosition.Value + step;
            if (!hasAngularLimit)
            {
                angularLimitHigh = angularLimitLow + 6.28f - (step * 2.0f);
            }
            hasAngularLimit = true;
        }
        else if (hasAngularLimit)
        {
            angularLimitLow = angularLimitHigh - 6.28f + (step * 2.0f);
        }

        driver.OnCollision -= MotionLimits.OnCollisionEvent;
        cache.DoContactSetup(false, wrapped.childGroup, wrapped.parentGroup);

        wrapped.asmJoint.AngularPosition.Value = currentAngularPosition;

        Console.WriteLine(hasAngularLimit + "; high: " + angularLimitHigh + "; low: " + angularLimitLow);

        // Stash results
        wrapped.asmJoint.HasAngularPositionLimits = hasAngularLimit;
        if (hasAngularLimit)
        {
            wrapped.asmJoint.AngularPositionStartLimit.Value = angularLimitLow;
            wrapped.asmJoint.AngularPositionEndLimit.Value   = angularLimitHigh;
        }
    }
    public void DetermineLimits()
    {
        MotionLimits  cache  = new MotionLimits();
        DriveSettings driver = wrapped.asmJointOccurrence.DriveSettings;

        driver.CollisionDetection = true;
        driver.OnCollision       += MotionLimits.OnCollisionEvent;
        driver.FrameRate          = 1000;

        cache.DoContactSetup(true, wrapped.childGroup, wrapped.parentGroup);

        {   // Rotational motion
            driver.DriveType = DriveTypeEnum.kDriveAngularPositionType;
            wrapped.asmJoint.LinearPosition.Value = currentLinearPosition;

            float step = 0.05f; // rad
            driver.SetIncrement(IncrementTypeEnum.kAmountOfValueIncrement, step + " rad");

            driver.StartValue = currentAngularPosition + " rad";
            driver.EndValue   = (currentAngularPosition + 6.5) + " rad";

            // Forward
            driver.GoToStart();
            MotionLimits.DID_COLLIDE = false;
            driver.PlayForward();
            if (MotionLimits.DID_COLLIDE)
            {
                angularLimitHigh = (float)wrapped.asmJoint.AngularPosition.Value - step;
                hasAngularLimit  = true;
            }

            // Reverse
            driver.EndValue   = currentAngularPosition + " rad";
            driver.StartValue = (currentAngularPosition - 6.5) + " rad";
            driver.GoToEnd();
            MotionLimits.DID_COLLIDE = false;
            driver.PlayReverse();
            if (MotionLimits.DID_COLLIDE)
            {
                angularLimitLow = (float)wrapped.asmJoint.AngularPosition.Value + step;
                if (!hasAngularLimit)
                {
                    angularLimitHigh = angularLimitLow + 6.28f - (step * 2.0f);
                }
                hasAngularLimit = true;
            }
            else if (hasAngularLimit)
            {
                angularLimitLow = angularLimitHigh - 6.28f + (step * 2.0f);
            }

            wrapped.asmJoint.AngularPosition.Value = currentAngularPosition;

            Console.WriteLine(hasAngularLimit + "; high: " + angularLimitHigh + "; low: " + angularLimitLow);
        }

        {   // Linear motion
            driver.DriveType = DriveTypeEnum.kDriveLinearPositionType;
            wrapped.asmJoint.AngularPosition.Value = currentAngularPosition;

            float step      = 0.1f; // cm
            Box   mover     = (wrapped.childIsTheOne ? wrapped.asmJointOccurrence.OccurrenceOne : wrapped.asmJointOccurrence.OccurrenceTwo).RangeBox;
            float maxOffset = (float)mover.MinPoint.VectorTo(mover.MaxPoint).DotProduct(Utilities.ToInventorVector(axis));
            Console.WriteLine("Max linear offset: " + maxOffset);

            driver.SetIncrement(IncrementTypeEnum.kAmountOfValueIncrement, step + " cm");

            driver.StartValue = currentLinearPosition + " cm";
            driver.EndValue   = (currentLinearPosition + maxOffset) + " cm";

            // Forward
            driver.GoToStart();
            MotionLimits.DID_COLLIDE = false;
            driver.PlayForward();
            if (MotionLimits.DID_COLLIDE)
            {
                linearLimitEnd    = (float)wrapped.asmJoint.LinearPosition.Value - step;
                hasLinearEndLimit = true;
            }

            // Reverse
            driver.EndValue   = currentLinearPosition + " cm";
            driver.StartValue = (currentLinearPosition - maxOffset) + " cm";
            driver.GoToEnd();
            MotionLimits.DID_COLLIDE = false;
            driver.PlayReverse();
            if (MotionLimits.DID_COLLIDE)
            {
                linearLimitStart    = (float)wrapped.asmJoint.LinearPosition.Value + step;
                hasLinearStartLimit = true;
            }

            wrapped.asmJoint.LinearPosition.Value = currentLinearPosition;
            Console.WriteLine(hasLinearStartLimit + " low: " + linearLimitStart + "\t" + hasLinearEndLimit + " high: " + linearLimitEnd);
        }

        driver.OnCollision -= MotionLimits.OnCollisionEvent;
        cache.DoContactSetup(false, wrapped.childGroup, wrapped.parentGroup);

        // Stash results
        wrapped.asmJoint.HasLinearPositionStartLimit = hasLinearStartLimit;
        wrapped.asmJoint.HasLinearPositionEndLimit   = hasLinearEndLimit;
        if (hasLinearStartLimit)
        {
            wrapped.asmJoint.LinearPositionStartLimit.Value = linearLimitStart;
        }
        if (hasLinearEndLimit)
        {
            wrapped.asmJoint.LinearPositionEndLimit.Value = linearLimitEnd;
        }

        wrapped.asmJoint.HasAngularPositionLimits = hasAngularLimit;
        if (hasAngularLimit)
        {
            wrapped.asmJoint.AngularPositionStartLimit.Value = angularLimitLow;
            wrapped.asmJoint.AngularPositionEndLimit.Value   = angularLimitHigh;
        }
    }
Exemple #3
0
    public void DetermineLimits()
    {
        MotionLimits  cache  = new MotionLimits();
        DriveSettings driver = wrapped.asmJointOccurrence.DriveSettings;

        driver.DriveType          = DriveTypeEnum.kDriveLinearPositionType;
        driver.CollisionDetection = true;
        driver.OnCollision       += MotionLimits.OnCollisionEvent;
        driver.FrameRate          = 1;
        float step      = 0.1f;
        Box   mover     = (wrapped.childIsTheOne ? wrapped.asmJointOccurrence.OccurrenceOne : wrapped.asmJointOccurrence.OccurrenceTwo).RangeBox;
        float maxOffset = (float)mover.MinPoint.VectorTo(mover.MaxPoint).DotProduct(Utilities.ToInventorVector(axis));

        driver.SetIncrement(IncrementTypeEnum.kAmountOfValueIncrement, step + " cm");

        cache.DoContactSetup(true, wrapped.childGroup, wrapped.parentGroup);

        driver.StartValue = currentLinearPosition + " cm";
        driver.EndValue   = (currentLinearPosition + maxOffset) + " cm";

        // Forward
        driver.GoToStart();
        MotionLimits.DID_COLLIDE = false;
        driver.PlayForward();
        if (MotionLimits.DID_COLLIDE)
        {
            linearLimitHigh = (float)wrapped.asmJoint.LinearPosition.Value - step;
            hasUpperLimit   = true;
        }

        // Reverse
        driver.EndValue   = currentLinearPosition + " cm";
        driver.StartValue = (currentLinearPosition - maxOffset) + " cm";
        driver.GoToEnd();
        MotionLimits.DID_COLLIDE = false;
        driver.PlayReverse();
        if (MotionLimits.DID_COLLIDE)
        {
            linearLimitLow = (float)wrapped.asmJoint.LinearPosition.Value + step;
            hasLowerLimit  = true;
        }

        driver.OnCollision -= MotionLimits.OnCollisionEvent;
        cache.DoContactSetup(false, wrapped.childGroup, wrapped.parentGroup);

        wrapped.asmJoint.LinearPosition.Value = currentLinearPosition;

        Console.WriteLine(hasLowerLimit + " low: " + linearLimitLow + "\t" + hasUpperLimit + " high: " + linearLimitHigh);

        // Stash results
        wrapped.asmJoint.HasLinearPositionStartLimit = hasLowerLimit;
        wrapped.asmJoint.HasLinearPositionEndLimit   = hasUpperLimit;
        if (hasLowerLimit)
        {
            wrapped.asmJoint.LinearPositionStartLimit.Value = linearLimitLow;
        }
        if (hasUpperLimit)
        {
            wrapped.asmJoint.LinearPositionEndLimit.Value = linearLimitHigh;
        }
    }