RandomFloatingRevoluteJointChain floatingChain = new RandomFloatingRevoluteJointChain(random, numberOfJoints); SixDoFJoint floatingJoint = floatingChain.getRootJoint(); List<RevoluteJoint> revoluteJoints = floatingChain.getRevoluteJoints(); List<Joint> joints = floatingChain.getJoints(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, revoluteJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, revoluteJoints); floatingChain.getElevator().updateFramesRecursively();
@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); GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(), randomFloatingChain.getLeafBody().getBodyFixedFrame()); List<RevoluteJoint> revoluteJoints = randomFloatingChain.getRevoluteJoints();
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0);
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0);
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0);
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor();
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0);
RandomFloatingRevoluteJointChain floatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); SixDoFJoint floatingJoint = floatingChain.getRootJoint(); List<RevoluteJoint> revoluteJoints = floatingChain.getRevoluteJoints(); List<Joint> joints = floatingChain.getJoints(); List<JointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneKinematicChain(joints.toArray(new JointBasics[numberOfRevoluteJoints + 1])));
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); List<RevoluteJoint> joints = randomFloatingChain.getRevoluteJoints(); RigidBodyBasics elevator = randomFloatingChain.getElevator(); RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor();
RandomFloatingRevoluteJointChain floatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); SixDoFJoint floatingJoint = floatingChain.getRootJoint(); List<RevoluteJoint> revoluteJoints = floatingChain.getRevoluteJoints(); List<Joint> joints = floatingChain.getJoints(); List<JointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneKinematicChain(joints.toArray(new JointBasics[numberOfRevoluteJoints + 1])));
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(), randomFloatingChain.getLeafBody().getBodyFixedFrame()); List<RevoluteJoint> revoluteJoints = randomFloatingChain.getRevoluteJoints();
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics base = randomFloatingChain.getRootJoint().getSuccessor(); RigidBodyBasics endEffector = randomFloatingChain.getLeafBody(); GeometricJacobian geometricJacobian = new GeometricJacobian(base, endEffector, base.getBodyFixedFrame()); geometricJacobian.compute();
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(), randomFloatingChain.getLeafBody().getBodyFixedFrame()); List<RevoluteJoint> revoluteJoints = randomFloatingChain.getRevoluteJoints();
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 11.3) @Test(timeout=3000000) public void testLinearAndAngularMomentumAreConverved() { int numberOfRobotsToTest = 5; int minNumberOfAxes = 2; int maxNumberOfAxes = 10; Robot[] robots = new Robot[numberOfRobotsToTest]; for (int i = 0; i < numberOfRobotsToTest; i++) { Robot robot = new RobotTools.SCSRobotFromInverseDynamicsRobotModel("robot" + i, getRandomFloatingChain(minNumberOfAxes, maxNumberOfAxes).getRootJoint()); robot.setGravity(0.0, 0.0, 0.0); robot.setController(new ConservedQuantitiesChecker(robot)); robot.setController(new SinusoidalTorqueController(robot)); robots[i] = robot; } SimulationConstructionSet scs = new SimulationConstructionSet(robots, simulationTestingParameters); scs.startOnAThread(); BlockingSimulationRunner blockingSimulationRunner = new BlockingSimulationRunner(scs, 30.0); try { blockingSimulationRunner.simulateAndBlock(8.0); } catch(BlockingSimulationRunner.SimulationExceededMaximumTimeException | ControllerFailureException e) { e.printStackTrace(); fail(); } }
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics base = randomFloatingChain.getRootJoint().getSuccessor(); RigidBodyBasics endEffector = randomFloatingChain.getLeafBody(); GeometricJacobian geometricJacobian = new GeometricJacobian(base, endEffector, base.getBodyFixedFrame()); geometricJacobian.compute(); double dt = 1e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(dt); integrator.integrateFromVelocity(randomFloatingChain.getRevoluteJoints()); randomFloatingChain.getElevator().updateFramesRecursively(); point2.changeFrame(base.getBodyFixedFrame());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testFilterJoints_dest() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); RevoluteJoint[] justRevolutes = new RevoluteJoint[jointsArr.length - 1]; MultiBodySystemTools.filterJoints(jointsArr, justRevolutes, RevoluteJoint.class); assertEquals(jointsArr.length - 1, justRevolutes.length); SixDoFJoint[] justSix = new SixDoFJoint[1]; MultiBodySystemTools.filterJoints(jointsArr, justSix, SixDoFJoint.class); assertEquals(1, justSix.length); assertTrue(justSix[0] instanceof SixDoFJoint); Boolean clean = false; for(JointBasics joint: justRevolutes) { if(joint instanceof RevoluteJoint) { clean = true; } else { clean = false; } assertTrue(clean); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetVelocities() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++) { jointVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, jointVelocities); DenseMatrix64F sixDoFVeloc = new DenseMatrix64F(6, 1); jointsArray[0].getJointVelocity(0, sixDoFVeloc); for(int i = 0; i < 6; i++) { assertEquals("Should be equal velocitiess", jointVelocities.get(i), sixDoFVeloc.get(i), epsilon); } OneDoFJointBasics joint; for(int i = 6; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++) { joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6 assertEquals("Should be equal velocities", jointVelocities.get(i), joint.getQd(), epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testFilterJoints() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); RevoluteJoint[] justRevolutes = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class); assertEquals(jointsArr.length - 1, justRevolutes.length); SixDoFJoint[] justSix = MultiBodySystemTools.filterJoints(jointsArr, SixDoFJoint.class); assertEquals(1, justSix.length); assertTrue(justSix[0] instanceof SixDoFJoint); Boolean clean = false; for(JointBasics joint: justRevolutes) { if(joint instanceof RevoluteJoint) { clean = true; } else { clean = false; } assertTrue(clean); } }
RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(), randomFloatingChain.getLeafBody().getBodyFixedFrame()); List<RevoluteJoint> revoluteJoints = randomFloatingChain.getRevoluteJoints();