public void removeWeight() { setWeight(HARD_CONSTRAINT); }
spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.set(elevator, chest); spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity); spatialVelocityCommand.setWeight(chestWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.set(elevator, pelvis); spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity); spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand);
spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity); spatialVelocityCommand.set(desiredChestTwist, chestSelectionMatrix); spatialVelocityCommand.setWeight(chestWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity); spatialVelocityCommand.set(desiredPelvisTwist, pelvisSelectionMatrix); spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand);