public void setWrench(WrenchReadOnly newWrench) { measurementFrame.checkReferenceFrameMatch(newWrench.getReferenceFrame()); measurementFrame.checkReferenceFrameMatch(newWrench.getBodyFrame()); newWrench.get(wrench); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { getJointWrench().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** * Computes and packs the joint torque vector that corresponds to the given wrench. * * @param wrench the resulting wrench at the end effector. The wrench should be expressed in * {@code jacobianFrame} and the wrench's {@code bodyFrame} should be the body fixed * frame of the end-effector. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. */ public void computeJointTorques(WrenchReadOnly wrench, DenseMatrix64F jointTorquesToPack) { // reference frame check wrench.getReferenceFrame().checkReferenceFrameMatch(this.jacobianFrame); // FIXME add the following reference frame check // wrench.getBodyFrame().checkReferenceFrameMatch(getEndEffectorFrame()); wrench.get(tempMatrix); jointTorquesToPack.reshape(1, jacobian.getNumCols()); CommonOps.multTransA(tempMatrix, jacobian, jointTorquesToPack); CommonOps.transpose(jointTorquesToPack); }
static void compareWrenches(WrenchReadOnly inputWrench, Wrench outputWrench, DenseMatrix64F selectionMatrix) { inputWrench.getBodyFrame().checkReferenceFrameMatch(outputWrench.getBodyFrame()); outputWrench.changeFrame(inputWrench.getReferenceFrame()); inputWrench.getReferenceFrame().checkReferenceFrameMatch(outputWrench.getReferenceFrame()); DenseMatrix64F inputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F outputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F selectedValues = new DenseMatrix64F(Wrench.SIZE, 1); inputWrench.get(inputWrenchMatrix); outputWrench.get(outputWrenchMatrix); double epsilon = 1e-4; int taskSize = selectionMatrix.getNumRows(); int colIndex = 0; for (int i = 0; i < taskSize; i++) for (int j = colIndex; j < Wrench.SIZE; j++) if (selectionMatrix.get(i, j) == 1) selectedValues.set(j, 0, 1); // only compare the selected values for (int i = 0; i < Wrench.SIZE; i++) { if (selectedValues.get(i, 0) == 1) Assert.assertEquals(inputWrenchMatrix.get(i, 0), outputWrenchMatrix.get(i, 0), epsilon); } }
/** * Computes and packs the joint torque vector that corresponds to the given wrench. * * @param endEffectorWrench the resulting wrench at the end effector. The wrench should be expressed * in {@code jacobianFrame} and the wrench's {@code bodyFrame} should be the body fixed * frame of the end-effector. Not modified. * @param jointTorquesToPack the dense matrix used to store the computed joint torques. Modified. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. * @throws RuntimeException if either the base or the end-effector has not been provided beforehand. */ public void getJointTorques(WrenchReadOnly endEffectorWrench, DenseMatrix64F jointTorquesToPack) { endEffectorWrench.checkReferenceFrameMatch(getEndEffectorFrame(), jacobianFrame); endEffectorWrench.get(spatialVector); jointTorquesToPack.reshape(1, numberOfDegreesOfFreedom); CommonOps.multTransA(spatialVector, getJacobianMatrix(), jointTorquesToPack); CommonOps.transpose(jointTorquesToPack); }
public static void compareWrenches(WrenchReadOnly inputWrench, Wrench outputWrench, SelectionMatrix6D selectionMatrix) { inputWrench.getBodyFrame().checkReferenceFrameMatch(outputWrench.getBodyFrame()); outputWrench.changeFrame(inputWrench.getReferenceFrame()); inputWrench.getReferenceFrame().checkReferenceFrameMatch(outputWrench.getReferenceFrame()); DenseMatrix64F inputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F outputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); inputWrench.get(inputWrenchMatrix); outputWrench.get(outputWrenchMatrix); double epsilon = 1e-4; if (selectionMatrix.isAngularXSelected()) Assert.assertEquals(inputWrenchMatrix.get(0, 0), outputWrenchMatrix.get(0, 0), epsilon); if (selectionMatrix.isAngularYSelected()) Assert.assertEquals(inputWrenchMatrix.get(1, 0), outputWrenchMatrix.get(1, 0), epsilon); if (selectionMatrix.isAngularZSelected()) Assert.assertEquals(inputWrenchMatrix.get(2, 0), outputWrenchMatrix.get(2, 0), epsilon); if (selectionMatrix.isLinearXSelected()) Assert.assertEquals(inputWrenchMatrix.get(3, 0), outputWrenchMatrix.get(3, 0), epsilon); if (selectionMatrix.isLinearYSelected()) Assert.assertEquals(inputWrenchMatrix.get(4, 0), outputWrenchMatrix.get(4, 0), epsilon); if (selectionMatrix.isLinearZSelected()) Assert.assertEquals(inputWrenchMatrix.get(5, 0), outputWrenchMatrix.get(5, 0), epsilon); }