private RandomFloatingRevoluteJointChain getRandomFloatingChain() { Random random = new Random(); Vector3D[] jointAxes = {new Vector3D(1.0, 0.0, 0.0)}; RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); return randomFloatingChain; }
private RandomFloatingRevoluteJointChain getRandomFloatingChain(int minNumberOfAxes, int maxNumberOfAxes) { Random random = new Random(519651L); int numberOfAxes = RandomNumbers.nextInt(random, minNumberOfAxes, maxNumberOfAxes); Vector3D[] jointAxes = new Vector3D[numberOfAxes]; for (int i = 0; i < numberOfAxes; i++) { jointAxes[i] = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); } RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); return randomFloatingChain; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDegreesOfFreedom_Iterable() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>(jointsArray.length); RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator()); RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1]; bodiesArray[0] = chain.getElevator(); for(int i = 0; i < partialBodiesArray.length; i++) { bodiesArray[i+1] = partialBodiesArray[i]; } MultiBodySystemTools.computeDegreesOfFreedom(jointsList); }
}; RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
}; RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRevoluteJoint_String_RigidBody_Vector3d_Vector3d() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics[] rootBodies = {chain.getElevator()}; JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies); RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator()); RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1]; bodiesArray[0] = chain.getElevator(); for(int i = 0; i < partialBodiesArray.length; i++) { bodiesArray[i+1] = partialBodiesArray[i]; } String jointName = "joint"; RigidBodyBasics parentBody = bodiesArray[bodiesArray.length - 1]; Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); assertTrue(jointAxis.equals(joint.getJointAxis())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCreateGravitationalSpatialAcceleration() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); double gravity = RandomNumbers.nextDouble(random, 100.0); SpatialAcceleration result = ScrewTools. createGravitationalSpatialAcceleration(chain.getElevator(), gravity); Vector3DReadOnly angularPart = result.getAngularPart(); Vector3D zeroes = new Vector3D(0.0, 0.0, 0.0); assertTrue(angularPart.epsilonEquals(zeroes, epsilon)); Vector3DReadOnly linearPart = result.getLinearPart(); assertEquals(zeroes.getX(), linearPart.getX(), epsilon); assertEquals(zeroes.getY(), linearPart.getY(), epsilon); assertEquals(gravity, linearPart.getZ(), epsilon); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDegreesOfFreedom_Array() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics[] rootBodies = {chain.getElevator()}; JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies); RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator()); RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1]; bodiesArray[0] = chain.getElevator(); for(int i = 0; i < partialBodiesArray.length; i++) { bodiesArray[i+1] = partialBodiesArray[i]; } int result = MultiBodySystemTools.computeDegreesOfFreedom(jointsArray); assertEquals(11, result); }