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); }
private void printInitialJointPositions() { masterJointA.setQ(0.0); passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); passiveJointD.setQ(0.0); 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("\njoint a position = " + jointAPosition); System.out.println("joint b position = " + jointBPosition); System.out.println("joint c position = " + jointCPosition); System.out.println("joint d position = " + jointDPosition); } }