assertEquals(passiveJointB.getQd(), -1.0, eps); assertEquals(passiveJointC.getQ(), 0.25 * Math.PI, eps); assertEquals(passiveJointC.getQd(), 1.0, eps); assertEquals(passiveJointD.getQ(), 0.5 * Math.PI, eps); assertEquals(passiveJointD.getQd(), -1.0, eps); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), - masterQ + 0.25 * Math.PI, eps); assertEquals(passiveJointB.getQd(), - masterQd, eps); assertEquals(passiveJointC.getQ(), masterQ + 0.25 * Math.PI, eps); assertEquals(passiveJointC.getQd(), masterQd, eps); assertEquals(passiveJointD.getQ(), - masterQ + 0.5 * Math.PI, eps); assertEquals(passiveJointD.getQd(), - masterQd, eps);
assertEquals(passiveJointD.getQ(), -0.5 * Math.PI, eps); assertEquals(passiveJointB.getQd(), 0.0, eps); assertEquals(passiveJointC.getQd(), 0.0, eps); assertEquals(passiveJointD.getQd(), 0.0, eps); assertEquals(passiveJointD.getQ(), -0.75 * Math.PI, eps); assertEquals(passiveJointB.getQd(), -1.0, eps); assertEquals(passiveJointC.getQd(), 1.0, eps); assertEquals(passiveJointD.getQd(), -1.0, eps);
assertEquals(passiveJointC.getQ(), 0.0, eps); assertEquals(passiveJointD.getQ(), 0.5 * Math.PI, eps); assertEquals(passiveJointB.getQd(), 0.0, eps); assertEquals(passiveJointC.getQd(), 0.0, eps); assertEquals(passiveJointD.getQd(), 0.0, eps); assertEquals(passiveJointC.getQ(), 0.5 * Math.PI - angleEpsilon, eps); assertEquals(passiveJointD.getQ(), angleEpsilon, eps); assertEquals(passiveJointB.getQd(), -masterJointVelocity, eps); assertEquals(passiveJointC.getQd(), masterJointVelocity, eps); assertEquals(passiveJointD.getQd(), -masterJointVelocity, eps); assertEquals(passiveJointC.getQ(), -0.5 * Math.PI + angleEpsilon, eps); assertEquals(passiveJointD.getQ(), Math.PI - angleEpsilon, eps); assertEquals(passiveJointB.getQd(), -masterJointVelocity, eps); assertEquals(passiveJointC.getQd(), masterJointVelocity, eps); assertEquals(passiveJointD.getQd(), -masterJointVelocity, eps);