diff --git a/engine/profiles/servo_motor/profile_servo_motor.c b/engine/profiles/servo_motor/profile_servo_motor.c
index 2857f3b3c..982b0732d 100644
--- a/engine/profiles/servo_motor/profile_servo_motor.c
+++ b/engine/profiles/servo_motor/profile_servo_motor.c
@@ -152,6 +152,14 @@ void ProfileServo_Handler(service_t *service, const msg_t *msg)
             }
         }
         break;
+        case TORQUE:
+        {
+            // set the motor target torque
+            if (servo_motor_profile->mode.current)
+            {
+                ForceOD_TorqueFromMsg((torque_t *)&servo_motor_profile->target_torque, msg);
+            }
+        }
         case LINEAR_POSITION:
         {
             // set the motor target linear position
diff --git a/engine/profiles/servo_motor/profile_servo_motor.h b/engine/profiles/servo_motor/profile_servo_motor.h
index 0f69cf3e7..22afc6e84 100644
--- a/engine/profiles/servo_motor/profile_servo_motor.h
+++ b/engine/profiles/servo_motor/profile_servo_motor.h
@@ -54,6 +54,7 @@ typedef struct
     servo_motor_mode_t mode;
     angular_position_t target_angular_position;
     angular_speed_t target_angular_speed;
+    torque_t target_torque;
 
     // limits
     angular_position_t limit_angular_position[2];