@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(); }