private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(jointName, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
inverseDynamicsParentBody = inverseDynamicsParentJoint.getSuccessor(); RevoluteJoint currentJoint = new RevoluteJoint("jointID" + i, inverseDynamicsParentBody, jointOffset, jointAxis); double jointPosition = random.nextDouble(); currentJoint.setQ(jointPosition); currentJoint.setQd(0.0); currentJoint.setQdd(0.0); new RigidBody("bodyID" + i, currentJoint, momentOfInertia, mass, comOffset);
private void initializeJointLimits(double lowerLimitA, double upperLimitA, double lowerLimitB, double upperLimitB, double lowerLimitC, double upperLimitC, double lowerLimitD, double upperLimitD) { masterJointA.setJointLimitLower(lowerLimitA); masterJointA.setJointLimitUpper(upperLimitA); passiveJointB.setJointLimitLower(lowerLimitB); passiveJointB.setJointLimitUpper(upperLimitB); passiveJointC.setJointLimitLower(lowerLimitC); passiveJointC.setJointLimitUpper(upperLimitC); passiveJointD.setJointLimitLower(lowerLimitD); passiveJointD.setJointLimitUpper(upperLimitD); }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { revoluteJoint.setQ(revoluteJoint.getQ() + RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { // revoluteJoint.setQ(revoluteJoint.getQ() + RandomTools.generateRandomDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.setQ(RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
/** * Builds a new implementation of {@code RevoluteJointBasics}. * * @param name the joint name. * @param predecessor the predecessor of the joint. * @param transformToParent the transform to the frame after the parent joint. * @param jointAxis the joint axis. * @return the new revolute joint. */ default RevoluteJointBasics buildRevoluteJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, Vector3DReadOnly jointAxis) { return new RevoluteJoint(name, predecessor, transformToParent, jointAxis); }
FrameVector3D masterJointAxis = new FrameVector3D(masterJointA.getJointAxis()); masterJointAxis.changeFrame(masterJointA.getFrameBeforeJoint()); frameBeforeFourBarWithZAlongJointAxis = GeometryTools .constructReferenceFrameFromPointAndAxis(name + "FrameWithZAlongJointAxis", new FramePoint3D(masterJointA.getFrameBeforeJoint()), Axis.Z, masterJointAxis); FrameVector3D masterAxis = new FrameVector3D(masterJointA.getJointAxis()); FrameVector3D jointBAxis = new FrameVector3D(passiveJointB.getJointAxis()); FrameVector3D jointCAxis = new FrameVector3D(passiveJointC.getJointAxis()); masterJointA.setQ(0.0); passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); passiveJointD.setQ(0.0); masterJointA.getFrameAfterJoint().update(); passiveJointB.getFrameAfterJoint().update(); passiveJointC.getFrameAfterJoint().update(); masterJointA.setJointLimitLower(masterJointBounds[0]); masterJointA.setJointLimitUpper(masterJointBounds[1]); System.out.println("\nOutput joint: " + fourBarOutputJoint.getName() + "\n"); double qA = masterJointA.getQ(); double qB = passiveJointB.getQ();
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRevoluteJoint_String_RigidBody_Transform3D_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); RigidBodyTransform transformToParent = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); assertTrue(jointAxis.equals(joint.getJointAxis())); }
RevoluteJoint parentJoint = new RevoluteJoint(rootJoint.getJointName(false), elevator, rootJoint.getJointAxis()); robotArmRevoluteJoints.put(rootJoint, parentJoint); parentJoint = new RevoluteJoint(armJoint.getJointName(false), parentBody, armJoint.getJointOffset(), armJoint.getJointAxis()); robotArmRevoluteJoints.put(armJoint, parentJoint); robotArmRevoluteJoint.setJointLimitLower(armJoint.getJointLowerLimit()); robotArmRevoluteJoint.setJointLimitUpper(armJoint.getJointUpperLimit());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyRotation() { Random random = new Random(1766L); RigidBodyBasics elevator = new RigidBody("elevator", world); Vector3D jointAxis = RandomGeometry.nextVector3D(random); jointAxis.normalize(); RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setIdentity(); RevoluteJoint joint = new RevoluteJoint("joint", elevator, transformToParent, jointAxis); RigidBodyBasics body = new RigidBody("body", joint, RandomGeometry.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D()); joint.setQ(random.nextDouble()); joint.setQd(random.nextDouble()); Momentum momentum = computeMomentum(elevator, world); momentum.changeFrame(world); FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart())); FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart())); FrameVector3D linearMomentumCheck = new FrameVector3D(world); Matrix3D inertia = new Matrix3D(body.getInertia().getMomentOfInertia()); Vector3D angularMomentumCheckVector = new Vector3D(jointAxis); angularMomentumCheckVector.scale(joint.getQd()); inertia.transform(angularMomentumCheckVector); FrameVector3D angularMomentumCheck = new FrameVector3D(body.getInertia().getReferenceFrame(), angularMomentumCheckVector); angularMomentumCheck.changeFrame(world); double epsilon = 1e-9; EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon); assertTrue(angularMomentum.length() > epsilon); }
public static void main(String[] args) { Random random = new Random(); int numberOfJoints = 5; List<RevoluteJoint> randomChainRobot = MultiBodySystemRandomTools.nextRevoluteJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, randomChainRobot.get(0).getPredecessor()); Twist dummyTwist = new Twist(); while (true) { twistCalculator.compute(); for (int i = 0; i < 100; i++) twistCalculator.getTwistOfBody(randomChainRobot.get(random.nextInt(numberOfJoints)).getSuccessor(), dummyTwist); } } }
int numberOfJoints = 100; List<RevoluteJoint> revoluteJoints = MultiBodySystemRandomTools.nextRevoluteJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, revoluteJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); RigidBodyBasics body = joint.getSuccessor(); Twist actualTwist = new Twist(); twistCalculator.getTwistOfBody(body, actualTwist); FrameVector3D jointAxis = new FrameVector3D(parentJoint.getJointAxis()); cumulatedAngularVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedAngularVelocity.scaleAdd(parentJoint.getQd(), jointAxis, cumulatedAngularVelocity); currentBody = parentJoint.getPredecessor();
/** * If part of a kinematic loop, this should only be called by that loop's calculator */ @Override public void setQ(double q) { super.setQ(q); }
public void update() double currentMasterJointA = masterJointA.getQ(); double interiorAngleA = convertJointAngleToInteriorAngle(currentMasterJointA, 0); double interiorAngleDtA = jointSigns[0] * masterJointA.getQd(); 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()); CommonOps.scale(masterJointA.getQd(), columnJacobian);
/** * Gets the only leaf body, i.e. the rigid-body without any children joints, it is also called the * end-effector. * * @return the leaf body. */ public RigidBodyBasics getLeafBody() { int nRevoluteJoints = revoluteJoints.size(); if (nRevoluteJoints > 0) return revoluteJoints.get(nRevoluteJoints - 1).getSuccessor(); else return rootJoint.getSuccessor(); } }
masterJointA.setJointLimitLower(-0.5 * Math.PI + angleEpsilon); masterJointA.setJointLimitUpper(0.5 * Math.PI - 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); assertEquals(masterJointAUpper, 0.5 * Math.PI - angleEpsilon, 0.01 * angleEpsilon); masterJointA.setJointLimitLower(-0.5 * Math.PI - angleEpsilon); masterJointA.setJointLimitUpper(0.5 * Math.PI + 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); assertEquals(masterJointAUpper, 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); assertEquals(masterJointAUpper, 0.5 * Math.PI - angleEpsilon, 0.01 * angleEpsilon); fourBarKinematicLoop = new FourBarKinematicLoop("fourBar", masterJointA, passiveJointB, passiveJointC, passiveJointD, jointAtoD, recomputeJointLimits); masterJointALower = masterJointA.getJointLimitLower(); masterJointAUpper = masterJointA.getJointLimitUpper();
private void testPlanarSquare(boolean recomputeJointLimits, Vector3DReadOnly jointAtoD) masterJointA.setQ(0.0); passiveJointB.setQ(0.0); passiveJointC.setQ(0.0); masterJointA.setQ(0.0); masterJointA.setQd(0.0); masterJointA.setQdd(0.0); fourBarKinematicLoop.update(); masterJointA.setQ(0.25 * Math.PI); masterJointA.setQd(1.0); masterJointA.setQdd(1.0); fourBarKinematicLoop.update(); masterJointA.setQ(-0.6 * Math.PI); fourBarKinematicLoop.update(); fail(); masterJointA.setQ(0.6 * Math.PI); fourBarKinematicLoop.update(); fail();
double qdMaster = RandomNumbers.nextDouble(random, -2.0, 2.0); masterJointA.setQ(qMaster); masterJointA.setQd(qdMaster); MultiBodySystemTools.getRootBody(masterJointA.getPredecessor()).updateFramesRecursively(); fourBarKinematicLoop.update(); fourBarKinematicLoop.getJacobian(jacobian);