public void copy() { for(int i = 0; i < originalAndTarget.size(); i++) { RawJointSensorDataHolder original = originalAndTarget.first(i); RawJointSensorDataHolder target = originalAndTarget.second(i); target.set(original); } } }
@Override public void initialize() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJointBasics state = controlledJoints.first(i); jointTrajectories.get(state).initialize(state.getQ(), 0.0); } startTime.set(yoTime.getDoubleValue()); hasBeenInitialized = true; }
for (int i = 0; i < controlledJoints.size(); i++)
positionTorqueMap = new LinkedHashMap<>(); for (int i = 0; i < jointStateAndData.size(); i++)
private void doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJointBasics state = controlledJoints.first(i); JointDesiredOutputBasics output = controlledJoints.second(i); PDController jointPDController = jointPDControllerMap.get(state); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(state); jointTrajectory.compute(trajectoryTimeProvider.getValue()); YoDouble jointDesiredPosition = jointDesiredPositionMap.get(state); YoDouble jointDesiredVelocity = jointDesiredVelocityMap.get(state); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = state.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = state.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired); double qdMax = qdMaxIdle.getDoubleValue(); double qddMax = qddMaxIdle.getDoubleValue(); double tauMax = tauMaxIdle.getDoubleValue(); qDesired = q + MathTools.clamp(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clamp(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clamp(tauDesired, tauMax); output.setDesiredTorque(tauDesired); output.setDesiredPosition(qDesired); output.setDesiredVelocity(qdDesired); jointDesiredTauMap.get(state).set(tauDesired); } }
@Override public void processAfterController(long timestamp) for (int i = 0; i < jointStateAndData.size(); i++)
for (int i = 0; i < jointTorquesSmoothedAtStateChange.size(); i++)
@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); } }
DiagnosticTask currentTask = diagnosticTaskExecutor.getCurrentTask(); for (int i = 0; i < controlledJoints.size(); i++)
@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 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(); }
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 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()); }
@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()); }