public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
public V second(int i) { return get(i).getRight(); } }
public void add(L leftObjectToAdd, R rightObjectToAdd) { PairList<L, R> existingList = getCopyForReading(); PairList<L, R> updatedList = getCopyForWriting(); updatedList.clear(); if (existingList != null) updatedList.addAll(existingList); updatedList.add(new ImmutablePair<>(leftObjectToAdd, rightObjectToAdd)); this.commit(); }
public ImmutablePair<L, R> remove(int indexToRemove) { PairList<L, R> existingList = getCopyForReading(); PairList<L, R> updatedList = getCopyForWriting(); updatedList.clear(); if (existingList != null) updatedList.addAll(existingList); ImmutablePair<L, R> objectToReturn = updatedList.remove(indexToRemove); this.commit(); return objectToReturn; }
jointStateAndData = new PairList<>(); jointStateAndData.add(controllerRobotModel.getLegJoint(robotSide, jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getLegJoint(robotSide, jointName))); for (ArmJointName jointName : armJointsForIntegratingAcceleration) jointStateAndData.add(controllerRobotModel.getArmJoint(robotSide, jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getArmJoint(robotSide, jointName))); jointStateAndData.add(controllerRobotModel.getSpineJoint(jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getSpineJoint(jointName))); jointStateAndData.add(lowLevelControllerCoreOutput.getOneDoFJoint(i), lowLevelControllerCoreOutput.getJointDesiredOutput(lowLevelControllerCoreOutput.getOneDoFJoint(i))); positionTorqueMap = new LinkedHashMap<>(); for (int i = 0; i < jointStateAndData.size(); i++) final OneDoFJointBasics jointState = jointStateAndData.first(i); final JointDesiredOutputBasics jointData = jointStateAndData.second(i);
controlledJoints.add(joint, lowLevelOutput.getJointDesiredOutput(joint)); for (int i = controlledJoints.size() - 1; i >= 0; i--) if (controlledJoints.first(i).getName().equals(jointToIgnore)) controlledJoints.remove(i); submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue())); for (int i = 0; i < controlledJoints.size(); i++) OneDoFJointBasics joint = controlledJoints.first(i); String name = joint.getName(); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = new OneDoFJointQuinticTrajectoryGenerator(name, joint, trajectoryTimeProvider, registry);
@Override public void onEntry() { for (int i = 0; i < jointsData.size(); i++) { OneDoFJointBasics joint = jointsData.get(i).getLeft(); TrajectoryData trajectoryData = jointsData.get(i).getRight(); YoDouble initialPosition = trajectoryData.getInitialPosition(); YoPolynomial trajectory = trajectoryData.getTrajectory(); double startAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint); double startVelocity = 0.0; double finalAngle = initialPosition.getDoubleValue(); double finalVelocity = 0.0; trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity); } }
@Override public void setJointDesiredOutputList(JointDesiredOutputList lowLevelDataHolder) { revoluteJoints.clear(); for (int i = 0; i < lowLevelDataHolder.getNumberOfJointsWithDesiredOutput(); i++) { OneDoFJointBasics revoluteJoint = lowLevelDataHolder.getOneDoFJoint(i); JointDesiredOutputReadOnly data = lowLevelDataHolder.getJointDesiredOutput(i); String name = revoluteJoint.getName(); OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name); ImmutablePair<OneDegreeOfFreedomJoint, JointDesiredOutputReadOnly> jointPair = new ImmutablePair<OneDegreeOfFreedomJoint, JointDesiredOutputReadOnly>(oneDoFJoint, data); this.revoluteJoints.add(jointPair); } }
@Override public void setLowLevelControllerCoreOutput(FullHumanoidRobotModel controllerRobotModel, JointDesiredOutputList lowLevelControllerCoreOutput, RawJointSensorDataHolderMap rawJointSensorDataHolderMap) { if (drcOutputWriter != null) { drcOutputWriter.setLowLevelControllerCoreOutput(controllerRobotModel, lowLevelControllerCoreOutput, rawJointSensorDataHolderMap); } torqueOffsetList = new PairList<>(); torqueOffsetMap = new HashMap<>(); for (int i = 0; i < lowLevelControllerCoreOutput.getNumberOfJointsWithDesiredOutput(); i++) { JointDesiredOutputBasics jointData = lowLevelControllerCoreOutput.getJointDesiredOutput(i); final YoDouble torqueOffset = new YoDouble("tauOffset_" + lowLevelControllerCoreOutput.getJointName(i), registry); torqueOffsetList.add(jointData, torqueOffset); torqueOffsetMap.put(lowLevelControllerCoreOutput.getOneDoFJoint(i), torqueOffset); } }
@Override public void onEntry() { for (int i = 0; i < jointsData.size(); i++) { OneDoFJointBasics joint = jointsData.get(i).getLeft(); TrajectoryData trajectoryData = jointsData.get(i).getRight(); YoDouble initialPosition = trajectoryData.getInitialPosition(); YoPolynomial trajectory = trajectoryData.getTrajectory(); JointDesiredOutputReadOnly jointDesiredOutput = highLevelControlOutput.getJointDesiredOutput(joint); double startAngle = jointDesiredOutput != null && jointDesiredOutput.hasDesiredPosition() ? jointDesiredOutput.getDesiredPosition() : joint.getQ(); double startVelocity = 0.0; double finalAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint); double finalVelocity = 0.0; initialPosition.set(startAngle); trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity); } jointTorqueOffsetEstimatorController.initialize(); }
@Override public void processAfterController(long timestamp) { for (int i = 0; i < torqueOffsetList.size(); i++) { JointDesiredOutputBasics jointData = torqueOffsetList.first(i); YoDouble torqueOffsetVariable = torqueOffsetList.second(i); double desiredAcceleration = jointData.hasDesiredAcceleration() ? jointData.getDesiredAcceleration() : 0.0; if (resetTorqueOffsets.getBooleanValue()) torqueOffsetVariable.set(0.0); double offsetTorque = torqueOffsetVariable.getDoubleValue(); double ditherTorque = 0.0; double alpha = alphaTorqueOffset.getDoubleValue(); offsetTorque = alpha * (offsetTorque + desiredAcceleration * updateDT) + (1.0 - alpha) * offsetTorque; torqueOffsetVariable.set(offsetTorque); double desiredTorque = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0; jointData.setDesiredTorque(desiredTorque + offsetTorque + ditherTorque); } if (drcOutputWriter != null) { drcOutputWriter.processAfterController(timestamp); } }
@Override public void doAction(double timeInState) { double timeInTrajectory = MathTools.clamp(timeInState, 0.0, timeToMoveForCalibration.getDoubleValue()); for (int jointIndex = 0; jointIndex < jointsData.size(); jointIndex++) { OneDoFJointBasics joint = jointsData.get(jointIndex).getLeft(); TrajectoryData trajectoryData = jointsData.get(jointIndex).getRight(); YoPolynomial trajectory = trajectoryData.getTrajectory(); trajectory.compute(timeInTrajectory); double desiredPosition = trajectory.getPosition(); JointDesiredOutputBasics lowLevelJointData = lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(joint); lowLevelJointData.clear(); lowLevelJointData.setDesiredPosition(desiredPosition); lowLevelJointData.setDesiredVelocity(trajectory.getVelocity()); lowLevelJointData.setDesiredAcceleration(trajectory.getAcceleration()); JointDesiredOutputReadOnly estimatorOutput = jointTorqueOffsetEstimatorController.getOutputForLowLevelController().getJointDesiredOutput(joint); if (estimatorOutput != null && estimatorOutput.hasDesiredTorque()) { double desiredTorque = estimatorOutput.getDesiredTorque(); desiredTorque *= 1.0 - timeInTrajectory / timeToMoveForCalibration.getDoubleValue(); lowLevelJointData.setDesiredTorque(desiredTorque); } } lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings()); }
public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
public V second(int i) { return get(i).getRight(); } }
for (int i = 0; i < jointTorquesSmoothedAtStateChange.size(); i++) JointDesiredOutputBasics jointData = jointTorquesSmoothedAtStateChange.first(i); double tau = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0; AlphaFilteredYoVariable smoothedJointTorque = jointTorquesSmoothedAtStateChange.second(i); smoothedJointTorque.update(tau); jointData.setDesiredTorque(smoothedJointTorque.getDoubleValue());
@Override public void doAction(double timeInState) { double timeInTrajectory = MathTools.clamp(timeInState, 0.0, timeToMoveForCalibration.getDoubleValue()); jointTorqueOffsetEstimatorController.doControl(); for (int jointIndex = 0; jointIndex < jointsData.size(); jointIndex++) { OneDoFJointBasics joint = jointsData.get(jointIndex).getLeft(); TrajectoryData trajectoryData = jointsData.get(jointIndex).getRight(); YoPolynomial trajectory = trajectoryData.getTrajectory(); trajectory.compute(timeInTrajectory); double desiredPosition = trajectory.getPosition(); JointDesiredOutputBasics lowLevelJointData = lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(joint); lowLevelJointData.clear(); lowLevelJointData.setDesiredPosition(desiredPosition); lowLevelJointData.setDesiredVelocity(trajectory.getVelocity()); lowLevelJointData.setDesiredAcceleration(trajectory.getAcceleration()); JointDesiredOutputReadOnly estimatorOutput = jointTorqueOffsetEstimatorController.getOutputForLowLevelController().getJointDesiredOutput(joint); if (estimatorOutput != null && estimatorOutput.hasDesiredTorque()) { double desiredTorque = estimatorOutput.getDesiredTorque(); desiredTorque *= timeInTrajectory / timeToMoveForCalibration.getDoubleValue(); lowLevelJointData.setDesiredTorque(desiredTorque); } } lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings()); }
public RawJointSensorDataHolderMapCopier(RawJointSensorDataHolderMap originalMap, RawJointSensorDataHolderMap targetMap) { List<RawJointSensorDataHolder> originals = new ArrayList<RawJointSensorDataHolder>(originalMap.values()); List<RawJointSensorDataHolder> targets = new ArrayList<RawJointSensorDataHolder>(targetMap.values()); if(originals.size() != targets.size()) { throw new RuntimeException("Original and Target are not of equal length"); } for(int i = 0; i < originals.size(); i++) { RawJointSensorDataHolder original = originals.get(i); RawJointSensorDataHolder target = targets.get(i); if(!original.getName().equals(target.getName())) { throw new RuntimeException("Original and Target don't match. Got: " + target.getName() + ", expected: " + original.getName()); } originalAndTarget.add(original, target); } }