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); } }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBodyBasics parentBody, RigidBodyTransform transformToParent, Vector3D jointAxis, boolean isPartOfClosedKinematicLoop) { return new PassiveRevoluteJoint(jointName, parentBody, transformToParent, jointAxis, isPartOfClosedKinematicLoop); }
public static void checkCorrectJointOrder(String fourBarName, RevoluteJoint masterJointA, PassiveRevoluteJoint passiveJointB, PassiveRevoluteJoint passiveJointC, PassiveRevoluteJoint passiveJointD) { boolean successorAisPredecessorB = masterJointA.getSuccessor() == passiveJointB.getPredecessor(); boolean successorBisPredecessorC = passiveJointB.getSuccessor() == passiveJointC.getPredecessor(); boolean succesorCisPredecessorD = passiveJointC.getSuccessor() == passiveJointD.getPredecessor(); if (!successorAisPredecessorB || !successorBisPredecessorC || !succesorCisPredecessorD) { throw new RuntimeException("The joints that form the " + fourBarName + " four bar must be passed in clockwise or counterclockwise order"); } if (DEBUG) { System.out.println("\nDebugging check joint order:\n\nsuccessor \t predecessor\n" + masterJointA.getSuccessor() + "\t " + passiveJointB.getPredecessor() + "\n" + passiveJointB.getSuccessor() + "\t " + passiveJointC.getPredecessor() + "\n" + passiveJointC.getSuccessor() + "\t " + passiveJointD.getPredecessor() + "\n"); } }
passiveJointB.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleABC(), 1)); passiveJointC.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleBCD(), 2)); passiveJointD.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleCDA(), 3)); passiveJointB.setQd(jointSigns[1] * fourBarCalculator.getAngleDtABC()); passiveJointC.setQd(jointSigns[2] * fourBarCalculator.getAngleDtBCD()); passiveJointD.setQd(jointSigns[3] * fourBarCalculator.getAngleDtCDA()); passiveJointB.setQdd(jointSigns[1] * fourBarCalculator.getAngleDt2ABC()); passiveJointC.setQdd(jointSigns[2] * fourBarCalculator.getAngleDt2BCD()); passiveJointD.setQdd(jointSigns[3] * fourBarCalculator.getAngleDt2CDA());
FrameVector3D jointBAxis = new FrameVector3D(passiveJointB.getJointAxis()); FrameVector3D jointCAxis = new FrameVector3D(passiveJointC.getJointAxis()); FrameVector3D jointDAxis = new FrameVector3D(passiveJointD.getJointAxis()); FourBarKinematicLoopTools.checkJointAxesAreParallel(masterAxis, jointBAxis, jointCAxis, jointDAxis); passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); passiveJointD.setQ(0.0); masterJointA.getFrameAfterJoint().update(); passiveJointB.getFrameAfterJoint().update(); passiveJointC.getFrameAfterJoint().update(); passiveJointD.getFrameAfterJoint().update();
private void testPlanarSquare(boolean recomputeJointLimits, Vector3DReadOnly jointAtoD) passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); passiveJointD.setQ(0.0); 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); assertEquals(passiveJointD.getQd(), 0.0, eps); assertEquals(passiveJointB.getQdd(), 0.0, eps); assertEquals(passiveJointC.getQdd(), 0.0, eps); assertEquals(passiveJointD.getQdd(), 0.0, eps); assertEquals(passiveJointB.getQ(), -0.25 * Math.PI, eps); assertEquals(passiveJointC.getQ(), 0.25 * Math.PI, 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(passiveJointB.getQdd(), -1.0, eps); assertEquals(passiveJointC.getQdd(), 1.0, eps); assertEquals(passiveJointD.getQdd(), -1.0, eps);
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointB.getJointLimitLower(), 1, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointB.getJointLimitUpper(), 1, true); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointC.getJointLimitLower(), 2, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointC.getJointLimitUpper(), 2, true); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointD.getJointLimitLower(), 3, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointD.getJointLimitUpper(), 3, true);
FrameVector jointBAxis = passiveJointB.getJointAxis(); FrameVector jointCAxis = passiveJointC.getJointAxis(); FrameVector jointDAxis = passiveJointD.getJointAxis(); FourBarKinematicLoopTools.checkJointAxesAreParallel(masterAxis, jointBAxis, jointCAxis, jointDAxis); passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); passiveJointD.setQ(0.0);
private void initializeFourBar(RigidBodyTransform jointAtoElevator, RigidBodyTransform jointBtoA, RigidBodyTransform jointCtoB, RigidBodyTransform jointDtoC, Vector3D jointAxisA, Vector3D jointAxisB, Vector3D jointAxisC, Vector3D jointAxisD) { masterJointA = new RevoluteJoint("jointA", elevator, jointAtoElevator, jointAxisA); rigidBodyAB = createAndAttachCylinderRB("rigidBodyAB", masterJointA); passiveJointB = ScrewTools.addPassiveRevoluteJoint("jointB", rigidBodyAB, jointBtoA, jointAxisB, true); rigidBodyBC = createAndAttachCylinderRB("rigidBodyBC", passiveJointB); passiveJointC = ScrewTools.addPassiveRevoluteJoint("jointC", rigidBodyBC, jointCtoB, jointAxisC, true); rigidBodyCD = createAndAttachCylinderRB("rigidBodyCD", passiveJointC); passiveJointD = ScrewTools.addPassiveRevoluteJoint("jointD", rigidBodyCD, jointDtoC, jointAxisD, true); rigidBodyDA = createAndAttachCylinderRB("rigidBodyCD", passiveJointD); masterJointA.setQ(random.nextDouble()); passiveJointB.setQ(random.nextDouble()); passiveJointC.setQ(random.nextDouble()); passiveJointD.setQ(random.nextDouble()); elevator.updateFramesRecursively(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetQd() { try { joint.setQd(qd); } catch(RuntimeException e) { return; } Assert.fail(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetQdd() { try { joint.setQdd(qdd); } catch(RuntimeException e) { return; } Assert.fail(); }
public static PassiveRevoluteJoint getFourBarOutputJoint(PassiveRevoluteJoint passiveJointB, PassiveRevoluteJoint passiveJointC, PassiveRevoluteJoint passiveJointD) { // If the output joint is D then it will have at least 1 child, otherwise it won't have any if(passiveJointD.getSuccessor().hasChildrenJoints()) { return passiveJointD; } // Joint C wil only have joint D as its child, unless it's the output joint of the fourbar else if (passiveJointC.getSuccessor().getChildrenJoints().size() > 1) { return passiveJointC; } else { return passiveJointB; } }
private void initializeInteriorAnglesAtZeroConfigurationAndJointSigns(FrameVector2D vectorDAProjected, FrameVector2D vectorABProjected, FrameVector2D vectorBCProjected, FrameVector2D vectorCDProjected) FrameVector3D jointBAxis = new FrameVector3D(passiveJointB.getJointAxis()); FrameVector3D jointCAxis = new FrameVector3D(passiveJointC.getJointAxis()); FrameVector3D jointDAxis = new FrameVector3D(passiveJointD.getJointAxis());
passiveJointB.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleABC(), 1)); passiveJointC.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleBCD(), 2)); passiveJointD.setQ(convertInteriorAngleToJointAngle(fourBarCalculator.getAngleCDA(), 3)); passiveJointB.setQd(jointSigns[1] * fourBarCalculator.getAngleDtABC()); passiveJointC.setQd(jointSigns[2] * fourBarCalculator.getAngleDtBCD()); passiveJointD.setQd(jointSigns[3] * fourBarCalculator.getAngleDtCDA()); passiveJointB.setQdd(jointSigns[1] * fourBarCalculator.getAngleDt2ABC()); passiveJointC.setQdd(jointSigns[2] * fourBarCalculator.getAngleDt2BCD()); passiveJointD.setQdd(jointSigns[3] * fourBarCalculator.getAngleDt2CDA());
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointB.getJointLimitLower(), 1, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointB.getJointLimitUpper(), 1, true); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointC.getJointLimitLower(), 2, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointC.getJointLimitUpper(), 2, true); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointD.getJointLimitLower(), 3, false); addJointLimitToBoundsIfValid(lowerBounds, upperBounds, jointD.getJointLimitUpper(), 3, true);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetQ() { try { joint.setQ(q); } catch(RuntimeException e) { return; } Assert.fail(); }
public static PassiveRevoluteJoint getFourBarOutputJoint(PassiveRevoluteJoint passiveJointB, PassiveRevoluteJoint passiveJointC, PassiveRevoluteJoint passiveJointD) { // If the output joint is D then it will have at least 1 child, otherwise it won't have any if(passiveJointD.getSuccessor().hasChildrenJoints()) { return passiveJointD; } // Joint C wil only have joint D as its child, unless it's the output joint of the fourbar else if (passiveJointC.getSuccessor().getChildrenJoints().size() > 1) { return passiveJointC; } else { return passiveJointB; } }
private void initializeInteriorAnglesAtZeroConfigurationAndJointSigns(FrameVector2d vectorDAProjected, FrameVector2d vectorABProjected, FrameVector2d vectorBCProjected, FrameVector2d vectorCDProjected) FrameVector jointBAxis = passiveJointB.getJointAxis(); FrameVector jointCAxis = passiveJointC.getJointAxis(); FrameVector jointDAxis = passiveJointD.getJointAxis();