[spinal][servo][dynamixel] support current based position control mode with direct servo mode

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/drivers/Dynamixel/dynamixel_serial.cpp

@@ -80,6 +80,8 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, osMutexId* mutex)
 	getCurrentLimit();
 	getPositionGains();
 	getProfileVelocity();
+        getModelNumber();
+        getOperatingMode();
 
 
         //initialize encoder: only can support one encoder
@@ -103,6 +105,17 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, osMutexId* mutex)
           }
 	}
 
+        //  set initial goal current if necessary
+        for (unsigned int i = 0; i < servo_num_; i++) {
+          uint8_t operating_mode = servo_[i].operating_mode_;
+          uint16_t current_limit = servo_[i].current_limit_;
+          if (operating_mode == CURRENT_BASE_POSITION_CONTROL_MODE) {
+            servo_[i].goal_current_ = current_limit * 0.8; // workaround: set 80% of the overload threshold
+            cmdWriteGoalCurrent(i);
+          }
+	}
+
+
         /*set angle scale values*/
         for (int i = 0; i < MAX_SERVO_NUM; i++){
           servo_[i].zero_point_offset_ = 2047;
@@ -302,7 +315,7 @@ void DynamixelSerial::update()
     if (set_pos_tick_ == 0) set_pos_tick_ = current_time + SET_POS_OFFSET; // init
     else set_pos_tick_ = current_time;
 
-    instruction_buffer_.push(std::make_pair(INST_SET_GOAL_POS, 0));
+    instruction_buffer_.push(std::make_pair(INST_SET_GOAL_COMMAND, 0));
   }
 
   /* read servo position(angle) */
@@ -388,8 +401,9 @@ void DynamixelSerial::update()
 
       /* set command */
       switch (instruction.first) {
-      case INST_SET_GOAL_POS: /* send angle command to servo */
+      case INST_SET_GOAL_COMMAND: /* send angle command to servo */
         cmdSyncWriteGoalPosition();
+        cmdSyncWriteGoalCurrent();
         break;
       case INST_SET_TORQUE: /* send torque enable flag */
         cmdWriteTorqueEnable(servo_index);
@@ -774,6 +788,16 @@ int8_t DynamixelSerial::readStatusPacket(uint8_t status_packet_instruction)
 			s->moving_ = parameters[0];
 		}
 		return 0;
+	case INST_GET_MODEL_NUMBER:
+		if (s != servo_.end()) {
+			s->model_number_ = ((parameters[1] << 8) & 0xFF00) | (parameters[0] & 0xFF);
+		}
+		return 0;
+	case INST_GET_OPERATING_MODE:
+		if (s != servo_.end()) {
+			s->operating_mode_ = parameters[0];
+		}
+		return 0;
 	case INST_GET_HOMING_OFFSET:
 		if (s != servo_.end()) {
 			s->homing_offset_ = ((parameters[3] << 24) & 0xFF000000) | ((parameters[2] << 16) & 0xFF0000) | ((parameters[1] << 8) & 0xFF00) | (parameters[0] & 0xFF);
@@ -891,6 +915,16 @@ void DynamixelSerial::cmdReadMoving(uint8_t servo_index)
 	cmdRead(servo_[servo_index].id_, CTRL_MOVING, MOVING_BYTE_LEN);
 }
 
+void DynamixelSerial::cmdReadModelNumber(uint8_t servo_index)
+{
+	cmdRead(servo_[servo_index].id_, CTRL_MODEL_NUMBER, MODEL_NUMBER_BYTE_LEN);
+}
+
+void DynamixelSerial::cmdReadOperatingMode(uint8_t servo_index)
+{
+	cmdRead(servo_[servo_index].id_, CTRL_OPERATING_MODE, OPERATING_MODE_BYTE_LEN);
+}
+
 void DynamixelSerial::cmdReadPositionGains(uint8_t servo_index)
 {
 	cmdRead(servo_[servo_index].id_, CTRL_POSITION_D_GAIN, POSITION_GAINS_BYTE_LEN);
@@ -975,6 +1009,15 @@ void DynamixelSerial::cmdWriteTorqueEnable(uint8_t servo_index)
 	cmdWrite(servo_[servo_index].id_, CTRL_TORQUE_ENABLE, &parameter, TORQUE_ENABLE_BYTE_LEN);
 }
 
+void DynamixelSerial::cmdWriteGoalCurrent(uint8_t servo_index)
+{
+  int16_t cur = servo_[servo_index].goal_current_;
+  uint8_t parameters[GOAL_CURRENT_BYTE_LEN];
+  parameters[0] = (uint8_t)(cur & 0xFF);
+  parameters[1] = (uint8_t)((cur >> 8) & 0xFF);
+  cmdWrite(servo_[servo_index].id_, CTRL_GOAL_CURRENT, parameters, GOAL_CURRENT_BYTE_LEN);
+}
+
 void DynamixelSerial::cmdSyncReadCurrentLimit(bool send_all)
 {
 	cmdSyncRead(CTRL_CURRENT_LIMIT, CURRENT_LIMIT_BYTE_LEN, send_all);
@@ -995,6 +1038,16 @@ void DynamixelSerial::cmdSyncReadMoving(bool send_all)
 	cmdSyncRead(CTRL_MOVING, MOVING_BYTE_LEN, send_all);
 }
 
+void DynamixelSerial::cmdSyncReadModelNumber(bool send_all)
+{
+  cmdSyncRead(CTRL_MODEL_NUMBER, MODEL_NUMBER_BYTE_LEN, send_all);
+}
+
+void DynamixelSerial::cmdSyncReadOperatingMode(bool send_all)
+{
+  cmdSyncRead(CTRL_OPERATING_MODE, OPERATING_MODE_BYTE_LEN, send_all);
+}
+
 void DynamixelSerial::cmdSyncReadPositionGains(bool send_all)
 {
 	cmdSyncRead(CTRL_POSITION_D_GAIN, POSITION_GAINS_BYTE_LEN, send_all);
@@ -1035,6 +1088,24 @@ void DynamixelSerial::cmdSyncWriteGoalPosition()
 	cmdSyncWrite(CTRL_GOAL_POSITION, parameters, GOAL_POSITION_BYTE_LEN);
 }
 
+void DynamixelSerial::cmdSyncWriteGoalCurrent()
+{
+  uint8_t parameters[INSTRUCTION_PACKET_SIZE];
+
+  for (unsigned int i = 0; i < servo_num_; i++) {
+    // uint8_t operating_mode = servo_[i].operating_mode;
+    // if (operating_mode != EXTENDED_POSITION_CONTROL_MODE &&
+    //     operating_mode != CURRENT_BASE_POSITION_CONTROL_MODE) {
+    //   continue
+    // }
+    int16_t goal_current = servo_[i].goal_current_;
+    parameters[i * 2 + 0] = (uint8_t)(goal_current & 0xFF);
+    parameters[i * 2 + 1] = (uint8_t)((goal_current >> 8) & 0xFF);
+  }
+
+  cmdSyncWrite(CTRL_GOAL_CURRENT, parameters, GOAL_CURRENT_BYTE_LEN);
+}
+
 void DynamixelSerial::cmdSyncWriteLed()
 {
 	uint8_t parameters[INSTRUCTION_PACKET_SIZE];
@@ -1101,6 +1172,36 @@ void DynamixelSerial::setStatusReturnLevel()
 	cmdWriteStatusReturnLevel(DX_BROADCAST_ID, READ);
 }
 
+void DynamixelSerial::getModelNumber()
+{
+	if (ttl_rs485_mixed_ != 0) {
+		for (unsigned int i = 0; i < servo_num_; ++i) {
+			cmdReadModelNumber(i);
+			readStatusPacket(INST_GET_MODEL_NUMBER);
+		}
+	} else {
+		cmdSyncReadModelNumber();
+		for (unsigned int i = 0; i < servo_num_; i++) {
+			readStatusPacket(INST_GET_MODEL_NUMBER);
+		}
+	}
+}
+
+void DynamixelSerial::getOperatingMode()
+{
+	if (ttl_rs485_mixed_ != 0) {
+		for (unsigned int i = 0; i < servo_num_; ++i) {
+			cmdReadOperatingMode(i);
+			readStatusPacket(INST_GET_OPERATING_MODE);
+		}
+	} else {
+		cmdSyncReadOperatingMode();
+		for (unsigned int i = 0; i < servo_num_; i++) {
+			readStatusPacket(INST_GET_OPERATING_MODE);
+		}
+	}
+}
+
 void DynamixelSerial::getHomingOffset()
 {
 	if (ttl_rs485_mixed_ != 0) {

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/drivers/Dynamixel/dynamixel_serial.h

@@ -46,6 +46,14 @@
 //#define WHEEL_TEST
 //#define SET_HOMING_OFFSET
 
+//################ define - Dynamixel operating mode table ######################
+#define CURRENT_CONTROL_MODE 0
+#define VELOCITY_CONTROL_MODE 1
+#define POSITION_CONTROL_MODE 3
+#define EXTENDED_POSITION_CONTROL_MODE 4
+#define CURRENT_BASE_POSITION_CONTROL_MODE 5
+#define PWM_CONTROL_MODE 16
+
 //#########################################################################
 //################ define - Dynamixel Hex control table ######################
 
@@ -125,9 +133,12 @@
 #define LED_BYTE_LEN					1
 #define STATUS_RETURN_LEVEL_BYTE_LEN	1
 #define GOAL_POSITION_BYTE_LEN			4
+#define GOAL_CURRENT_BYTE_LEN			2
 #define PRESENT_POSITION_BYTE_LEN		4
 #define PRESENT_CURRENT_BYTE_LEN		2
 #define PRESENT_TEMPERATURE_BYTE_LEN	1
+#define OPERATING_MODE_BYTE_LEN 		1
+#define MODEL_NUMBER_BYTE_LEN 			2
 #define MOVING_BYTE_LEN 				1
 #define HARDWARE_ERROR_STATUS_BYTE_LEN	1
 #define POSITION_GAINS_BYTE_LEN			6
@@ -194,11 +205,13 @@
 #define INST_GET_PROFILE_VELOCITY		8
 #define INST_PING						9
 #define INST_SET_CURRENT_LIMIT			10
-#define INST_SET_GOAL_POS 				11
+#define INST_SET_GOAL_COMMAND 				11
 #define INST_SET_HOMING_OFFSET			12
 #define INST_SET_POSITION_GAINS			13
 #define INST_SET_PROFILE_VELOCITY		14
 #define INST_SET_TORQUE					15
+#define INST_GET_MODEL_NUMBER			16
+#define INST_GET_OPERATING_MODE			17
 
 //instruction frequency: 0 means no process
 #define SET_POS_DU 20 //[msec], 20ms => 50Hz
@@ -324,6 +337,8 @@ class DynamixelSerial : public ServoBase
   inline void cmdReadHardwareErrorStatus(uint8_t servo_index);
   inline void cmdReadHomingOffset(uint8_t servo_index);
   inline void cmdReadMoving(uint8_t servo_index);
+  inline void cmdReadModelNumber(uint8_t servo_index);
+  inline void cmdReadOperatingMode(uint8_t servo_index);
   inline void cmdReadPositionGains(uint8_t servo_index);
   inline void cmdReadPresentCurrent(uint8_t servo_index);
   inline void cmdReadPresentPosition(uint8_t servo_index);
@@ -335,16 +350,20 @@ class DynamixelSerial : public ServoBase
   inline void cmdWriteProfileVelocity(uint8_t servo_index);
   inline void cmdWriteStatusReturnLevel(uint8_t id, uint8_t set);
   inline void cmdWriteTorqueEnable(uint8_t servo_index);
+  inline void cmdWriteGoalCurrent(uint8_t servo_index);
   inline void cmdSyncReadCurrentLimit(bool send_all = true);
   inline void cmdSyncReadHardwareErrorStatus(bool send_all = true);
   inline void cmdSyncReadHomingOffset(bool send_all = true);
   inline void cmdSyncReadMoving(bool send_all = true);
+  inline void cmdSyncReadModelNumber(bool send_all = true);
+  inline void cmdSyncReadOperatingMode(bool send_all = true);
   inline void cmdSyncReadPositionGains(bool send_all = true);
   inline void cmdSyncReadPresentCurrent(bool send_all = true);
   inline void cmdSyncReadPresentPosition(bool send_all = true);
   inline void cmdSyncReadPresentTemperature(bool send_all = true);
   inline void cmdSyncReadProfileVelocity(bool send_all = true);
   inline void cmdSyncWriteGoalPosition();
+  inline void cmdSyncWriteGoalCurrent();
   inline void cmdSyncWriteLed();
   inline void cmdSyncWritePositionGains();
   inline void cmdSyncWriteProfileVelocity();
@@ -355,6 +374,8 @@ class DynamixelSerial : public ServoBase
   inline void getCurrentLimit() override;
   inline void getPositionGains() override;
   inline void getProfileVelocity() override;
+  inline void getModelNumber();
+  inline void getOperatingMode();
 
   uint16_t calcCRC16(uint16_t crc_accum, uint8_t *data_blk_ptr, int data_blk_size);
 };

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/drivers/servo_base_driver.h

@@ -22,12 +22,15 @@ class ServoData {
 	uint8_t id_;
   	int32_t present_position_;
 	int32_t goal_position_;
+        int16_t goal_current_;
         int32_t calib_value_;
 	int32_t homing_offset_;
         int32_t internal_offset_;
         uint8_t present_temp_;
 	int16_t present_current_;
 	uint8_t moving_;
+        uint16_t model_number_;
+        uint8_t operating_mode_;
 	uint8_t hardware_error_status_;
 	uint16_t p_gain_, i_gain_, d_gain_;
 	uint16_t profile_velocity_;
@@ -49,9 +52,10 @@ class ServoData {
 	int32_t getPresentPosition() const {return present_position_;}
 	void setGoalPosition(int32_t goal_position) {goal_position_ = resolution_ratio_ * goal_position - internal_offset_;}
         int32_t getGoalPosition() const {return goal_position_;}
+        void setGoalCurrent(int16_t goal_current) {goal_current_ = goal_current;}
+        int16_t getGoalCurrent() const { return goal_current_;}
         float getAngleScale() const {return angle_scale_;}
         uint16_t getZeroPointOffset() const {return zero_point_offset_;}
-
 
 	bool operator==(const ServoData& r) const {return this->id_ == r.id_;}
 };

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp

@@ -23,7 +23,8 @@ void DirectServo::init(UART_HandleTypeDef* huart,  ros::NodeHandle* nh, osMutexI
 #endif
 
   nh_ = nh;
-  nh_->subscribe(servo_ctrl_sub_);
+  nh_->subscribe(servo_position_sub_);
+  nh_->subscribe(servo_current_sub_);
   nh_->subscribe(servo_torque_ctrl_sub_);
   nh_->subscribe(joint_profiles_sub_);
   nh_->advertise(servo_state_pub_);
@@ -166,7 +167,7 @@ void DirectServo::setGoalAngle(const std::map<uint8_t, float>& servo_map, uint8_
     }
 }
 
-void DirectServo::servoControlCallback(const spinal::ServoControlCmd& control_msg)
+void DirectServo::servoPositionCallback(const spinal::ServoControlCmd& control_msg)
 {
   if (control_msg.index_length != control_msg.angles_length) return;
   for (unsigned int i = 0; i < control_msg.index_length; i++) {
@@ -186,6 +187,22 @@ void DirectServo::servoControlCallback(const spinal::ServoControlCmd& control_ms
   }
 }
 
+void DirectServo::servoCurrentCallback(const spinal::ServoControlCmd& control_msg)
+{
+  if (control_msg.index_length != control_msg.angles_length) return;
+  for (unsigned int i = 0; i < control_msg.index_length; i++) {
+    uint8_t index = control_msg.index[i];
+    if(index >= servo_handler_.getServoNum())
+      {
+        nh_->logerror("Invalid Servo ID!");
+        return;
+      }
+    ServoData& s = servo_handler_.getServo()[index];
+    int32_t goal_curr = static_cast<int32_t>(control_msg.angles[i]);
+    s.setGoalCurrent(goal_curr);
+  }
+}
+
 void DirectServo::servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg)
 {
   if (control_msg.index_length != control_msg.torque_enable_length) return;

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h

@@ -36,7 +36,8 @@ class DirectServo
 {
 public:
   DirectServo():
-    servo_ctrl_sub_("servo/target_states", &DirectServo::servoControlCallback,this),
+    servo_position_sub_("servo/target_states", &DirectServo::servoPositionCallback,this),
+    servo_current_sub_("servo/target_current", &DirectServo::servoCurrentCallback, this),
     servo_torque_ctrl_sub_("servo/torque_enable", &DirectServo::servoTorqueControlCallback,this),
     joint_profiles_sub_("joint_profiles", &DirectServo::jointProfilesCallback,this),
     servo_state_pub_("servo/states", &servo_state_msg_),
@@ -65,7 +66,8 @@ class DirectServo
 private:
   /* ROS */
   ros::NodeHandle* nh_;
-  ros::Subscriber<spinal::ServoControlCmd, DirectServo> servo_ctrl_sub_;
+  ros::Subscriber<spinal::ServoControlCmd, DirectServo> servo_position_sub_;
+  ros::Subscriber<spinal::ServoControlCmd, DirectServo> servo_current_sub_;
   ros::Subscriber<spinal::ServoTorqueCmd, DirectServo> servo_torque_ctrl_sub_;
   ros::Subscriber<spinal::JointProfiles, DirectServo> joint_profiles_sub_;
   ros::Publisher servo_state_pub_;
@@ -81,7 +83,8 @@ class DirectServo
   uint32_t servo_last_pub_time_;
   uint32_t servo_torque_last_pub_time_;
 
-  void servoControlCallback(const spinal::ServoControlCmd& control_msg);
+  void servoPositionCallback(const spinal::ServoControlCmd& control_msg);
+  void servoCurrentCallback(const spinal::ServoControlCmd& control_msg);
   void servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg);
   void jointProfilesCallback(const spinal::JointProfiles& joint_prof_msg);