public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
/** * Sort an array in place without allocating any memory. */ public static <T> void sort(T[] ts, Comparator<T> comparator) { boolean ordered = false; while (!ordered) { ordered = true; for (int i = 0; i < ts.length - 1; i++) { T a = ts[i]; T b = ts[i + 1]; if(comparator.compare(a, b) > 0) { ordered = false; swap(ts, i, i + 1); } } } }
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(); }
@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(); 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); } }
public V second(int i) { return get(i).getRight(); } }
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; }
public boolean isEmpty() { PairList<L, R> readCopy = getCopyForReading(); if (readCopy == null) return true; else return readCopy.isEmpty(); }
/** * Sort an array in place without allocating any memory. */ public static <T> void sort(List<T> ts, Comparator<T> comparator) { boolean ordered = false; while (!ordered) { ordered = true; for (int i = 0; i < ts.size() - 1; i++) { T a = ts.get(i); T b = ts.get(i + 1); if(comparator.compare(a, b) > 0) { ordered = false; swap(ts, i, i + 1); } } } }
public void clear() { PairList<L, R> updatedList = getCopyForWriting(); updatedList.clear(); commit(); }
@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); } }
public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
public V second(int i) { return get(i).getRight(); } }
protected void write() { for (int i = 0; i < revoluteJoints.size(); i++) { OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i); JointDesiredOutputReadOnly data = revoluteJoints.second(i); if(data.hasDesiredTorque()) { pinJoint.setTau(data.getDesiredTorque()); } if (data.hasStiffness()) { pinJoint.setKp(data.getStiffness()); } if (data.hasDamping()) { pinJoint.setKd(data.getDamping()); } if (data.hasDesiredPosition()) { pinJoint.setqDesired(data.getDesiredPosition()); } if (data.hasDesiredVelocity()) { pinJoint.setQdDesired(data.getDesiredVelocity()); } } }
@Override public void setLowLevelControllerCoreOutput(FullHumanoidRobotModel controllerRobotModel, JointDesiredOutputList lowLevelControllerCoreOutput, RawJointSensorDataHolderMap rawJointSensorDataHolderMap) { if(drcOutputProcessor != null) { drcOutputProcessor.setLowLevelControllerCoreOutput(controllerRobotModel, lowLevelControllerCoreOutput, rawJointSensorDataHolderMap); } for (int i = 0; i < lowLevelControllerCoreOutput.getNumberOfJointsWithDesiredOutput(); i++) { JointDesiredOutputBasics jointData = lowLevelControllerCoreOutput.getJointDesiredOutput(i); String jointName = lowLevelControllerCoreOutput.getOneDoFJoint(i).getName(); AlphaFilteredYoVariable jointTorqueSmoothedAtStateChange = new AlphaFilteredYoVariable("smoothed_tau_" + jointName, registry, alphaForJointTorqueForStateChanges); jointTorquesSmoothedAtStateChange.add(jointData, jointTorqueSmoothedAtStateChange); } }
public T first(int i) { return get(i).getLeft(); }
public T first(int i) { return get(i).getLeft(); }