public MomentumCommand(MomentumCommand momentumCommand) { this(); set(momentumCommand); }
public void setWeight(double weight) { setWeights(weight, weight); }
MomentumCommand momentumCommand = new MomentumCommand(); momentumCommand.setLinearMomentumXY(desiredMomentumXY); momentumCommand.setWeight(momentumWeight.getDoubleValue()); ret.addCommand(momentumCommand);
/** * Converts a {@link MomentumCommand} into a {@link MotionQPInput}. * @return true if the command was successfully converted. */ public boolean convertMomentumCommand(MomentumCommand commandToConvert, MotionQPInput motionQPInputToPack) { DenseMatrix64F selectionMatrix = commandToConvert.getSelectionMatrix(); int taskSize = selectionMatrix.getNumRows(); if (taskSize == 0) return false; motionQPInputToPack.reshape(taskSize); motionQPInputToPack.setUseWeightScalar(false); motionQPInputToPack.setIsMotionConstraint(false); // Compute the weight: W = S * W * S^T tempTaskWeight.reshape(SpatialAccelerationVector.SIZE, SpatialAccelerationVector.SIZE); commandToConvert.getWeightMatrix(tempTaskWeight); tempTaskWeightSubspace.reshape(taskSize, SpatialAccelerationVector.SIZE); CommonOps.mult(selectionMatrix, tempTaskWeight, tempTaskWeightSubspace); CommonOps.multTransB(tempTaskWeightSubspace, selectionMatrix, motionQPInputToPack.taskWeightMatrix); // Compute the task Jacobian: J = S * A DenseMatrix64F centroidalMomentumMatrix = getCentroidalMomentumMatrix(); CommonOps.mult(selectionMatrix, centroidalMomentumMatrix, motionQPInputToPack.taskJacobian); DenseMatrix64F momemtum = commandToConvert.getMomentum(); // Compute the task objective: p = S * h CommonOps.mult(selectionMatrix, momemtum, motionQPInputToPack.taskObjective); recordTaskJacobian(motionQPInputToPack.taskJacobian); return true; }
MomentumCommand momentumCommand = new MomentumCommand(); momentumCommand.setLinearMomentumXY(desiredMomentumXY); momentumCommand.setWeight(momentumWeight.getDoubleValue()); ret.addCommand(momentumCommand);
private void copyMomentumCommand(MomentumCommand commandToCopy) { MomentumCommand localCommand = momentumCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
private void copyMomentumCommand(MomentumCommand commandToCopy) { MomentumCommand localCommand = momentumCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }