public void verifyMasterJointLimits() { double maxValidMasterJointAngle = fourBarIsClockwise ? convertInteriorAngleToJointAngle(fourBarCalculator.getMaxDAB(), 0) : convertInteriorAngleToJointAngle(fourBarCalculator.getMinDAB(), 0); double minValidMasterJointAngle = fourBarIsClockwise ? convertInteriorAngleToJointAngle(fourBarCalculator.getMinDAB(), 0) : convertInteriorAngleToJointAngle(fourBarCalculator.getMaxDAB(), 0); // A) Angle limits not set if (masterJointA.getJointLimitLower() == Double.NEGATIVE_INFINITY || masterJointA.getJointLimitUpper() == Double.POSITIVE_INFINITY) { throw new RuntimeException( "Must set the joint limits for the master joint of the " + name + " four bar.\nNote that for the given link lengths max angle is " + maxValidMasterJointAngle + "and min angle is" + minValidMasterJointAngle); } // B) Max angle limit is too large if (masterJointA.getJointLimitUpper() > maxValidMasterJointAngle + eps) { throw new RuntimeException("The maximum valid joint angle for the master joint of the " + name + " four bar is " + maxValidMasterJointAngle + " to avoid flipping, but was set to " + masterJointA.getJointLimitUpper()); } // C) Min angle limit is too small if (masterJointA.getJointLimitLower() < minValidMasterJointAngle - eps) { throw new RuntimeException("The minimum valid joint angle for the master joint of the " + name + " four bar is " + minValidMasterJointAngle + " to avoid flipping, but was set to " + masterJointA.getJointLimitLower()); } }
addJointLimitToBoundsIfValid(lowerBounds, upperBounds, masterJointA.getJointLimitLower(), 0, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, masterJointA.getJointLimitUpper(), 0, true); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointB.getJointLimitLower(), 1, false);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRecomputingJointLimits_NoLimitsAreSet_UnitSquare() { // initialize to a square of unit length Vector3D jointAxis = new Vector3D(0.0, 0.0, 1.0); Vector3D elevatorToJointA = new Vector3D(); Vector3D jointAtoB = new Vector3D(1.0, 0.0, random.nextDouble()); Vector3D jointBtoC = new Vector3D(0.0, 1.0, random.nextDouble()); Vector3D jointCtoD = new Vector3D(-1.0, 0.0, random.nextDouble()); Vector3D jointAtoD = new Vector3D(0.0, 1.0, random.nextDouble()); initializeFourBar(elevatorToJointA, jointAtoB, jointBtoC, jointCtoD, jointAxis, 1); boolean recomputeJointLimits = true; fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); // check that joint limits are consistent with four bar kinematic constraints double jointAMin = masterJointA.getJointLimitLower(); double jointAMax = masterJointA.getJointLimitUpper(); assertEquals(jointAMin, -0.5 * Math.PI, 1e-7); assertEquals(jointAMax, 0.5 * Math.PI, 1e-7); }
maxMasterInteriorAngle = Math.PI; double jointAMinActual = masterJointA.getJointLimitLower(); double jointAMaxActual = masterJointA.getJointLimitUpper();
fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); double masterJointALower = masterJointA.getJointLimitLower(); double masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, - angleEpsilon, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -angleEpsilon, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -angleEpsilon, 0.01 * angleEpsilon);
fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); double masterJointALower = masterJointA.getJointLimitLower(); double masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -0.5 * Math.PI + angleEpsilon, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -0.5 * Math.PI, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -0.5 * Math.PI + angleEpsilon, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -0.5 * Math.PI, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper(); assertEquals(masterJointALower, -0.5 * Math.PI + angleEpsilon, 0.01 * angleEpsilon);
passiveJointB.setJointLimitUpper(0.5 * Math.PI - angleEpsilon1); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); assertEquals(masterJointA.getJointLimitLower(), -0.5 * Math.PI + angleEpsilon0, eps); assertEquals(masterJointA.getJointLimitUpper(), 0.5 * Math.PI - angleEpsilon1, eps); passiveJointC.setJointLimitUpper(0.5 * Math.PI - angleEpsilon1); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); assertEquals(masterJointA.getJointLimitLower(), -0.5 * Math.PI + angleEpsilon1, eps); assertEquals(masterJointA.getJointLimitUpper(), 0.5 * Math.PI - angleEpsilon0, eps); passiveJointD.setJointLimitUpper(- angleEpsilon1); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); assertEquals(masterJointA.getJointLimitLower(), - 0.5 * Math.PI + angleEpsilon0, eps); assertEquals(masterJointA.getJointLimitUpper(), 0.5 * Math.PI - angleEpsilon1, eps);
double interiorAngleDt2A = jointSigns[0] * masterJointA.getQdd(); if (currentMasterJointA < masterJointA.getJointLimitLower() || currentMasterJointA > masterJointA.getJointLimitUpper()) masterJointA.getName() + " is set outside of its bounds [" + masterJointA.getJointLimitLower() + ", " + masterJointA.getJointLimitUpper() + "]. The current value is: " + masterJointA.getQ());