/** * If part of a kinematic loop, this should only be called by that loop's calculator */ @Override public void setQd(double qd) { super.setQd(qd); }
double jointPosition = random.nextDouble(); currentJoint.setQ(jointPosition); currentJoint.setQd(0.0); currentJoint.setQdd(0.0); new RigidBody("bodyID" + i, currentJoint, momentOfInertia, mass, comOffset);
currentIDJoint.setQd(jointVelocity); currentIDJoint.setQdd(jointAcceleration); new RigidBody("bodyID" + i, currentIDJoint, momentOfInertia, mass, comOffset);
joint.setQd(random.nextDouble());
currentIDJoint.setQd(jointVelocity); currentIDJoint.setQdd(jointAcceleration);
masterJointA.setQd(qdMaster); MultiBodySystemTools.getRootBody(masterJointA.getPredecessor()).updateFramesRecursively(); fourBarKinematicLoop.update();
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPendulumCentripetalAcceleration() { Random random = new Random(1779L); double mass = random.nextDouble(); double qd = random.nextDouble(); double length = 3.0; Vector3D comOffset = new Vector3D(0.0, 0.0, length); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); Vector3D jointAxis = new Vector3D(0.0, 1.0, 0.0); RevoluteJoint j1 = new RevoluteJoint("j1", elevator, new Vector3D(), jointAxis); new RigidBody("r1", j1, getRandomDiagonalMatrix(random), mass, comOffset); j1.setQ(random.nextDouble()); j1.setQd(qd); j1.setQdd(0.0); elevator.updateFramesRecursively(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(elevatorFrame, worldFrame, elevatorFrame); SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(elevator, ReferenceFrame.getWorldFrame()); spatialAccelerationCalculator.setRootAcceleration(rootAcceleration); CenterOfMassAccelerationCalculator comAccelerationCalculator = new CenterOfMassAccelerationCalculator(elevator, spatialAccelerationCalculator); spatialAccelerationCalculator.reset(); FrameVector3D comAcceleration = new FrameVector3D(worldFrame); comAccelerationCalculator.getCoMAcceleration(comAcceleration); assertEquals(length * qd * qd, comAcceleration.length(), 1e-5); }
@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); }
masterJointA.setQd(0.0); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.0, eps); double angleEpsilon = 1e-4; masterJointA.setQ(0.5 * Math.PI - angleEpsilon); masterJointA.setQd(masterJointVelocity); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), -0.5 * Math.PI + angleEpsilon, eps); masterJointA.setQd(masterJointVelocity); fourBarKinematicLoop.update(); assertEquals(passiveJointB.getQ(), 0.5 * Math.PI - angleEpsilon, eps);