private void printJointPositionsAndAngles() { System.out.println("joint angles:"); System.out.println("joint a = " + masterJointA.getQ() / Math.PI); System.out.println("joint b = " + passiveJointB.getQ() / Math.PI); System.out.println("joint c = " + passiveJointC.getQ() / Math.PI); System.out.println("joint d = " + passiveJointD.getQ() / Math.PI); FramePoint3D jointAPosition = new FramePoint3D(masterJointA.getFrameBeforeJoint()); FramePoint3D jointBPosition = new FramePoint3D(passiveJointB.getFrameBeforeJoint()); FramePoint3D jointCPosition = new FramePoint3D(passiveJointC.getFrameBeforeJoint()); FramePoint3D jointDPosition = new FramePoint3D(passiveJointD.getFrameBeforeJoint()); jointAPosition.changeFrame(worldFrame); jointBPosition.changeFrame(worldFrame); jointCPosition.changeFrame(worldFrame); jointDPosition.changeFrame(worldFrame); System.out.println("joint a position = " + jointAPosition); System.out.println("joint b position = " + jointBPosition); System.out.println("joint c position = " + jointCPosition); System.out.println("joint d position = " + jointDPosition); }
masterJointA.setQd(0.0); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.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); masterJointA.setQd(masterJointVelocity); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), -0.5 * Math.PI + angleEpsilon, 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); masterJointA.setQd(masterJointVelocity); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.5 * Math.PI - angleEpsilon, 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);
fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.0, eps); assertEquals(passiveJointC.getQ(), 0.0, eps); assertEquals(passiveJointD.getQ(), -0.5 * Math.PI, eps); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), -0.25 * Math.PI, eps); assertEquals(passiveJointC.getQ(), 0.25 * Math.PI, eps); assertEquals(passiveJointD.getQ(), -0.75 * Math.PI, eps);
fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.25 * Math.PI, eps); 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); masterJointA.setQd(masterQd); 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);