Upgrade driver to support controller mode other than position control

configs/wx250s_motor_config.yaml

@@ -0,0 +1 @@
+../wx_armor/configs/wx250s_motor_config.yaml

wx_armor/configs/wx250s_motor_config.yaml

@@ -5,41 +5,48 @@
 # SETUP OF REGISTERS FOR THE INTERBOTIX PLATFORMS
 #
 # All registers were left to their default values except for the ones listed here:
-#   1) ID.........................The ID of the Dynamixel servo. Valids IDs go from 1 - 251.
-#   2) Baud_Rate..................The speed at which serial communication occurs. All motors have this set to 1M
-#                                 baud (corresponds to a register value of '3')
-#   3) Return_Delay_Time..........The amount of time the servo delays in sending a reply packet after receiving a
-#                                 command packet. A value of '0' tells the servo to send a reply without any delay.
-#   4) Drive_Mode:................Used to define what direction is positive rotation. A value of '0' means CCW is
-#                                 positive while a value of '1' means CW is positive. It also defines if profiles are
-#                                 built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time'
-#                                 is a value of '4'. Adding the 'direction' value to the 'profile' value gives the
-#                                 appropiate register value
-#   5) Velocity_Limit.............Defines the max speed of the motor. A value of '131' correpsonds to a max speed
-#                                 of PI rad/s
-#   6) Min_Position_Limit.........Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048
-#                                 being equivalent to '0' rad and 0 being '-PI' rad.
-#   7) Max_Position_Limit.........Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048
-#                                 being equivalent to '0' rad and 4095 being 'PI' rad.
-#   8) Secondary_ID...............If a joint is controlled by two motors (usually by the shoulder or elbow), one motor
-#                                 can be set to follow the commands of another motor by setting this register to the ID
-#                                 of the master. A value of '255' disables this.
+#   1) ID............................The ID of the Dynamixel servo. Valid IDs range from 1 to 251.
+#   2) Baud_Rate.....................The speed at which serial communication occurs. All motors have this set to 1M
+#                                    baud (corresponds to a register value of '3')
+#   3) Return_Delay_Time.............The amount of time the servo delays in sending a reply packet after receiving a
+#                                    command packet. A value of '0' tells the servo to send a reply without any delay.
+#   4) Drive_Mode:...................Used to define what direction is positive rotation. A value of '0' means CCW is
+#                                    positive while a value of '1' means CW is positive. It also defines if profiles are
+#                                    built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time'
+#                                    is a value of '4'. Adding the 'direction' value to the 'profile' value gives the
+#                                    appropriate register value
+#   5) Velocity_Limit................Defines the max speed of the motor. A value of '131' corresponds to a max speed
+#                                    of PI rad/s. NOTE: this is only used in velocity control mode. I.e., has no effect
+#                                    in the default position control setting.
+#   6) Min_Position_Limit............Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048
+#                                    being equivalent to '0' rad and 0 being '-PI' rad.
+#   7) Max_Position_Limit............Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048
+#                                    being equivalent to '0' rad and 4095 being 'PI' rad.
+#   8) Secondary_ID..................If a joint is controlled by two motors (usually by the shoulder or elbow), one motor
+#                                    can be set to follow the commands of another motor by setting this register to the ID
+#                                    of the master. A value of '255' disables this.
+#   9) Safety_Velocity_Limit.........Defines the max safety velocity limit [deg/s]. If the joint velocity ever violates
+#                                    this limit, we override the current policy and attempt to slowly decelerate to a stop.
+#                                    Note that for shadowed motors, though an entry is still expected, this value is unused.
+#   10) Safety_Acceleration_Limit....Defines the max safety acceleration limit [deg/s^2]. If the joint acceleration ever violates
+#                                    this limit, we override the current policy and attempt to slowly decelerate to a stop.
+#                                    Note that for shadowed motors, though an entry is still expected, this value is unused.
 #
 # Each motor's configs are grouped under the name of the joint the motor is controlling. The names are defined at
 # http://support.interbotix.com/ under the 'Specifications' section. (Click a robot and scroll down to the 'Default Joint Limits' section.)
 
 port: /dev/ttyDXL
 
-joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper]
-sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0, 0]
+joint_order: [ waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper ]
+sleep_positions: [ 0, -1.80, 1.55, 0, 0.8, 0, 0 ]
 
 joint_state_publisher:
   update_rate: 1000
   publish_states: true
   topic_name: joint_states
 
 groups:
-  arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper]
+  arm: [ waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper ]
 
 grippers:
   gripper:
@@ -50,10 +57,10 @@ grippers:
 
 shadows:
   shoulder:
-    shadow_list: [shoulder_shadow]
+    shadow_list: [ shoulder_shadow ]
     calibrate: true
   elbow:
-    shadow_list: [elbow_shadow]
+    shadow_list: [ elbow_shadow ]
     calibrate: true
 
 sisters:
@@ -70,6 +77,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 180
 
   shoulder:
     ID: 2
@@ -82,6 +90,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 180
 
   shoulder_shadow:
     ID: 3
@@ -94,6 +103,7 @@ motors:
     Secondary_ID: 2
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 180
 
   elbow:
     ID: 4
@@ -106,6 +116,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 180
 
   elbow_shadow:
     ID: 5
@@ -118,6 +129,7 @@ motors:
     Secondary_ID: 4
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 180
 
   forearm_roll:
     ID: 6
@@ -130,6 +142,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 500
 
   wrist_angle:
     ID: 7
@@ -142,6 +155,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 1000
 
   wrist_rotate:
     ID: 8
@@ -154,6 +168,7 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 500
     Position_D_Gain: 0
+    Safety_Velocity_Limit: 1000
 
   gripper:
     ID: 9
@@ -166,3 +181,4 @@ motors:
     Secondary_ID: 255
     Position_P_Gain: 400
     Position_D_Gain: 3
+    Safety_Velocity_Limit: 1000

wx_armor/wx_armor/robot_profile.cc

@@ -12,12 +12,17 @@ bool convert<horizon::wx_armor::RobotProfile>::decode(
   for (const auto &child : node["motors"]) {
     YAML::Node info = child.second;
     uint8_t motor_id = info["ID"].as<uint8_t>();
-    profile.motors.emplace_back(MotorInfo{
-        .id = motor_id,
-        .name = child.first.as<std::string>(),
-        // By default, set the operation mode to position control.
-        .op_mode = OpMode::POSITION,
-    });
+
+    // Safety velocity limit is given in [deg/s]. Convert it to [rad/s]
+    float safety_vel_limit = info["Safety_Velocity_Limit"].as<float>();
+    safety_vel_limit *= M_PI / 180.;
+
+    profile.motors.emplace_back(
+        MotorInfo{.id = motor_id,
+                  .name = child.first.as<std::string>(),
+                  // By default, set the operation mode to position control.
+                  .op_mode = OpMode::POSITION,
+                  .safety_vel_limit = safety_vel_limit});
 
     // Now, populate the register table.
     for (const auto &kv : info) {
@@ -40,6 +45,7 @@ bool convert<horizon::wx_armor::RobotProfile>::decode(
           name);
     }
     profile.joint_ids.emplace_back(motor->id);
+    profile.joint_motors.emplace_back(motor);
     profile.joint_names.emplace_back(motor->name);
   }
 
@@ -95,8 +101,6 @@ std::string_view OpModeName(OpMode mode) {
       return "CURRENT_BASED_POSITION";
     case OpMode::PWM:
       return "PWM";
-    case OpMode::TORQUE:
-      return "TORQUE";
   }
   return "UNKNOWN";
 }