private DenseMatrix64F getJointVelocityMatrix() { if (!isJointVelocityMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); isJointVelocityMatrixUpToDate = true; } return jointVelocityMatrix; }
private DenseMatrix64F getJointAccelerationMatrix() { if (!isJointAccelerationMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.ACCELERATION, jointAccelerationMatrix); isJointAccelerationMatrixUpToDate = true; } return jointAccelerationMatrix; }
private DenseMatrix64F getJointVelocityMatrix() { if (!isJointVelocityMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); isJointVelocityMatrixUpToDate = true; } return jointVelocityMatrix; }
private void updateCenterOfMassVelocity() { if (isCenterOfMassVelocityUpToDate) return; List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); CommonOps.mult(getJacobianMatrix(), jointVelocityMatrix, centerOfMassVelocityMatrix); centerOfMassVelocity.set(centerOfMassVelocityMatrix); isCenterOfMassVelocityUpToDate = true; }
private void storeJointState() { MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getJointTau(0, tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); DenseMatrix64F motionSubspace = new DenseMatrix64F(6, joint.getDegreesOfFreedom()); joint.getMotionSubspace(motionSubspace); CommonOps.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getFrameAfterJoint()); } }
public static Momentum computeCoMMomentum(RigidBodyBasics elevator, ReferenceFrame centerOfMassFrame, CentroidalMomentumCalculator centroidalMomentumMatrix) { DenseMatrix64F mat = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); JointBasics[] jointList = MultiBodySystemTools.collectSubtreeJoints(elevator); DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointList), 1); MultiBodySystemTools.extractJointsState(jointList, JointStateType.VELOCITY, jointVelocities); DenseMatrix64F comMomentumMatrix = MatrixTools.mult(mat, jointVelocities); Momentum comMomentum = new Momentum(centerOfMassFrame, comMomentumMatrix); return comMomentum; } }
/** * Computes the joint efforts needed to achieve the given joint accelerations. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointAccelerationMatrix the matrix containing the joint accelerations to use. Not * modified. */ public void compute(DenseMatrix64F jointAccelerationMatrix) { if (jointAccelerationMatrix != null) { this.jointAccelerationMatrix.set(jointAccelerationMatrix); } else { List<? extends JointReadOnly> indexedJointsInOrder = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(indexedJointsInOrder, JointStateType.ACCELERATION, this.jointAccelerationMatrix); } initialRecursionStep.passOne(); initialRecursionStep.passTwo(); }
/** * Computes the joint accelerations resulting from the given joint efforts. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointTauMatrix the matrix containing the joint efforts to use. Not modified. */ public void compute(DenseMatrix64F jointTauMatrix) { if (jointTauMatrix != null) { this.jointTauMatrix.set(jointTauMatrix); } else { List<? extends JointReadOnly> indexedJointsInOrder = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(indexedJointsInOrder, JointStateType.EFFORT, this.jointTauMatrix); } initialRecursionStep.passOne(); initialRecursionStep.passTwo(); initialRecursionStep.passThree(); }
@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); } }
DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); DenseMatrix64F jointVelocitiesMatrix = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, jointVelocitiesMatrix); DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1); CommonOps.mult(centroidalMomentumMatrixMatrix, jointVelocitiesMatrix, momentumFromCentroidalMomentumMatrix);
MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix);
MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix);
@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); } }
this.constrainedComJacobianSigmaMin.set(computeSmallestSingularValue(constrainedCenterOfMassJacobian)); MultiBodySystemTools.extractJointsState(allJoints, JointStateType.VELOCITY, v); CommonOps.mult(centerOfMassJacobian, v, tempCoMVelocityMatrix); tempCoMVelocity.set(tempCoMVelocityMatrix); comVelocity.set(tempCoMVelocity); MultiBodySystemTools.extractJointsState(actuatedJoints, JointStateType.VELOCITY, vActuated);
MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocities);
MultiBodySystemTools.extractJointsState(idJoints, JointStateType.VELOCITY, v);