public T first(int i) { return get(i).getLeft(); }
public V second(int i) { return get(i).getRight(); } }
public V second(int i) { return get(i).getRight(); } }
public T first(int i) { return get(i).getLeft(); }
@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 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 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()); }