private void restoreJointState() { MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { joint.setJointWrench(storedJointWrenches.get(joint)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackJointVelocitiesMatrix_Iterable() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>(); for(int i = 0; i < jointsArray.length; i++) { jointsList.add(jointsArray[i]); } DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++) { //create original matrix originalVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsList, JointStateType.VELOCITY, newVelocities);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon); } }
@Override public void compute() { storeJointState(); setDesiredAccelerationsToZero(); int column = 0; for (int i = 0 ; i < totalNumberOfDoFs; i++) { tmpDesiredJointAccelerationsMatrix.set(i, 0, 1.0); MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.ACCELERATION, tmpDesiredJointAccelerationsMatrix); idCalculator.compute(); tmpTauMatrix.set(idCalculator.getJointTauMatrix()); MatrixTools.setMatrixBlock(massMatrix, 0, column, tmpTauMatrix, 0, 0, totalNumberOfDoFs, 1, 1.0); column++; tmpDesiredJointAccelerationsMatrix.set(i, 0, 0.0); } restoreJointState(); }
MultiBodySystemTools.insertJointsState(jointIndexHandler.getIndexedJoints(), JointStateType.ACCELERATION, qddotSolution); inverseDynamicsCalculator.compute(); inverseDynamicsCalculator.writeComputedJointWrenches(SubtreeStreams.fromChildren(toolbox.getRootBody()).toArray(JointBasics[]::new));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackJointVelocitiesMatrix_Array() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++) { //create original matrix originalVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, newVelocities);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackDesiredJointAccelerationsMatrix() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F originalAccel = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalAccel.getNumRows() * originalAccel.getNumCols(); i++) { //create original matrix originalAccel.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, originalAccel); //set velocities from matrix DenseMatrix64F newAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.ACCELERATION, newAccelerations);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalAccel.get(i), newAccelerations.get(i), epsilon); } }
for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++) jointPositions.set(jointIdx, jointDesiredsMap.get(spineJoints[jointIdx]).getDoubleValue()); MultiBodySystemTools.insertJointsState(spineJointClones, JointStateType.CONFIGURATION, jointPositions); FrameQuaternion chestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); chestOrientation.changeFrame(ReferenceFrame.getWorldFrame());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetDesiredAccelerations() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F jointAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++) { jointAccelerations.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, jointAccelerations); DenseMatrix64F sixDoFAccel = new DenseMatrix64F(6, 1); jointsArray[0].getJointAcceleration(0, sixDoFAccel); for(int i = 0; i < 6; i++) { assertEquals("Should be equal accelerations", jointAccelerations.get(i), sixDoFAccel.get(i), epsilon); } OneDoFJointBasics joint; for(int i = 6; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++) { joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6 assertEquals("Should be equal accelerations", jointAccelerations.get(i), joint.getQdd(), epsilon); } }
@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); } }
pseudoInverseSolver.solve(motionQPInput.taskObjective, desiredJointAccelerations); MultiBodySystemTools.insertJointsState(joints, JointStateType.ACCELERATION, desiredJointAccelerations); pseudoInverseSolver.solve(motionQPInput.taskObjective, desiredJointAccelerations); MultiBodySystemTools.insertJointsState(joints, JointStateType.ACCELERATION, desiredJointAccelerations);
MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); elevator.updateFramesRecursively();
MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); elevator.updateFramesRecursively();
MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); elevator.updateFramesRecursively();
MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); elevator.updateFramesRecursively();