public static int computeDegreesOfFreedom(Iterable<? extends InverseDynamicsJoint> jointList) { int ret = 0; for (InverseDynamicsJoint joint : jointList) { ret += joint.getDegreesOfFreedom(); } return ret; }
public static int computeDegreesOfFreedom(InverseDynamicsJoint[] jointList) { int ret = 0; for (InverseDynamicsJoint joint : jointList) { ret += joint.getDegreesOfFreedom(); } return ret; }
public static int computeDegreesOfFreedom(List<? extends InverseDynamicsJoint> jointList) { int ret = 0; for (int i = 0; i < jointList.size(); i++) { ret += jointList.get(i).getDegreesOfFreedom(); } return ret; }
public static void getJointVelocitiesMatrix(Iterable<? extends InverseDynamicsJoint> joints, DenseMatrix64F jointVelocitiesMatrixToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { int dof = joint.getDegreesOfFreedom(); joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart); rowStart += dof; } }
private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder) { int[] ret = new int[rigidBodiesInOrder.length]; int currentIndex = 0; for (int i = 0; i < rigidBodiesInOrder.length; i++) { RigidBody rigidBody = rigidBodiesInOrder[i]; ret[i] = currentIndex; currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom(); } return ret; }
public static void getJointVelocitiesMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F jointVelocitiesMatrixToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { int dof = joint.getDegreesOfFreedom(); joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart); rowStart += dof; } }
public static void getJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositionsToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { joint.getConfigurationMatrix(jointPositionsToPack, rowStart); rowStart += joint.getDegreesOfFreedom(); if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint) rowStart++; // Because of stupid quaternions } }
public static void setJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositions) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { joint.setConfiguration(jointPositions, rowStart); rowStart += joint.getDegreesOfFreedom(); if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint) rowStart++; // Because of stupid quaternions } }
public static void getDesiredJointAccelerationsMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F desiredJointAccelerationsMatrixToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { int dof = joint.getDegreesOfFreedom(); joint.getDesiredAccelerationMatrix(desiredJointAccelerationsMatrixToPack, rowStart); rowStart += dof; } }
public static void setVelocities(InverseDynamicsJoint[] jointList, DenseMatrix64F jointVelocities) { int rowStart = 0; for (InverseDynamicsJoint joint : jointList) { joint.setVelocity(jointVelocities, rowStart); rowStart += joint.getDegreesOfFreedom(); } }
public static void setDesiredAccelerations(InverseDynamicsJoint[] jointList, DenseMatrix64F jointAccelerations) { int rowStart = 0; for (InverseDynamicsJoint joint : jointList) { joint.setDesiredAcceleration(jointAccelerations, rowStart); rowStart += joint.getDegreesOfFreedom(); } }
private static int determineSize(RigidBody[] rigidBodiesInOrder) { int ret = 0; for (RigidBody rigidBody : rigidBodiesInOrder) { ret += rigidBody.getParentJoint().getDegreesOfFreedom(); } return ret; }
public static void computeIndexForJoint(InverseDynamicsJoint[] jointsInOrder, TIntArrayList listToPackIndices, InverseDynamicsJoint jointToComputeIndicesFor) { int startIndex = 0; for (int i = 0; i < jointsInOrder.length; i++) { int nDegreesOfFreedom = jointsInOrder[i].getDegreesOfFreedom(); if (jointsInOrder[i] == jointToComputeIndicesFor) { for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++) { listToPackIndices.add(k); } } startIndex += nDegreesOfFreedom; } }
private void checkConsistency(InverseDynamicsJoint joint, DenseMatrix64F desiredVelocity) { MathTools.checkIfEqual(joint.getDegreesOfFreedom(), desiredVelocity.getNumRows()); }
private void checkConsistency(InverseDynamicsJoint joint, DenseMatrix64F desiredAcceleration) { MathTools.checkIfEqual(joint.getDegreesOfFreedom(), desiredAcceleration.getNumRows()); }
public static void setRandomVelocities(InverseDynamicsJoint[] joints, Random random) { for (InverseDynamicsJoint joint : joints) { DenseMatrix64F jointVelocity = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); RandomMatrices.setRandom(jointVelocity, random); joint.setVelocity(jointVelocity, 0); } }
private void setDesiredAccelerationsToZero() { for (InverseDynamicsJoint joint : jointsInOrder) { joint.setDesiredAccelerationToZero(); joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0); } }
public void setOneDoFJointDesiredAcceleration(int jointIndex, double desiredVelocity) { MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1); desiredVelocities.get(jointIndex).reshape(1, 1); desiredVelocities.get(jointIndex).set(0, 0, desiredVelocity); }
public void setOneDoFJointDesiredAcceleration(int jointIndex, double desiredAcceleration) { MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1); desiredAccelerations.get(jointIndex).reshape(1, 1); desiredAccelerations.get(jointIndex).set(0, 0, desiredAcceleration); }
private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (InverseDynamicsJoint joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.set(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } }