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(MathUtils.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;
}
}
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(MathUtils.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;
}
}
