diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index f36d867556784de6989695c320eaf7372a1782dc..40b679ee8894ece71d5c431ef24e5e4f40ce1b3d 100644 --- a/src/dynamixel_motor.cpp +++ b/src/dynamixel_motor.cpp @@ -1364,7 +1364,23 @@ void CDynamixelMotor::set_torque(float torque_ratio) float CDynamixelMotor::get_torque(void) { - return 0.0; + unsigned short int load; + float torque; + + if(this->dynamixel_dev==NULL) + { + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured."); + } + else + { + this->dynamixel_dev->read_word_register(current_load,&load); + torque=(load&0x3FF)*100.0/1023; + if(load>0x3FF) + torque=-1*torque; + } + + return torque; } CDynamixelMotor::~CDynamixelMotor()