/** * Builds a new implementation of {@code PrismaticJointBasics}. * * @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 prismatic joint. */ default PrismaticJointBasics buildPrismaticJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, Vector3DReadOnly jointAxis) { return new PrismaticJoint(name, predecessor, transformToParent, jointAxis); }
/** * Generates a prismatic joint with random physical parameters and attaches it to the given * {@code predecessor}. * * @param random the random generator to use. * @param name the joint name. * @param jointAxis used to define the joint axis. * @param predecessor the rigid-body to which the joint is added as a child. * @return the random joint. */ public static PrismaticJoint nextPrismaticJoint(Random random, String name, Vector3DReadOnly jointAxis, RigidBodyBasics predecessor) { RigidBodyTransform transformToParent = predecessor.isRootBody() ? null : EuclidCoreRandomTools.nextRigidBodyTransform(random); return new PrismaticJoint(name, predecessor, transformToParent, jointAxis); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_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); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_String_RigidBody_Vector3d_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, jointOffset, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testTree() { Random random = new Random(1779L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); Vector3D jointAxis = new Vector3D(1.0, 0.0, 0.0); PrismaticJoint j1 = new PrismaticJoint("j1", elevator, new Vector3D(), jointAxis); new RigidBody("r1", j1, getRandomDiagonalMatrix(random), random.nextDouble(), getRandomVector(random)); PrismaticJoint j2 = new PrismaticJoint("j2", elevator, new Vector3D(), jointAxis); new RigidBody("r2", j2, getRandomDiagonalMatrix(random), random.nextDouble(), getRandomVector(random)); 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(ReferenceFrame.getWorldFrame()); comAccelerationCalculator.getCoMAcceleration(comAcceleration); }
ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); Vector3D jointAxis = new Vector3D(1.0, 0.0, 0.0); PrismaticJoint j1 = new PrismaticJoint("j1", elevator, new Vector3D(), jointAxis); RigidBodyBasics r1 = new RigidBody("r1", j1, getRandomDiagonalMatrix(random), random.nextDouble(), getRandomVector(random)); PrismaticJoint j2 = new PrismaticJoint("j2", r1, new Vector3D(), jointAxis); RigidBodyBasics r2 = new RigidBody("r2", j2, getRandomDiagonalMatrix(random), random.nextDouble(), getRandomVector(random));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyTranslation() { Random random = new Random(1766L); RigidBodyBasics elevator = new RigidBody("elevator", world); Vector3D jointAxis = RandomGeometry.nextVector3D(random); jointAxis.normalize(); PrismaticJoint joint = new PrismaticJoint("joint", elevator, new Vector3D(), 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(joint.getFrameBeforeJoint(), jointAxis); linearMomentumCheck.scale(body.getInertia().getMass() * joint.getQd()); FrameVector3D angularMomentumCheck = new FrameVector3D(world); double epsilon = 1e-9; EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon); assertTrue(linearMomentum.length() > epsilon); }
private void buildMechanismAndJacobians() { RigidBodyBasics base = new RigidBody("base", ReferenceFrame.getWorldFrame()); joint1 = new PrismaticJoint("joint1", base, new Vector3D(), new Vector3D(0.0, -1.0, 0.0)); RigidBodyBasics body1 = new RigidBody("body1", joint1, new Matrix3D(), 0.0, new Vector3D()); joint2 = new RevoluteJoint("joint2", body1, new Vector3D(0.0, 0.0, 3.0), new Vector3D(-1.0, 0.0, 0.0)); RigidBodyBasics body2 = new RigidBody("body2", joint2, new Matrix3D(), 0.0, new Vector3D()); joint3 = new PrismaticJoint("joint4", body2, new Vector3D(), new Vector3D(0.0, 0.0, 1.0)); RigidBodyBasics body3 = new RigidBody("body3", joint3, new Matrix3D(), 0.0, new Vector3D()); // create Jacobians bodyManipulatorJacobian = new GeometricJacobian(base, body3, joint3.getFrameAfterJoint()); spatialManipulatorJacobian = new GeometricJacobian(base, body3, joint1.getFrameBeforeJoint()); } }
inverseDynamicsJoint = new PrismaticJoint(joint.getName(), parentBody, offset, jointAxis);
inverseDynamicsJoint = new PrismaticJoint(joint.getName(), parentBody, offset, jointAxis);
currentJoint.getOffset(jointOffset); PrismaticJoint currentIDJoint = new PrismaticJoint(currentJoint.getName(), parentIDBody, jointOffset, jointAxis); currentIDJoint.setJointLimitLower(currentJoint.getJointLowerLimit()); currentIDJoint.setJointLimitUpper(currentJoint.getJointUpperLimit());