/** * Generates a random state and update the given {@code joints} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param joints the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, JointBasics[] joints) { for (JointBasics joint : joints) nextState(random, stateToRandomize, joint); }
/** * Generates a random state and update the given {@code joints} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param min the minimum value for the generated random values. * @param max the maximum value for the generated random values. * @param joints the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, double min, double max, OneDoFJointBasics[] joints) { for (OneDoFJointBasics joint : joints) nextState(random, stateToRandomize, min, max, joint); }
/** * Generates a random state and update the given {@code joints} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param joints the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, Iterable<? extends JointBasics> joints) { joints.forEach(joint -> nextState(random, stateToRandomize, joint)); }
/** * Generates a random state and update the given {@code joints} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param min the minimum value for the generated random values. * @param max the maximum value for the generated random values. * @param joints the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, double min, double max, Iterable<? extends OneDoFJointBasics> joints) { joints.forEach(joint -> nextState(random, stateToRandomize, min, max, joint)); }
protected void setUpRandomChainRobot() { Random random = new Random(); joints = new ArrayList<RevoluteJoint>(); Vector3D[] jointAxes = {X, Y, Z, X, Z, Z, X, Y, Z, X}; joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "", elevator, jointAxes)); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); elevator.updateFramesRecursively(); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); }
public void compare() { for (MassMatrixCalculator massMatrixCalculator : massMatrixCalculators) { long startTime = System.nanoTime(); int nIterations = 10000; for (int i = 0; i < nIterations; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); elevator.updateFramesRecursively(); massMatrixCalculator.compute(); } long endTime = System.nanoTime(); double timeTaken = (endTime - startTime) / (1e9); double timeTakenPerIteration = timeTaken / nIterations; System.out.println("Time taken per iteration: " + timeTakenPerIteration + " s"); } }
public void altCompare() { double diffIdTimeTaken = 0.0; double compositeTimeTaken = 0.0; int nIterations = 10000; for (int i = 0; i < nIterations; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); elevator.updateFramesRecursively(); long startTime = System.nanoTime(); diffIdMassMatricCalculator.compute(); long endTime = System.nanoTime(); diffIdTimeTaken += (endTime - startTime) / (1e9); startTime = System.nanoTime(); compositeMassMatricCalculator.reset(); endTime = System.nanoTime(); compositeTimeTaken += (endTime - startTime) / (1e9); } double diffIdTimeTakenPerIteration = diffIdTimeTaken / nIterations; double compositeTimeTakenPerIteration = compositeTimeTaken / nIterations; System.out.println("Diff ID time taken per iteration: " + diffIdTimeTakenPerIteration + " s"); System.out.println("Composite RBM time taken per iteration: " + compositeTimeTakenPerIteration + " s"); }
/** * Randomizes the state of this multi-body system and updates its reference frames. * * @param random the random generator to use. * @param stateSelections the states to randomize. */ public void nextState(Random random, JointStateType... stateSelections) { for (JointStateType selection : stateSelections) MultiBodySystemRandomTools.nextState(random, selection, getJoints()); getElevator().updateFramesRecursively(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testForceGravityAndCoriolisOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); solveAndCompare(qddotSolution, rhoSolution, false); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGravityAndCoriolisOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); DenseMatrix64F rhoSolution = new DenseMatrix64F(wrenchMatrixCalculator.getRhoSize(), 1); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); solveAndCompare(qddotSolution, rhoSolution, false); } }
MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI/2.0, Math.PI/2.0, joints);
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithPrismaticChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<PrismaticJoint> joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testMassMatrixGravityAndCoriolisOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); update(); dynamicsMatrixCalculator.computeRequiredRhoAndAchievableQddotGivenRho(dynamicsMatrixCalculator, qddotSolution, rhoSolution); solveAndCompare(qddotSolution, rhoSolution, true); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testEquivalence() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); update(); dynamicsMatrixCalculator.computeRequiredRhoAndAchievableQddotGivenRho(dynamicsMatrixCalculator, qddotSolution, rhoSolution); solveAndCompare(qddotSolution, rhoSolution, true); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testMassMatrixAndCoriolisOnly() throws Exception { setupTest(); gravityZ = 0.0; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); update(); dynamicsMatrixCalculator.computeRequiredRhoAndAchievableQddotGivenRho(dynamicsMatrixCalculator, qddotSolution, rhoSolution); solveAndCompare(qddotSolution, rhoSolution, true); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNoLoad() throws Exception { setupTest(); gravityZ = 0.0; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); DenseMatrix64F rhoSolution = new DenseMatrix64F(wrenchMatrixCalculator.getRhoSize(), 1); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); solveAndCompare(qddotSolution, rhoSolution, false); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGravityOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); DenseMatrix64F rhoSolution = new DenseMatrix64F(wrenchMatrixCalculator.getRhoSize(), 1); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); solveAndCompare(qddotSolution, rhoSolution, false); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testForceAndGravityOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); solveAndCompare(qddotSolution, rhoSolution, false); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testMassMatrixAndGravityOnly() throws Exception { setupTest(); gravityZ = 9.81; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, wrenchMatrixCalculator.getRhoSize(), 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); update(); dynamicsMatrixCalculator.computeRequiredRhoAndAchievableQddotGivenRho(dynamicsMatrixCalculator, qddotSolution, rhoSolution); solveAndCompare(qddotSolution, rhoSolution, true); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testMassMatrixOnly() throws Exception { setupTest(); gravityZ = 0.0; ArrayList<OneDoFJointBasics> joints = new ArrayList<>(); fullHumanoidRobotModel.getOneDoFJoints(joints); for (int i = 0; i < iters; i++) { inverseDynamicsCalculator.setExternalWrenchesToZero(); dynamicsMatrixCalculator.reset(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); int rhoSize = wrenchMatrixCalculator.getRhoSize(); DenseMatrix64F rhoSolution = RandomGeometry.nextDenseMatrix64F(random, rhoSize, 1, 0.0, maxRho); DenseMatrix64F qddotSolution = new DenseMatrix64F(degreesOfFreedom, 1); update(); dynamicsMatrixCalculator.computeQddotGivenRho(dynamicsMatrixCalculator, qddotSolution, rhoSolution); solveAndCompare(qddotSolution, rhoSolution, true); } }