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