Separated estimateJointTemperature process to member function
Added error2torque conversion to ThermoEstimator and make servoState pass when calculated temperature is invalid.
Debugged read port miss in inports and modified debug message position