[Ninja][Dynamixel] workaround to use torque control via dynamixel api…

… script.
sugihara-16 · Nov 16, 2024 · 5c2366e · 5c2366e
1 parent 3fc7cc8
commit 5c2366e
Showing 1 changed file with 18 additions and 10 deletions.
diff --git a/robots/ninja/script/ninja/dynamixel_control_api.py b/robots/ninja/script/ninja/dynamixel_control_api.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from spinal.msg import ServoControlCmd
+from spinal.msg import ServoControlCmd, ServoTorqueCmd
 from sensor_msgs.msg import JointState
 
 import rospy
@@ -11,21 +11,25 @@
 class DynamixelControl():
     def __init__(self,robot_name,robot_id,servo_id,real_machine):
 
-        self.DYNAMIXEL_SERVO_POSITION_MIN = 3500
-        self.DYNAMIXEL_SERVO_POSITION_MAX = 11500
-        self.DYNAMIXEL_SERVO_ANGLE_MIN = -2.36
-        self.DYNAMIXEL_SERVO_ANGLE_MAX = 2.36
+        self.DYNAMIXEL_SERVO_POSITION_MIN = 0
+        self.DYNAMIXEL_SERVO_POSITION_MAX = 4096
+        self.DYNAMIXEL_SERVO_ANGLE_MIN = 0
+        self.DYNAMIXEL_SERVO_ANGLE_MAX = 2*math.pi
         self.real_machine = real_machine
         self.robot_name = robot_name
         self.robot_id = robot_id
         self.servo_id = servo_id
         self.servo_name = 'gimbal' + str(robot_id)
 
-        self.dynamixel_pub = rospy.Publisher('/' + self.robot_name +'/servo/target_states', ServoControlCmd, queue_size=1)
+        self.dx_pos_pub = rospy.Publisher('/' + self.robot_name +'/servo/target_states', ServoControlCmd, queue_size=1)
+        self.dx_torque_pub = rospy.Publisher('/' + self.robot_name +'/servo/torque_enable', ServoTorqueCmd, queue_size=1)
         self.gimbal_pub = rospy.Publisher('/' + self.robot_name + '/gimbals_ctrl', JointState, queue_size=1)
 
-        self.servo_cmd_msg = ServoControlCmd()
-        self.servo_cmd_msg.index = [self.servo_id]
+        self.servo_pos_msg = ServoControlCmd()
+        self.servo_pos_msg.index = [self.servo_id]
+
+        self.servo_torque_msg = ServoTorqueCmd()
+        self.servo_torque_msg.index = [self.servo_id]        
 
         self.joint_state_cmd_msg = JointState()
         self.joint_state_cmd_msg.name = [self.servo_name]
@@ -37,12 +41,16 @@ def rad2DynamixelPosConv(self,angle):
         return dynamixel_pos
 
     def sendTargetAngle(self,target_angle):
-        self.servo_cmd_msg.angles = [target_angle]
+        self.servo_pos_msg.angles = [target_angle]
         self.joint_state_cmd_msg.position = [target_angle]
         if self.real_machine:
-            self.dynamixel_pub.publish(self.servo_cmd_msg)
+            self.dx_pos_pub.publish(self.servo_pos_msg)
         else:
             self.gimbal_pub.publish(self.joint_state_cmd_msg)
 
+    def torqueEnable(self,torque_enable):
+        self.servo_torque_msg.torque_enable = [torque_enable]
+        self.dx_torque_pub.publish(self.servo_torque_msg)
+