@Override public JointDesiredOutputListReadOnly getOutputForLowLevelController() { return jointTorqueOffsetEstimatorController.getOutputForLowLevelController(); } }
@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()); }